// ============================================================
//  SLAVE FIRMWARE  v2.2
//  Board   : ESP32 38P WROOM CH340
//  MAC     : 20:E7:C8:AD:A0:E8
//
//  FIXES vs v2.1:
//    - OTA hotspot now reliably appears: full WIFI_OFF → WIFI_AP
//      radio reset before softAP(), plus disconnect(true) first.
//    - Forward declaration added for onDataReceived so initEspNow()
//      can safely register the callback regardless of file order.
//    - OTA progress % fixed: tracks running byte count instead of
//      using upload.currentSize (which is chunk size, not total).
//    - exitOTAMode() also does full radio reset before restoring STA.
//
//  MODES:
//    MANUAL  — ESP-NOW drive + AUX LED control
//    AUTO    — Move forward → IR/front-sonar detects → eyes ON →
//              back at full speed → charge LEDs 1→2→3 →
//              pivot right 4 s → repeat
//    SUMO    — 5 s countdown → pivot scan → detect closest target via sonar →
//              full-speed ram in that direction → edge backup+reorient
//    MAZE    — Right-hand wall-follow; goal = open space
//              (no wall front, walls both sides). Route stored in RAM.
//
//  RGB STATUS LED (common-cathode):
//    MANUAL → BLUE   (GPIO 4=L, 16=L, 17=H)
//    AUTO   → OFF    (all LOW)
//    SUMO   → RED    (GPIO 4=H, 16=L, 17=L)
//    MAZE   → GREEN  (GPIO 4=L, 16=H, 17=L)
//
//  MOTOR WIRING (single TB6612FNG, parallel):
//    Channel A → LEFT motors   (PWMA=25, AIN1=26, AIN2=27)
//    Channel B → RIGHT motors  (PWMB=32, BIN1=14, BIN2=13)
//    STBY=19 must be HIGH
//
//  PIN MAP:
//    PWMA=25 AIN1=26 AIN2=27 | PWMB=32 BIN1=14 BIN2=13
//    STBY=19
//    RGB R=4  G=16  B=17
//    IR=34  TRIG=18  ECHO_F=35  ECHO_L=12  ECHO_R=5
//    LINE_F=15  LINE_B=2
//    EYES=33  CHG1=23  CHG2=22  CHG3=21
// ============================================================

#include <WiFi.h>
#include <WebServer.h>
#include <Update.h>
#include <esp_now.h>
#include <Preferences.h>   // ESP32 NVS — built-in, no extra install

// ── Forward declaration (fixes initEspNow referencing onDataReceived before definition) ──
void onDataReceived(const esp_now_recv_info_t* info, const uint8_t* incomingData, int len);

// ── NVS namespace ─────────────────────────────────────────────
Preferences prefs;
static const char* NVS_NS   = "robot";   // namespace
static const char* NVS_MODE = "mode";    // key: stored as uint8_t (0-3)

// ── OTA hotspot credentials ───────────────────────────────────
#define OTA_SSID "ESP32-Robot-OTA"
#define OTA_PASS "12345678"

WebServer otaWebServer(80);
bool      otaServerRunning = false;

// ── Master MAC ────────────────────────────────────────────────
static uint8_t MASTER_MAC[] = { 0x1C, 0xC3, 0xAB, 0xC4, 0xEB, 0x80 };

// ── Motor pins ────────────────────────────────────────────────
static const int LEFT_PWM  = 25;
static const int LEFT_IN1  = 26;
static const int LEFT_IN2  = 27;
static const int RIGHT_PWM = 32;
static const int RIGHT_IN1 = 14;
static const int RIGHT_IN2 = 13;
static const int STBY_PIN  = 19;

// ── Sensor pins ───────────────────────────────────────────────
static const int IR_PIN      = 34;
static const int SONAR_TRIG  = 18;
static const int SONAR_FRONT = 35;
static const int SONAR_LEFT  = 12;   // was 16 — conflicts with WiFi internal on some variants
static const int SONAR_RIGHT = 5;
static const int LINE_FRONT  = 15;
static const int LINE_BACK   = 2;

// ── LED pins ──────────────────────────────────────────────────
static const int EYE_PIN     = 33;
static const int CHARGE1_PIN = 21;
static const int CHARGE2_PIN = 22;
static const int CHARGE3_PIN = 23;

// ── PWM settings ──────────────────────────────────────────────
static const uint32_t PWM_FREQ     = 2000;
static const uint8_t  PWM_RES_BITS = 8;
static const int      PWM_MAX      = 255;
static const int      SPEED_CAP    = 255;

// ── Speed constants ───────────────────────────────────────────
static const int SEARCH_SPEED   = 170;
static const int FULL_SPEED     = 255;
static const int TURN_SPEED     = 200;

// ── Sonar settings ────────────────────────────────────────────
static const int           SONAR_DETECT_CM  = 15;
static const int           MAZE_WALL_CM     = 10;
static const int           MAZE_STOP_CM     = 10;
static const unsigned long SONAR_TIMEOUT_US = 23200UL;
static const unsigned long SONAR_SETTLE_MS  = 5;

// ── Telemetry ─────────────────────────────────────────────────
static const unsigned long TEL_INTERVAL_MS = 800;
unsigned long lastTelMs = 0;

// ── Mode / state enums ────────────────────────────────────────
enum RobotMode { MODE_MANUAL = 0, MODE_AUTO = 1, MODE_SUMO = 2, MODE_MAZE = 3, MODE_OTA = 4 };

enum AutoState {
  AUTO_FORWARD,
  AUTO_BACKUP,
  AUTO_CHARGE_LEDS,
  AUTO_PIVOT
};

enum SumoState {
  SUMO_COUNTDOWN,
  SUMO_SCAN,
  SUMO_ATTACK,
  SUMO_BACKUP,
  SUMO_REORIENT
};

enum MazeState {
  MAZE_RUN,
  MAZE_TURN_RIGHT,
  MAZE_TURN_LEFT,
  MAZE_REVERSE
};

RobotMode currentMode = MODE_MANUAL;
AutoState autoState   = AUTO_FORWARD;
SumoState sumoState   = SUMO_COUNTDOWN;
MazeState mazeState   = MAZE_RUN;

// ── Telemetry / status ────────────────────────────────────────
String currentCommand = "STOP";
int    requestedSpeed = 180;
bool   eyesOnState    = false;
bool   charge1On      = false;
bool   charge2On      = false;
bool   charge3On      = false;
int    sonar1Cm       = 0;
int    sonar2Cm       = 0;
int    sonar3Cm       = 0;

// ── State timing ──────────────────────────────────────────────
unsigned long stateStartedMs = 0;

// ── Maze ──────────────────────────────────────────────────────
bool mazeIsBacktracking = false;

// ── ESP-NOW peer registered ───────────────────────────────────
static bool masterRegistered = false;

// ── Sumo countdown duration ───────────────────────────────────
static const unsigned long SUMO_COUNTDOWN_MS = 5000UL;

// ============================================================
// NVS — PERSISTENT MODE
// ============================================================
void saveModeToNvs(RobotMode mode) {
  if (mode == MODE_AUTO) return;
  if (mode == MODE_OTA)  return;
  prefs.begin(NVS_NS, false);
  prefs.putUChar(NVS_MODE, (uint8_t)mode);
  prefs.end();
}

RobotMode loadModeFromNvs() {
  prefs.begin(NVS_NS, true);
  uint8_t raw = prefs.getUChar(NVS_MODE, (uint8_t)MODE_MANUAL);
  prefs.end();
  if (raw == (uint8_t)MODE_AUTO || raw == (uint8_t)MODE_OTA || raw > (uint8_t)MODE_OTA) {
    return MODE_MANUAL;
  }
  return (RobotMode)raw;
}

// ============================================================
// HELPERS
// ============================================================
int clampPwm(int v) {
  if (v >  PWM_MAX) return  PWM_MAX;
  if (v < -PWM_MAX) return -PWM_MAX;
  return v;
}

bool irDetected()        { return digitalRead(IR_PIN)     == LOW; }
bool lineFrontDetected() { return digitalRead(LINE_FRONT) == LOW; }
bool lineBackDetected()  { return digitalRead(LINE_BACK)  == LOW; }

String modeString() {
  switch (currentMode) {
    case MODE_AUTO:  return "AUTO";
    case MODE_SUMO:  return "SUMO";
    case MODE_MAZE:  return "MAZE";
    case MODE_OTA:   return "OTA";
    default:         return "MANUAL";
  }
}

// Send STATUS:msg:pct back to Java via ESP-NOW → Master → Serial.
void sendStatusToMaster(const char* msg, uint8_t progress = 0) {
  char buf[64];
  snprintf(buf, sizeof(buf), "STATUS:%s:%u", msg, progress);
  int len = strlen(buf);
  uint8_t packet[250];
  memset(packet, 0, sizeof(packet));
  memcpy(packet, buf, len);
  esp_now_send(MASTER_MAC, packet, len + 1);
}

// ============================================================
// SONAR
// ============================================================
int readSonar(int echoPin) {
  digitalWrite(SONAR_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(SONAR_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(SONAR_TRIG, LOW);
  long dur = pulseIn(echoPin, HIGH, SONAR_TIMEOUT_US);
  if (dur == 0) return 999;
  return (int)(dur / 58);
}

void updateSonars() {
  sonar1Cm = readSonar(SONAR_FRONT);
  delay(SONAR_SETTLE_MS);
  sonar2Cm = readSonar(SONAR_LEFT);
  delay(SONAR_SETTLE_MS);
  sonar3Cm = readSonar(SONAR_RIGHT);
  delay(SONAR_SETTLE_MS);
}

// RGB hardware removed — stub kept so MODE: handler compiles without changes
void updateRgbStatus() {}

// ============================================================
// LED HELPERS
// ============================================================
void setEyes(bool on) {
  eyesOnState = on;
  digitalWrite(EYE_PIN, on ? HIGH : LOW);
}

void setCharge1(bool on) { charge1On = on; digitalWrite(CHARGE1_PIN, on ? HIGH : LOW); }
void setCharge2(bool on) { charge2On = on; digitalWrite(CHARGE2_PIN, on ? HIGH : LOW); }
void setCharge3(bool on) { charge3On = on; digitalWrite(CHARGE3_PIN, on ? HIGH : LOW); }

void clearChargeLeds() {
  charge1On = charge2On = charge3On = false;
  digitalWrite(CHARGE1_PIN, LOW);
  digitalWrite(CHARGE2_PIN, LOW);
  digitalWrite(CHARGE3_PIN, LOW);
}

void clearLeds() {
  setEyes(false);
  clearChargeLeds();
}

// ============================================================
// MOTOR WRITE — TB6612FNG (direct, no ramp)
// ============================================================
void writeMotor(int pwmPin, int in1Pin, int in2Pin, int signedSpeed) {
  signedSpeed = clampPwm(signedSpeed);
  if (signedSpeed > 0) {
    digitalWrite(in1Pin, HIGH);
    digitalWrite(in2Pin, LOW);
    ledcWrite(pwmPin, signedSpeed);
  } else if (signedSpeed < 0) {
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, HIGH);
    ledcWrite(pwmPin, -signedSpeed);
  } else {
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, LOW);
    ledcWrite(pwmPin, 0);
  }
}

void setDrive(int left, int right, const String& label) {
  left  = constrain(clampPwm(left),  -SPEED_CAP, SPEED_CAP);
  right = constrain(clampPwm(right), -SPEED_CAP, SPEED_CAP);
  writeMotor(LEFT_PWM,  LEFT_IN1,  LEFT_IN2,  left);
  writeMotor(RIGHT_PWM, RIGHT_IN1, RIGHT_IN2, right);
  currentCommand = label;
}

void stopRobot    (const String& lbl = "STOP")             { setDrive( 0,  0, lbl); }
void driveForward (int s, const String& lbl = "FORWARD")   { setDrive( s,  s, lbl); }
void driveBackward(int s, const String& lbl = "BACKWARD")  { setDrive(-s, -s, lbl); }
void pivotLeft    (int s, const String& lbl = "LEFT")      { setDrive(-s,  s, lbl); }
void pivotRight   (int s, const String& lbl = "RIGHT")     { setDrive( s, -s, lbl); }

// ============================================================
// AUTO MODE
// ============================================================
void enterAuto(AutoState next) {
  autoState      = next;
  stateStartedMs = millis();
}

void resetAuto() {
  clearLeds();
  stopRobot("AUTO_IDLE");
  enterAuto(AUTO_FORWARD);
}

void updateAuto() {
  unsigned long elapsed = millis() - stateStartedMs;

  switch (autoState) {

    case AUTO_FORWARD:
      clearLeds();
      if (irDetected() || (sonar1Cm > 0 && sonar1Cm <= SONAR_DETECT_CM)) {
        setEyes(true);
        stopRobot("OBSTACLE");
        enterAuto(AUTO_BACKUP);
      } else {
        driveForward(requestedSpeed, "AUTO_FWD");
      }
      break;

    case AUTO_BACKUP:
      driveBackward(FULL_SPEED, "AUTO_BACKUP");
      if (elapsed >= 1000) {
        stopRobot("BACKUP_DONE");
        clearChargeLeds();
        enterAuto(AUTO_CHARGE_LEDS);
      }
      break;

    case AUTO_CHARGE_LEDS:
      if (elapsed >= 600) {
        setCharge1(true); setCharge2(true); setCharge3(true);
      } else if (elapsed >= 300) {
        setCharge1(true); setCharge2(true); setCharge3(false);
      } else {
        setCharge1(true); setCharge2(false); setCharge3(false);
      }
      if (elapsed >= 900) {
        clearChargeLeds();
        setEyes(false);
        enterAuto(AUTO_PIVOT);
      }
      break;

    case AUTO_PIVOT:
      pivotRight(TURN_SPEED, "AUTO_PIVOT");
      if (elapsed >= 4000) {
        stopRobot("PIVOT_DONE");
        enterAuto(AUTO_FORWARD);
      }
      break;
  }
}

// ============================================================
// SUMO MODE
// ============================================================
void enterSumo(SumoState next) {
  sumoState      = next;
  stateStartedMs = millis();
}

void resetSumo() {
  clearLeds();
  stopRobot("SUMO_COUNTDOWN");
  enterSumo(SUMO_COUNTDOWN);
}

bool sumoCheckAndAttack() {
  int bestDist = SONAR_DETECT_CM + 1;
  int bestDir  = 0;

  if (sonar1Cm > 0 && sonar1Cm < bestDist) { bestDist = sonar1Cm; bestDir = 1; }
  if (sonar2Cm > 0 && sonar2Cm < bestDist) { bestDist = sonar2Cm; bestDir = 2; }
  if (sonar3Cm > 0 && sonar3Cm < bestDist) { bestDist = sonar3Cm; bestDir = 3; }

  if (bestDir == 0) return false;

  setEyes(true);
  switch (bestDir) {
    case 1: driveForward(FULL_SPEED, "SUMO_RAM_F");                    break;
    case 2: setDrive(-FULL_SPEED,  FULL_SPEED,  "SUMO_RAM_L");        break;
    case 3: setDrive( FULL_SPEED, -FULL_SPEED,  "SUMO_RAM_R");        break;
  }
  return true;
}

void sumoCountdownLeds(unsigned long elapsed) {
  if (elapsed < 1000) {
    setCharge3(true);  setCharge2(false); setCharge1(false);
  } else if (elapsed < 2000) {
    setCharge3(true);  setCharge2(true);  setCharge1(false);
  } else if (elapsed < 3000) {
    setCharge3(true);  setCharge2(true);  setCharge1(true);
  } else if (elapsed < 4000) {
    bool on = ((elapsed / 500) % 2) == 0;
    setCharge1(on); setCharge2(on); setCharge3(on);
  } else {
    bool on = ((elapsed / 250) % 2) == 0;
    setCharge1(on); setCharge2(on); setCharge3(on);
  }
}

void updateSumo() {
  unsigned long elapsed = millis() - stateStartedMs;

  if (sumoState == SUMO_COUNTDOWN) {
    stopRobot("SUMO_ARMED");
    sumoCountdownLeds(elapsed);
    if (elapsed >= SUMO_COUNTDOWN_MS) {
      clearChargeLeds();
      enterSumo(SUMO_SCAN);
    }
    return;
  }

  if ((sumoState != SUMO_BACKUP) &&
      (lineFrontDetected() || lineBackDetected())) {
    setEyes(false);
    clearChargeLeds();
    enterSumo(SUMO_BACKUP);
    return;
  }

  switch (sumoState) {

    case SUMO_SCAN:
      clearLeds();
      if (sumoCheckAndAttack()) {
        enterSumo(SUMO_ATTACK);
      } else {
        pivotRight(TURN_SPEED, "SUMO_CIRCLE");
      }
      break;

    case SUMO_ATTACK:
      if (!sumoCheckAndAttack()) {
        setEyes(false);
        enterSumo(SUMO_SCAN);
      }
      break;

    case SUMO_BACKUP:
      setEyes(false);
      if (lineBackDetected()) {
        driveForward(FULL_SPEED, "SUMO_EDGE_FWD");
      } else {
        driveBackward(FULL_SPEED, "SUMO_EDGE_BACK");
      }
      if (elapsed >= 600) {
        stopRobot("EDGE_CLEAR");
        enterSumo(SUMO_REORIENT);
      }
      break;

    case SUMO_REORIENT:
      pivotRight(TURN_SPEED, "SUMO_REORIENT");
      if (elapsed >= 500) {
        enterSumo(SUMO_SCAN);
      }
      break;

    case SUMO_COUNTDOWN:
      break;
  }
}

// ============================================================
// MAZE SOLVER — Right-hand wall follower
// ============================================================
static const int           MAZE_DRIVE_SPEED = 120;
static const unsigned long MAZE_TURN_MS     = 550;
static const unsigned long MAZE_REV_MS      = 900;

static bool mazeRevTurnRight = false;

void enterMaze(MazeState next) {
  mazeState      = next;
  stateStartedMs = millis();
}

void resetMaze() {
  clearLeds();
  mazeIsBacktracking = false;
  mazeRevTurnRight   = false;
  enterMaze(MAZE_RUN);
  driveForward(MAZE_DRIVE_SPEED, "MAZE_START");
}

bool mazeWallFront() { return sonar1Cm > 0 && sonar1Cm < 999 && sonar1Cm <= MAZE_WALL_CM; }
bool mazeWallLeft()  { return sonar2Cm > 0 && sonar2Cm < 999 && sonar2Cm <= MAZE_WALL_CM; }
bool mazeWallRight() { return sonar3Cm > 0 && sonar3Cm < 999 && sonar3Cm <= MAZE_WALL_CM; }

void updateMaze() {
  unsigned long elapsed = millis() - stateStartedMs;

  switch (mazeState) {

    case MAZE_RUN: {
      bool wF = mazeWallFront();
      bool wL = mazeWallLeft();
      bool wR = mazeWallRight();

      if (!wF && !wL && !wR) {
        driveForward(MAZE_DRIVE_SPEED, "MAZE_FWD");
        break;
      }
      if (wF && wL && wR) {
        driveBackward(MAZE_DRIVE_SPEED, "MAZE_DEADEND");
        enterMaze(MAZE_REVERSE);
        return;
      }
      if (wF && wR && !wL) {
        pivotLeft(MAZE_DRIVE_SPEED, "MAZE_TL");
        enterMaze(MAZE_TURN_LEFT);
        return;
      }
      if (wF && wL && !wR) {
        pivotRight(MAZE_DRIVE_SPEED, "MAZE_TR");
        enterMaze(MAZE_TURN_RIGHT);
        return;
      }
      if (wF && !wL && !wR) {
        pivotLeft(MAZE_DRIVE_SPEED, "MAZE_TL");
        enterMaze(MAZE_TURN_LEFT);
        return;
      }
      driveForward(MAZE_DRIVE_SPEED, "MAZE_FWD");
      break;
    }

    case MAZE_TURN_RIGHT:
      if (elapsed >= MAZE_TURN_MS) {
        driveForward(MAZE_DRIVE_SPEED, "MAZE_FWD");
        enterMaze(MAZE_RUN);
      }
      break;

    case MAZE_TURN_LEFT:
      if (elapsed >= MAZE_TURN_MS) {
        driveForward(MAZE_DRIVE_SPEED, "MAZE_FWD");
        enterMaze(MAZE_RUN);
      }
      break;

    case MAZE_REVERSE:
      if (elapsed >= MAZE_REV_MS) {
        driveForward(MAZE_DRIVE_SPEED, "MAZE_FWD");
        enterMaze(MAZE_RUN);
      }
      break;
  }
}

// ============================================================
// ESP-NOW INIT HELPER (boot + post-OTA restore)
// ============================================================
void initEspNow() {
  if (esp_now_init() != ESP_OK) return;
  esp_now_register_recv_cb(onDataReceived);   // safe — forward-declared above
  esp_now_peer_info_t peerInfo = {};
  memcpy(peerInfo.peer_addr, MASTER_MAC, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  if (esp_now_add_peer(&peerInfo) == ESP_OK) masterRegistered = true;
}

// ============================================================
// OTA MODE
// ============================================================
void enterOTAMode() {
  currentMode = MODE_OTA;
  clearLeds();
  stopRobot("OTA");
  updateRgbStatus();

  // 1. Tear down ESP-NOW
  esp_now_deinit();
  masterRegistered = false;

  // 2. Full radio reset — critical: STA → OFF → AP.
  //    Skipping WIFI_OFF causes softAP() to silently fail on ESP32
  //    because the radio is still partially in STA state.
  WiFi.disconnect(true);   // drop STA and clear credentials
  delay(100);
  WiFi.mode(WIFI_OFF);     // full radio off
  delay(200);
  WiFi.mode(WIFI_AP);      // switch to AP
  delay(300);              // give radio time to settle

  // 3. Start AP — retry up to 5 times
  bool apStarted = false;
  for (int attempt = 0; attempt < 5; attempt++) {
    if (WiFi.softAP(OTA_SSID, OTA_PASS)) {
      apStarted = true;
      break;
    }
    Serial.printf("[OTA] softAP attempt %d failed, retrying...\n", attempt + 1);
    delay(300);
  }

  if (!apStarted) {
    Serial.println("[OTA] ERROR: Could not start AP hotspot!");
    currentMode = MODE_MANUAL;
    clearLeds();
    stopRobot("MANUAL_READY");
    updateRgbStatus();
    WiFi.mode(WIFI_OFF);
    delay(150);
    WiFi.mode(WIFI_STA);
    WiFi.disconnect();
    initEspNow();
    return;
  }

  delay(100);
  Serial.printf("[OTA] Hotspot: %s  IP: %s\n",
                OTA_SSID, WiFi.softAPIP().toString().c_str());

  otaWebServer.on("/", HTTP_GET, []() {
    otaWebServer.sendHeader("Location", "/update");
    otaWebServer.send(302, "text/plain", "");
  });

  otaWebServer.on("/update", HTTP_GET, []() {
    otaWebServer.send(200, "text/html",
      "<html><head><title>Robot OTA</title></head>"
      "<body style='background:#0a0a0f;color:#ff55aa;font-family:monospace;padding:24px'>"
      "<h2 style='color:#a050ff'>ESP32 Robot OTA Update</h2>"
      "<p>Select a <b>.bin</b> file compiled from Arduino IDE, then click Flash.</p>"
      "<form method='POST' action='/update' enctype='multipart/form-data'>"
      "<input type='file' name='update' accept='.bin' style='color:#f0ebff'><br><br>"
      "<input type='submit' value='Flash Firmware' "
      "style='padding:10px 22px;background:#a050ff;color:#fff;border:none;cursor:pointer;border-radius:4px'>"
      "</form></body></html>"
    );
  });

  // OTA progress — track running byte total in a static local
  otaWebServer.on("/update", HTTP_POST,
    []() {
      bool ok = !Update.hasError();
      otaWebServer.send(200, "text/plain", ok ? "OK - Rebooting" : "FAIL");
      sendStatusToMaster(ok ? "OTA Done! Rebooting" : "OTA FAILED", 100);
      delay(500);
      ESP.restart();
    },
    []() {
      HTTPUpload& upload = otaWebServer.upload();
      static size_t bytesWritten = 0;   // running total — fixes progress %

      if (upload.status == UPLOAD_FILE_START) {
        bytesWritten = 0;
        Serial.printf("[OTA] Start: %s\n", upload.filename.c_str());
        sendStatusToMaster("OTA Started", 0);
        Update.begin(UPDATE_SIZE_UNKNOWN);

      } else if (upload.status == UPLOAD_FILE_WRITE) {
        Update.write(upload.buf, upload.currentSize);
        bytesWritten += upload.currentSize;
        if (upload.totalSize > 0) {
          uint8_t pct = (uint8_t)((bytesWritten * 100UL) / upload.totalSize);
          sendStatusToMaster("Updating...", pct);
        }

      } else if (upload.status == UPLOAD_FILE_END) {
        Update.end(true);
        Serial.printf("[OTA] End: %u bytes\n", upload.totalSize);
      }
    }
  );

  otaWebServer.begin();
  otaServerRunning = true;
  Serial.println("[OTA] Server ready -> http://192.168.4.1/update");
  sendStatusToMaster("OTA Mode Active", 0);
}

void exitOTAMode() {
  if (otaServerRunning) {
    otaWebServer.stop();
    WiFi.softAPdisconnect(true);
    otaServerRunning = false;
  }
  // Full radio reset before restoring STA + ESP-NOW
  WiFi.mode(WIFI_OFF);
  delay(150);
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  delay(150);
  initEspNow();
}

// ============================================================
// MANUAL COMMAND HANDLER
// ============================================================
void handleManualCommand(const String& cmd) {
  if      (cmd == "F") driveForward (requestedSpeed, "FORWARD");
  else if (cmd == "B") driveBackward(requestedSpeed, "BACKWARD");
  else if (cmd == "L") pivotLeft    (requestedSpeed, "LEFT");
  else if (cmd == "R") pivotRight   (requestedSpeed, "RIGHT");
  else                 stopRobot("STOP");
}

// ============================================================
// TELEMETRY
// ============================================================
String buildTelemetry() {
  String body;
  body.reserve(440);
  body += "mode="    + modeString()                                          + "\n";
  body += "command=" + currentCommand                                        + "\n";
  body += "speed="   + String(requestedSpeed)                                + "\n";
  body += "ir="      + String(irDetected()        ? "DETECTED" : "CLEAR")   + "\n";
  body += "eyes="    + String(eyesOnState         ? "ON"       : "OFF")     + "\n";
  body += "charge1=" + String(charge1On           ? "ON"       : "OFF")     + "\n";
  body += "charge2=" + String(charge2On           ? "ON"       : "OFF")     + "\n";
  body += "charge3=" + String(charge3On           ? "ON"       : "OFF")     + "\n";
  body += "sonar1="  + String(sonar1Cm)                                      + "\n";
  body += "sonar2="  + String(sonar2Cm)                                      + "\n";
  body += "sonar3="  + String(sonar3Cm)                                      + "\n";
  body += "line1="   + String(lineFrontDetected() ? "DETECTED" : "CLEAR")   + "\n";
  body += "line2="   + String(lineBackDetected()  ? "DETECTED" : "CLEAR")   + "\n";
  if (currentMode == MODE_MAZE) {
    body += "maze_bt=" + String(mazeIsBacktracking ? "YES" : "NO") + "\n";
  }
  return body;
}

void sendTelemetryToMaster() {
  if (!masterRegistered) return;
  String tel = buildTelemetry();
  int    len = min((int)tel.length(), 249);
  uint8_t packet[250];
  memset(packet, 0, sizeof(packet));
  memcpy(packet, tel.c_str(), len);
  esp_now_send(MASTER_MAC, packet, len + 1);
}

// ============================================================
// ESP-NOW — RECEIVE COMMAND FROM MASTER
// ============================================================
void handleCommand(const String& raw) {
  String msg = raw;
  msg.trim();

  if (msg.equalsIgnoreCase("PING")) {
    uint8_t pkt[8] = {};
    memcpy(pkt, "PONG", 4);
    esp_now_send(MASTER_MAC, pkt, 5);
    return;
  }

  if (msg.equalsIgnoreCase("TEL?")) {
    sendTelemetryToMaster();
    return;
  }

  if (msg.startsWith("SPEED:")) {
    requestedSpeed = constrain(msg.substring(6).toInt(), 0, SPEED_CAP);
    return;
  }

  if (msg.startsWith("MODE:")) {
    String m = msg.substring(5);
    m.trim(); m.toUpperCase();
    if      (m == "MANUAL") {
      if (currentMode == MODE_OTA) exitOTAMode();
      currentMode = MODE_MANUAL; clearLeds(); stopRobot("MANUAL_READY");
    }
    else if (m == "AUTO")   { currentMode = MODE_AUTO;   resetAuto(); }
    else if (m == "SUMO")   { currentMode = MODE_SUMO;   resetSumo(); }
    else if (m == "MAZE")   { currentMode = MODE_MAZE;   resetMaze(); }
    else if (m == "OTA")    { enterOTAMode(); return; }
    else                    { return; }
    updateRgbStatus();
    saveModeToNvs(currentMode);
    return;
  }

  if (msg.startsWith("CMD:")) {
    if (currentMode != MODE_MANUAL) return;
    String c = msg.substring(4);
    c.trim(); c.toUpperCase();
    handleManualCommand(c);
    return;
  }

  if (currentMode != MODE_MANUAL) return;

  if (msg.equalsIgnoreCase("AUX:EYES:ON"))   { setEyes(true);  return; }
  if (msg.equalsIgnoreCase("AUX:EYES:OFF"))  { setEyes(false); return; }

  if (msg.equalsIgnoreCase("AUX:CHARGE:ON"))  { setCharge1(true);  setCharge2(true);  setCharge3(true);  return; }
  if (msg.equalsIgnoreCase("AUX:CHARGE:OFF")) { setCharge1(false); setCharge2(false); setCharge3(false); return; }

  if (msg.equalsIgnoreCase("AUX:CHARGE1:ON"))  { setCharge1(true);  return; }
  if (msg.equalsIgnoreCase("AUX:CHARGE1:OFF")) { setCharge1(false); return; }
  if (msg.equalsIgnoreCase("AUX:CHARGE2:ON"))  { setCharge2(true);  return; }
  if (msg.equalsIgnoreCase("AUX:CHARGE2:OFF")) { setCharge2(false); return; }
  if (msg.equalsIgnoreCase("AUX:CHARGE3:ON"))  { setCharge3(true);  return; }
  if (msg.equalsIgnoreCase("AUX:CHARGE3:OFF")) { setCharge3(false); return; }
}

void onDataReceived(const esp_now_recv_info_t* info,
                    const uint8_t* incomingData, int len) {
  if (len <= 0 || len >= 250) return;
  char buf[250];
  memcpy(buf, incomingData, len);
  buf[len] = '\0';
  handleCommand(String(buf));
}

// ============================================================
// SETUP
// ============================================================
void setup() {
  Serial.begin(115200);

  // Motor driver
  pinMode(LEFT_IN1,  OUTPUT); pinMode(LEFT_IN2,  OUTPUT);
  pinMode(RIGHT_IN1, OUTPUT); pinMode(RIGHT_IN2, OUTPUT);
  pinMode(STBY_PIN,  OUTPUT); digitalWrite(STBY_PIN, HIGH);

  // Sensors
  pinMode(IR_PIN,      INPUT);
  pinMode(SONAR_TRIG,  OUTPUT); digitalWrite(SONAR_TRIG, LOW);
  pinMode(SONAR_FRONT, INPUT);
  pinMode(SONAR_LEFT,  INPUT);
  pinMode(SONAR_RIGHT, INPUT);
  pinMode(LINE_FRONT,  INPUT);
  pinMode(LINE_BACK,   INPUT);

  // LEDs
  pinMode(EYE_PIN,     OUTPUT);
  pinMode(CHARGE1_PIN, OUTPUT);
  pinMode(CHARGE2_PIN, OUTPUT);
  pinMode(CHARGE3_PIN, OUTPUT);
  clearLeds();

  // PWM channels
  ledcAttach(LEFT_PWM,  PWM_FREQ, PWM_RES_BITS);
  ledcAttach(RIGHT_PWM, PWM_FREQ, PWM_RES_BITS);

  stopRobot("BOOT");

  // Restore last saved mode
  currentMode = loadModeFromNvs();
  switch (currentMode) {
    case MODE_MANUAL: clearLeds(); stopRobot("MANUAL_READY"); break;
    case MODE_SUMO:   resetSumo(); break;
    case MODE_MAZE:   resetMaze(); break;
    default:          currentMode = MODE_MANUAL; break;
  }
  updateRgbStatus();

  Serial.print("[Slave] Boot OK. Restored mode: ");
  Serial.println(modeString());

  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  initEspNow();

  Serial.println("[Slave] MAC: " + WiFi.macAddress());
}

// ============================================================
// LOOP
// ============================================================
void loop() {
  if (currentMode == MODE_OTA) {
    if (otaServerRunning) otaWebServer.handleClient();
    return;
  }

  updateSonars();

  switch (currentMode) {
    case MODE_AUTO:   updateAuto(); break;
    case MODE_SUMO:   updateSumo(); break;
    case MODE_MAZE:   updateMaze(); break;
    case MODE_MANUAL: break;
  }

  unsigned long now = millis();
  if (now - lastTelMs >= TEL_INTERVAL_MS) {
    lastTelMs = now;
    sendTelemetryToMaster();
  }
}
