// ============================================================
//  MASTER FIRMWARE  v2.1
//  Board   : ESP32 32P WROOM
//  MAC     : 1C:C3:AB:C4:EB:80
//  Slave   : 20:E7:C8:AD:A0:E8
//
//  Transport : Java GUI → USB Serial (115200) → ESP-NOW → Slave
//              Slave telemetry → ESP-NOW → Master → USB Serial → Java GUI
//
//  LED D2 (GPIO 2, built-in):
//    Solid ON  — last ESP-NOW send was ACK'd (slave reachable)
//    Blinking  — no successful ACK / slave unreachable
//
//  Protocol (Java → Master, newline-terminated):
//    CMD:F / CMD:B / CMD:L / CMD:R / CMD:S
//    SPEED:0-255
//    MODE:MANUAL / MODE:AUTO / MODE:SUMO / MODE:MAZE / MODE:OTA
//    AUX:EYES:ON  / AUX:EYES:OFF
//    AUX:CHARGE:ON / AUX:CHARGE:OFF
//    AUX:CHARGE1:ON|OFF  AUX:CHARGE2:ON|OFF  AUX:CHARGE3:ON|OFF
//    PING  TEL?
//
//  Slave → Java forwarding rules:
//    STATUS:msg:pct  → forwarded raw (consumed by OTA panel)
//    PONG            → forwarded as-is
//    everything else → prefixed with "TEL:"
// ============================================================

#include <esp_now.h>
#include <WiFi.h>

// ── Pin ───────────────────────────────────────────────────────
#define LED_PIN        2
#define BLINK_INTERVAL 500

// ── Slave peer MAC ────────────────────────────────────────────
static uint8_t SLAVE_MAC[] = { 0x20, 0xE7, 0xC8, 0xAD, 0xA0, 0xE8 };

// ── State ─────────────────────────────────────────────────────
volatile bool slaveConnected = false;
unsigned long lastBlink      = 0;
bool          blinkState     = false;

// ── ESP-NOW send callback ─────────────────────────────────────
void onSent(const wifi_tx_info_t* info, esp_now_send_status_t status) {
  slaveConnected = (status == ESP_NOW_SEND_SUCCESS);
}

// ── ESP-NOW receive callback — relay slave telemetry to Java ──
void onReceived(const esp_now_recv_info_t* info, const uint8_t* data, int len) {
  if (len <= 0 || len >= 250) return;
  char buf[251];
  memcpy(buf, data, len);
  buf[len] = '\0';

  String msg = String(buf);
  msg.trim();

  // PONG reply: forward as-is so Java ping handler recognises it
  if (msg.equalsIgnoreCase("PONG")) {
    Serial.println("PONG");
    return;
  }

  // Telemetry block: forward each non-empty line.
  // STATUS: lines are forwarded raw — Java OTA panel parses them directly.
  // Everything else is prefixed with TEL: so Java can distinguish it.
  for (int start = 0; start < (int)msg.length(); ) {
    int nl = msg.indexOf('\n', start);
    String line = (nl == -1) ? msg.substring(start) : msg.substring(start, nl);
    line.trim();
    if (line.length() > 0) {
      if (line.startsWith("STATUS:")) {
        Serial.println(line);        // raw — Java OTA panel consumes this
      } else {
        Serial.println("TEL:" + line);
      }
    }
    if (nl == -1) break;
    start = nl + 1;
  }
}

// ── LED heartbeat ─────────────────────────────────────────────
void updateLED() {
  if (slaveConnected) {
    digitalWrite(LED_PIN, HIGH);
  } else {
    unsigned long now = millis();
    if (now - lastBlink >= BLINK_INTERVAL) {
      lastBlink  = now;
      blinkState = !blinkState;
      digitalWrite(LED_PIN, blinkState);
    }
  }
}

// ── Forward string to slave via ESP-NOW ───────────────────────
void sendToSlave(const String& msg) {
  int len = min((int)msg.length(), 249);
  uint8_t packet[250];
  memset(packet, 0, sizeof(packet));
  memcpy(packet, msg.c_str(), len);
  esp_now_send(SLAVE_MAC, packet, len + 1);
}

// ── Setup ─────────────────────────────────────────────────────
void setup() {
  Serial.begin(115200);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);

  WiFi.mode(WIFI_STA);

  if (esp_now_init() != ESP_OK) {
    Serial.println("ERR:ESPNOW_INIT_FAILED");
    return;
  }

  esp_now_register_send_cb(onSent);
  esp_now_register_recv_cb(onReceived);

  esp_now_peer_info_t peerInfo = {};
  memcpy(peerInfo.peer_addr, SLAVE_MAC, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("ERR:PEER_ADD_FAILED");
    return;
  }

  Serial.println("MASTER_READY");
}

// ── Loop ──────────────────────────────────────────────────────
void loop() {
  updateLED();

  if (Serial.available()) {
    String line = Serial.readStringUntil('\n');
    line.trim();
    if (line.length() == 0) return;
    sendToSlave(line);
  }
}
