// ============================================================
//  SLAVE FIRMWARE  v2.3
//  Board   : ESP32 38P WROOM CH340
//  MAC     : 20:E7:C8:AD:A0:E8
//
//  CHANGES vs v2.2:
//    - Added MODE_SUMO_NL ("Sumo Bot Mode (no line sensor)") for
//      arenas without a ring boundary. Same 5 s arm countdown.
//      Search pattern: curve right 3 s → curve left 3 s → U-turn,
//      repeat. Search speed = ramming speed / 2 (exactly half).
//      Detection (any phase): any sonar ≤ 20 cm → attack —
//      front=straight ram, left/right=tank pivot toward target, all
//      at FULL_SPEED. Line sensors are NOT queried in this mode.
//    - Protocol: MODE:SUMO_NL. Telemetry mode string: "SUMO_NL".
//    - Persisted in NVS (MODE_SUMO_NL = 5, appended — no migration).
//
//  MODES:
//    MANUAL   — ESP-NOW drive + AUX LED control
//    AUTO     — Forward → IR/front-sonar → eyes ON → back full speed
//               → charge LEDs 1→2→3 → pivot right 4 s → repeat
//    SUMO     — Ring sumo. 5 s countdown → pivot scan → detect closest
//               via sonar → full-speed ram → edge backup+reorient
//               (uses line sensors as ring-edge detector)
//    SUMO_NL  — Open-floor sumo. 5 s countdown → curve-R 3 s / curve-L
//               3 s / U-turn search loop → any sonar ≤ 20 cm rams at
//               full speed. NO line sensors.
//    MAZE     — Seek open corridors. Priority: front → left → right.
//               Dead end: reverse until opening found, then turn.
//
//  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

// ── 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;   // swapped — physical wiring crossover
static const int SONAR_RIGHT = 5;    // swapped — physical wiring crossover
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  = 30;
static const int           MAZE_OPEN_CM     = 50;   // side corridor "open" if reading > 50 cm
static const int           MAZE_APPROACH_CM = 40;   // front: start evaluating turns at this distance
                                                     // must be > MAZE_STOP_CM + 15 cm body-gap buffer
static const int           MAZE_STOP_CM     = 20;   // emergency: wall this close → reverse
                                                     // (was 8 — too late, caused wall bang)
static const unsigned long SONAR_TIMEOUT_US = 23200UL;
static const unsigned long SONAR_SETTLE_MS  = 30;   // shared TRIG: 30 ms min between pings

// ── 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, MODE_SUMO_NL = 5 };

// AUTO sub-states
enum AutoState {
  AUTO_FORWARD,
  AUTO_BACKUP,
  AUTO_CHARGE_LEDS,
  AUTO_PIVOT
};

// SUMO sub-states
// SUMO_COUNTDOWN is the first state: 5 s non-blocking wait before combat.
enum SumoState {
  SUMO_COUNTDOWN,
  SUMO_SCAN,
  SUMO_ATTACK,
  SUMO_BACKUP,
  SUMO_REORIENT
};

// SUMO_NL (no line sensor) sub-states
//   Search pattern: SEARCH_CURVE_R → SEARCH_CURVE_L → SEARCH_UTURN → repeat
//   Any sonar ≤ 20 cm during any phase → ATTACK
enum SumoNlState {
  SUMO_NL_COUNTDOWN,
  SUMO_NL_SEARCH_CURVE_R,
  SUMO_NL_SEARCH_CURVE_L,
  SUMO_NL_SEARCH_UTURN,
  SUMO_NL_ATTACK
};

// MAZE sub-states
enum MazeState {
  MAZE_RUN,            // driving forward, evaluating corridor
  MAZE_TURN_RIGHT,     // timed 90° pivot right
  MAZE_TURN_LEFT,      // timed 90° pivot left
  MAZE_REVERSE,        // emergency backup (too close to wall)
  MAZE_REVERSE_SEARCH  // dead end: reverse until a side corridor opens
};

RobotMode currentMode = MODE_MANUAL;
AutoState autoState   = AUTO_FORWARD;
SumoState sumoState   = SUMO_COUNTDOWN;
SumoNlState sumoNlState = SUMO_NL_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;   // front
int    sonar2Cm       = 0;   // left
int    sonar3Cm       = 0;   // right

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

// ── Maze vars ─────────────────────────────────────────────────
bool mazeIsBacktracking = false;  // kept so telemetry compiles
bool mazeRevTurnRight   = false;  // direction chosen after emergency reverse

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

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

// ── Maze timing ───────────────────────────────────────────────
static const int           MAZE_DRIVE_SPEED = 80;   // slow crawl — safe reaction at 300RPM N20
static const int           MAZE_TURN_SPEED  = 160;  // pivot speed — slower = tighter angle control
static const unsigned long MAZE_TURN_MS     = 600;  // 90° tank pivot — tune if over/under rotates
static const unsigned long MAZE_REV_MS      = 400;  // backup duration after emergency close-wall

// ── Sumo (no line sensor) settings ────────────────────────────
static const int           SUMO_NL_RAM_SPEED    = FULL_SPEED;       // 255
static const int           SUMO_NL_SEARCH_SPEED = FULL_SPEED / 2;   // 127 — exactly half of ramming
static const int           SUMO_NL_DETECT_CM    = 20;                // attack threshold (all sonars)
static const unsigned long SUMO_NL_CURVE_MS     = 3000UL;            // each curve phase duration
static const unsigned long SUMO_NL_UTURN_MS     = 1200UL;            // ≈180° tank pivot (2 × MAZE_TURN_MS)

// ============================================================
// NVS — PERSISTENT MODE
// ============================================================

void saveModeToNvs(RobotMode mode) {
  if (mode == MODE_AUTO) return;   // excluded — always boots MANUAL
  if (mode == MODE_OTA)  return;   // excluded — never persisted
  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();
  // MODE_AUTO and MODE_OTA are never persisted; anything outside the enum range falls back.
  if (raw == (uint8_t)MODE_AUTO || raw == (uint8_t)MODE_OTA || raw > (uint8_t)MODE_SUMO_NL) {
    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_SUMO_NL: return "SUMO_NL";
    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.
// Master forwards STATUS: lines raw (not TEL:-prefixed).
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 / UNIQUE 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 BOT MODE
// ============================================================
void enterSumo(SumoState next) {
  sumoState      = next;
  stateStartedMs = millis();
}

void resetSumo() {
  clearLeds();
  stopRobot("SUMO_COUNTDOWN");
  enterSumo(SUMO_COUNTDOWN);   // always start with 5 s countdown
}

// Returns true and drives toward target if one is found within threshold.
// Priority: front → left → right (closest within threshold).
bool sumoCheckAndAttack() {
  int bestDist = SONAR_DETECT_CM + 1;
  int bestDir  = 0;  // 0=none 1=front 2=left 3=right

  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;
}

// Countdown LED animation: fills CHG3→CHG2→CHG1 one per second,
// then flashes all during the final two seconds.
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;

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

  // Edge detection always takes priority during combat
  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:
      // handled above — unreachable here
      break;
  }
}

// ============================================================
// SUMO BOT MODE — NO LINE SENSOR  v1.0
//
// For arenas with no sumo ring boundary. Line sensors are NEVER queried.
//
// Behaviour:
//   1. 5 s arm countdown (reuses existing LED animation).
//   2. Search pattern (looped, restarts after every attack):
//        a. Curve right  3 s   (outer=SEARCH_SPEED, inner=SEARCH_SPEED/2)
//        b. Curve left   3 s   (outer=SEARCH_SPEED, inner=SEARCH_SPEED/2)
//        c. U-turn       1.2 s (tank pivot right at TURN_SPEED, ≈180°)
//   3. Detection — checked every loop() tick, all phases:
//        Any sonar reading ≤ 20 cm → ATTACK
//          • Front detected → drive straight forward at FULL_SPEED
//          • Left  detected → tank-pivot left  at FULL_SPEED
//          • Right detected → tank-pivot right at FULL_SPEED
//        Priority: front > left > right (front wins once target enters
//        the front cone, naturally converting a side pivot into a straight ram).
//   4. Target lost → resume search from curve-right.
//
// Search speed = SUMO_NL_SEARCH_SPEED = FULL_SPEED / 2 (literally half).
// ============================================================

void enterSumoNl(SumoNlState next) {
  sumoNlState    = next;
  stateStartedMs = millis();
}

void resetSumoNl() {
  clearLeds();
  stopRobot("SUMO_NL_COUNTDOWN");
  enterSumoNl(SUMO_NL_COUNTDOWN);
}

// Returns true if any sonar sees a target ≤ SUMO_NL_DETECT_CM and drives toward it.
// Priority: front → left → right. Front wins outright (straight ram).
bool sumoNlCheckAndAttack() {
  bool front = (sonar1Cm > 0 && sonar1Cm <= SUMO_NL_DETECT_CM);
  bool left  = (sonar2Cm > 0 && sonar2Cm <= SUMO_NL_DETECT_CM);
  bool right = (sonar3Cm > 0 && sonar3Cm <= SUMO_NL_DETECT_CM);

  if (!front && !left && !right) return false;

  setEyes(true);
  if (front) {
    driveForward(SUMO_NL_RAM_SPEED, "SUMO_NL_RAM_F");
  } else if (left) {
    setDrive(-SUMO_NL_RAM_SPEED, SUMO_NL_RAM_SPEED, "SUMO_NL_RAM_L");
  } else {
    setDrive(SUMO_NL_RAM_SPEED, -SUMO_NL_RAM_SPEED, "SUMO_NL_RAM_R");
  }
  return true;
}

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

  // ── Countdown phase ──
  if (sumoNlState == SUMO_NL_COUNTDOWN) {
    stopRobot("SUMO_NL_ARMED");
    sumoCountdownLeds(elapsed);   // reuse existing 5 s LED animation
    if (elapsed >= SUMO_COUNTDOWN_MS) {
      clearChargeLeds();
      enterSumoNl(SUMO_NL_SEARCH_CURVE_R);
    }
    return;
  }

  // ── Detection always takes priority during combat ──
  if (sumoNlCheckAndAttack()) {
    if (sumoNlState != SUMO_NL_ATTACK) enterSumoNl(SUMO_NL_ATTACK);
    return;
  }

  // ── No target — execute search pattern ──
  switch (sumoNlState) {

    case SUMO_NL_SEARCH_CURVE_R:
      // Curve right: left wheel = SEARCH_SPEED (outer), right wheel = half (inner)
      setEyes(false);
      setDrive(SUMO_NL_SEARCH_SPEED, SUMO_NL_SEARCH_SPEED / 2, "SUMO_NL_CURVE_R");
      if (elapsed >= SUMO_NL_CURVE_MS) enterSumoNl(SUMO_NL_SEARCH_CURVE_L);
      break;

    case SUMO_NL_SEARCH_CURVE_L:
      // Curve left: right wheel = SEARCH_SPEED (outer), left wheel = half (inner)
      setEyes(false);
      setDrive(SUMO_NL_SEARCH_SPEED / 2, SUMO_NL_SEARCH_SPEED, "SUMO_NL_CURVE_L");
      if (elapsed >= SUMO_NL_CURVE_MS) enterSumoNl(SUMO_NL_SEARCH_UTURN);
      break;

    case SUMO_NL_SEARCH_UTURN:
      // 180° tank pivot — direction is consistent (right), rotating the
      // search heading each cycle to sweep the arena.
      setEyes(false);
      pivotRight(TURN_SPEED, "SUMO_NL_UTURN");
      if (elapsed >= SUMO_NL_UTURN_MS) enterSumoNl(SUMO_NL_SEARCH_CURVE_R);
      break;

    case SUMO_NL_ATTACK:
      // Target lost mid-attack — restart search cycle
      setEyes(false);
      enterSumoNl(SUMO_NL_SEARCH_CURVE_R);
      break;

    case SUMO_NL_COUNTDOWN:
      // handled above — unreachable here
      break;
  }
}

// ============================================================
// MAZE SOLVER  v2.2
//
// Physical layout:
//   Front sonar  — at the robot nose
//   Side sonars  — ~15 cm behind the nose
//
// The 15 cm gap means: when the front sonar sees a wall at 40 cm,
// the side sonars are still inside the current corridor.
// MAZE_APPROACH_CM (40 cm) gives the body enough travel time to
// reach the junction mouth before a turn is committed, preventing
// premature turns that miss the gap and steer into a wall.
//
// Decision priority in MAZE_RUN each loop() tick:
//   0. Emergency (front ≤ MAZE_STOP_CM)   → MAZE_REVERSE
//   1. Approaching (front ≤ MAZE_APPROACH_CM):
//        a. Left open  → stop + MAZE_TURN_LEFT
//        b. Right open → stop + MAZE_TURN_RIGHT
//        c. Both blocked → MAZE_REVERSE_SEARCH (dead end)
//   2. Front clear → keep driving
//
// Dead end behaviour:
//   No U-turn. Bot enters MAZE_REVERSE_SEARCH and reverses
//   continuously. Sonars are read every loop() cycle. As soon as
//   a side corridor opens, bot stops and pivots into it.
//   Line sensor (rear) used as a rear-wall safety stop.
// ============================================================

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

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

// ── Sonar predicates ─────────────────────────────────────────
// Open  = timeout (999, nothing in range) OR reading > MAZE_OPEN_CM
// Close = valid reading ≤ MAZE_APPROACH_CM (start evaluating turn)
// TooClose = valid reading ≤ MAZE_STOP_CM (emergency reverse)
bool mazeFrontOpen()     { return sonar1Cm == 999 || sonar1Cm > MAZE_OPEN_CM; }
bool mazeFrontClose()    { return sonar1Cm > 0 && sonar1Cm != 999 && sonar1Cm <= MAZE_APPROACH_CM; }
bool mazeTooClose()      { return sonar1Cm > 0 && sonar1Cm != 999 && sonar1Cm <= MAZE_STOP_CM; }
bool mazeLeftOpen()      { return sonar2Cm == 999 || sonar2Cm > MAZE_OPEN_CM; }
bool mazeRightOpen()     { return sonar3Cm == 999 || sonar3Cm > MAZE_OPEN_CM; }

// ── Main maze update ─────────────────────────────────────────
void updateMaze() {
  unsigned long elapsed = millis() - stateStartedMs;

  switch (mazeState) {

    // ── DRIVING FORWARD ──────────────────────────────────────
    case MAZE_RUN: {

      // 0. Emergency: too close — back up immediately
      if (mazeTooClose()) {
        mazeRevTurnRight = mazeRightOpen() && !mazeLeftOpen();
        driveBackward(MAZE_DRIVE_SPEED, "MAZE_EMERG_REV");
        enterMaze(MAZE_REVERSE);
        return;
      }

      // 1. Approaching a wall — evaluate junction now
      if (mazeFrontClose()) {
        bool oL = mazeLeftOpen();
        bool oR = mazeRightOpen();

        if (oL) {
          // Left corridor open — turn left
          stopRobot("MAZE_EVAL_STOP");
          delay(80);
          pivotLeft(MAZE_TURN_SPEED, "MAZE_TL");
          enterMaze(MAZE_TURN_LEFT);
          return;
        }

        if (oR) {
          // Right corridor open — turn right
          stopRobot("MAZE_EVAL_STOP");
          delay(80);
          pivotRight(MAZE_TURN_SPEED, "MAZE_TR");
          enterMaze(MAZE_TURN_RIGHT);
          return;
        }

        // Both sides blocked — dead end, begin reverse search
        driveBackward(MAZE_DRIVE_SPEED, "MAZE_DEADEND_REV");
        enterMaze(MAZE_REVERSE_SEARCH);
        return;
      }

      // 2. Path ahead is clear — keep driving
      driveForward(MAZE_DRIVE_SPEED, "MAZE_RUN");
      break;
    }

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

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

    // ── REVERSE — emergency (was too close to front wall) ────
    // Backs up for MAZE_REV_MS, then turns away from the wall.
    // Direction was chosen at the moment the emergency triggered.
    case MAZE_REVERSE:
      if (elapsed >= MAZE_REV_MS) {
        stopRobot("MAZE_REV_DONE");
        delay(80);
        if (mazeRevTurnRight) {
          pivotRight(MAZE_TURN_SPEED, "MAZE_EMERG_TR");
          enterMaze(MAZE_TURN_RIGHT);
        } else {
          pivotLeft(MAZE_TURN_SPEED, "MAZE_EMERG_TL");
          enterMaze(MAZE_TURN_LEFT);
        }
      }
      break;

    // ── REVERSE SEARCH — dead end ────────────────────────────
    // Continuously reverses while reading sonars each loop() cycle.
    // Turns into the first side corridor that opens.
    // No U-turn — the bot will naturally find the passage it came from.
    //
    // Rear line sensor used as safety stop: if bot backs onto a line
    // (or a physical rear wall), it stops reversing and re-enters
    // MAZE_RUN to prevent damage.
    case MAZE_REVERSE_SEARCH: {

      // Rear safety: stop if line sensor fires during reverse
      if (lineBackDetected()) {
        stopRobot("MAZE_RS_LINE_STOP");
        delay(200);
        driveForward(MAZE_DRIVE_SPEED, "MAZE_RS_FWD_RECOVER");
        enterMaze(MAZE_RUN);
        return;
      }

      bool oL = mazeLeftOpen();
      bool oR = mazeRightOpen();

      if (oL) {
        stopRobot("MAZE_RS_STOP");
        delay(80);
        pivotLeft(MAZE_TURN_SPEED, "MAZE_RS_TL");
        enterMaze(MAZE_TURN_LEFT);
        return;
      }

      if (oR) {
        stopRobot("MAZE_RS_STOP");
        delay(80);
        pivotRight(MAZE_TURN_SPEED, "MAZE_RS_TR");
        enterMaze(MAZE_TURN_RIGHT);
        return;
      }

      // No opening yet — keep reversing
      driveBackward(MAZE_DRIVE_SPEED, "MAZE_RS_REV");
      break;
    }
  }
}

// ============================================================
// ESP-NOW INIT HELPER (boot + post-OTA restore)
// ============================================================
void initEspNow() {
  if (esp_now_init() != ESP_OK) return;
  esp_now_register_recv_cb(onDataReceived);
  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();

  // Use WIFI_AP_STA so the STA radio stays alive — ESP-NOW can still
  // send status packets back to master during and after the flash.
  WiFi.mode(WIFI_AP_STA);
  WiFi.softAP(OTA_SSID, OTA_PASS);
  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>"
    );
  });

  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();
      if (upload.status == UPLOAD_FILE_START) {
        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);
        if (upload.totalSize > 0) {
          uint8_t pct = (uint8_t)((upload.currentSize * 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;
  }
  esp_now_deinit();
  masterRegistered = false;
  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();

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

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

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

  // ── MODE ──
  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 == "SUMO_NL") { currentMode = MODE_SUMO_NL; resetSumoNl(); }
    else if (m == "MAZE")    { currentMode = MODE_MAZE;    resetMaze();   }
    else if (m == "OTA")     { enterOTAMode(); return; }
    else                     { return; }
    updateRgbStatus();
    saveModeToNvs(currentMode);
    return;
  }

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

  // ── AUX LEDs (manual mode only) ──
  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_SUMO_NL: resetSumoNl();  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() {
  // OTA mode: only serve web requests, skip all robot logic
  if (currentMode == MODE_OTA) {
    if (otaServerRunning) otaWebServer.handleClient();
    return;
  }

  // Skip sonar during timed pivots and fixed-time reverse —
  // mid-pivot readings are physically meaningless.
  // MAZE_REVERSE_SEARCH is intentionally NOT skipped: it depends
  // on live sonar readings to detect when a side corridor opens.
  bool skipSonar = (currentMode == MODE_MAZE &&
                    (mazeState == MAZE_TURN_LEFT  ||
                     mazeState == MAZE_TURN_RIGHT ||
                     mazeState == MAZE_REVERSE));
  if (!skipSonar) updateSonars();

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

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