#include <Arduino.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>

// ─────────────────────────────────────────────
//  PIN DEFINITIONS
// ─────────────────────────────────────────────
#define PIN_SERVO_BASE    19
#define PIN_SERVO_LINK1   18
#define PIN_SERVO_LINK2    5
#define PIN_SERVO_LINK3   17
#define PIN_SERVO_GRIP1   16
#define PIN_SERVO_GRIP2   23
#define PIN_RELAY_MOTOR    4
#define PIN_ULTRA_X_TRIG  33
#define PIN_ULTRA_X_ECHO  35
#define PIN_ULTRA_Z_TRIG  34
#define PIN_ULTRA_Z_ECHO  32
#define PIN_COLOR_S0      12
#define PIN_COLOR_S1      13
#define PIN_COLOR_S2      14
#define PIN_COLOR_OUT     27
#define PIN_JOY_VRX        2
#define PIN_JOY_VRY        0
#define PIN_JOY_SW        15

#define JOY_DEADZONE      400
#define JOY_CENTRE        2048
#define JOY_REPEAT_MS     300

#define PCF8574_ADDR      0x20
// Buttons on P6/P7 — no conflict with LED outputs on P0-P4
#define PCF_BTN_BACK      6
#define PCF_BTN_HOME      7
#define PCF_LED_STATUS0   0
#define PCF_LED_STATUS1   1
#define PCF_LED_RED       2
#define PCF_LED_GREEN     3
#define PCF_LED_BLUE      4

// ─────────────────────────────────────────────
//  TIMING
// ─────────────────────────────────────────────
#define DEBOUNCE_MS         200
#define CURSOR_BLINK_MS     500
#define SENSOR_INTERVAL_MS  100
#define WARMUP_STEP_MS     2000
#define STATUS_REFRESH_MS   200

// ─────────────────────────────────────────────
//  ENUMS
// ─────────────────────────────────────────────
enum UIState {
  UI_MAIN_MENU, UI_START_MENU, UI_MODE_MENU, UI_WARMUP,
  UI_SETTINGS_MENU, UI_COLOR_SELECT, UI_STATUS_SCREEN
};

// Parent screen for each state (used by doBack())
const UIState BACK_TARGET[] = {
  UI_MAIN_MENU,     // MAIN_MENU   — root
  UI_MAIN_MENU,     // START_MENU  → Main
  UI_START_MENU,    // MODE_MENU   → Start
  UI_START_MENU,    // WARMUP      → Start
  UI_MAIN_MENU,     // SETTINGS    → Main
  UI_SETTINGS_MENU, // COLOR_SEL   → Settings
  UI_START_MENU,    // STATUS      → Start
};

enum RobotMode    { MODE_IDLE, MODE_MANUAL, MODE_AUTO };
enum WarmupStep   { WU_SERVO_BASE, WU_SERVO_LINKS, WU_SERVO_GRIPPER,
                    WU_CONVEYOR, WU_ULTRASONIC, WU_COLOR_SENSOR, WU_DONE };
enum DetectedColor{ COLOR_NONE, COLOR_RED, COLOR_GREEN, COLOR_BLUE };

// ─────────────────────────────────────────────
//  GLOBALS
// ─────────────────────────────────────────────
LiquidCrystal_I2C lcd(0x27, 20, 4);
Servo servoBase, servoLink1, servoLink2, servoLink3, servoGrip1, servoGrip2;

UIState       uiState         = UI_MAIN_MENU;
int           cursorPos       = 0;
bool          cursorVisible   = true;
unsigned long lastCursorBlink = 0;
bool          lcdDirty        = true;

struct JoyInput {
  bool up=false, down=false, select=false, back=false, home=false;
  int  vrx=JOY_CENTRE, vry=JOY_CENTRE;
  unsigned long swLastChange=0, backLastChange=0, homeLastChange=0;
  int  lastDir=0;
  unsigned long lastNavTime=0;
} joy;

bool colorSortEnabled=false, sortRed=false, sortGreen=false, sortBlue=false;
RobotMode      robotMode    = MODE_IDLE;
bool           robotRunning = false;
DetectedColor  detectedColor= COLOR_NONE;
WarmupStep     warmupStep   = WU_SERVO_BASE;
unsigned long  warmupTimer  = 0;
bool           warmupDone[7]= {false};
bool           inWarmup     = false;
float          distX=0, distZ=0;
unsigned long  lastSensorRead=0, lastStatusRefresh=0;
char           prevLine[4][21];

// ─────────────────────────────────────────────
//  PCF8574
// ─────────────────────────────────────────────
uint8_t pcf8574Read() {
  Wire.requestFrom(PCF8574_ADDR, 1);
  return Wire.available() ? Wire.read() : 0xFF;
}
void pcf8574Write(uint8_t val) {
  Wire.beginTransmission(PCF8574_ADDR); Wire.write(val); Wire.endTransmission();
}

// ─────────────────────────────────────────────
//  JOYSTICK
// ─────────────────────────────────────────────
void readJoystick() {
  unsigned long now = millis();
  joy.up=joy.down=joy.select=joy.back=joy.home=false;
  joy.vrx=analogRead(PIN_JOY_VRX); joy.vry=analogRead(PIN_JOY_VRY);

  int dir=0;
  if      (joy.vry<(JOY_CENTRE-JOY_DEADZONE)) dir=-1;
  else if (joy.vry>(JOY_CENTRE+JOY_DEADZONE)) dir=+1;
  if (dir!=0 && (dir!=joy.lastDir||(now-joy.lastNavTime)>=JOY_REPEAT_MS)) {
    if(dir==-1) joy.up=true; else joy.down=true;
    joy.lastNavTime=now;
  }
  joy.lastDir=dir;

  if (digitalRead(PIN_JOY_SW)==LOW && (now-joy.swLastChange)>=DEBOUNCE_MS) {
    joy.select=true; joy.swLastChange=now;
  }
  uint8_t pcf=pcf8574Read();
  if (!((pcf>>PCF_BTN_BACK)&1) && (now-joy.backLastChange)>=DEBOUNCE_MS) {
    joy.back=true; joy.backLastChange=now;
  }
  if (!((pcf>>PCF_BTN_HOME)&1) && (now-joy.homeLastChange)>=DEBOUNCE_MS) {
    joy.home=true; joy.homeLastChange=now;
  }
}

// ─────────────────────────────────────────────
//  LCD HELPERS
// ─────────────────────────────────────────────
void lcdWriteIfChanged(uint8_t row, const char *text) {
  char padded[21]; snprintf(padded,sizeof(padded),"%-20s",text);
  if (memcmp(prevLine[row],padded,20)!=0) {
    lcd.setCursor(0,row); lcd.print(padded);
    memcpy(prevLine[row],padded,20);
  }
}
void clearShadow() { for(int i=0;i<4;i++) memset(prevLine[i],0,sizeof(prevLine[i])); }
char cursorChar() { return cursorVisible?'>':' '; }

// ─────────────────────────────────────────────
//  GENERIC SCROLLING MENU RENDERER
//  items[] — array of label strings
//  count   — total items including the "< Back" entry
//  title   — row 0 header (must be exactly 20 chars or will be padded)
//
//  The visible window (rows 1-3) follows cursorPos so the
//  selected item is always visible.
// ─────────────────────────────────────────────
void renderScrollMenu(const char *title, const char **items, int count) {
  lcdWriteIfChanged(0, title);
  int start = cursorPos - 1;
  if (start < 0)          start = 0;
  if (start > count - 3)  start = max(0, count - 3);
  for (int row=1; row<=3; row++) {
    int idx = start + (row-1);
    if (idx < count) {
      char line[21];
      snprintf(line, 21, "%c %-17s", idx==cursorPos ? cursorChar() : ' ', items[idx]);
      lcdWriteIfChanged(row, line);
    } else {
      lcdWriteIfChanged(row, "                    ");
    }
  }
}

// ─────────────────────────────────────────────
//  MENU RENDERERS
// ─────────────────────────────────────────────

void renderMainMenu() {
  const char *items[] = { "Start", "Settings" };
  renderScrollMenu("    ROBOTIZER       ", items, 2);
}

void renderStartMenu() {
  // Index 2 = "< Back"
  const char *items[] = { "Mode", "Warm Up", "< Back" };
  renderScrollMenu("   START MENU       ", items, 3);
}

void renderModeMenu() {
  // Index 2 = "< Back"
  const char *items[] = { "Manual Control", "Automatic", "< Back" };
  renderScrollMenu("   MODE MENU        ", items, 3);
}

void renderSettingsMenu() {
  char sortLabel[18];
  snprintf(sortLabel, sizeof(sortLabel), "Color Sort: %s", colorSortEnabled ? "ON " : "OFF");
  // Index 2 = "< Back"
  const char *items[] = { sortLabel, "Color Filter", "< Back" };
  renderScrollMenu("    SETTINGS        ", items, 3);
}

void renderColorSelect() {
  char rLabel[18], gLabel[18], bLabel[18];
  snprintf(rLabel, sizeof(rLabel), "[%c] Red",   sortRed   ? 'X' : ' ');
  snprintf(gLabel, sizeof(gLabel), "[%c] Green", sortGreen ? 'X' : ' ');
  snprintf(bLabel, sizeof(bLabel), "[%c] Blue",  sortBlue  ? 'X' : ' ');
  // Index 3 = "< Back"
  const char *items[] = { rLabel, gLabel, bLabel, "< Back" };
  renderScrollMenu("  SELECT COLORS     ", items, 4);
}

void renderWarmupMenu() {
  const char *labels[6] = {
    "Base Servo","Link Servos","Gripper","Conveyor","Ultrasonic","Color Sensor"
  };
  lcdWriteIfChanged(0, "   WARM UP TEST     ");
  if (warmupStep == WU_DONE) {
    lcdWriteIfChanged(1, "  All Tests PASSED! ");
    lcdWriteIfChanged(2, "                    ");
    lcdWriteIfChanged(3, "  [Select] = Back   ");
    return;
  }
  int start = max(0, (int)warmupStep - 1);
  for (int row=1; row<=3; row++) {
    int idx = start + (row-1);
    if (idx < 6) {
      char line[21];
      const char *st = warmupDone[idx]         ? "Done \x7E"  :
                       (idx==(int)warmupStep   ? "Testing.." : "Waiting ");
      snprintf(line, 21, "%-12s%s", labels[idx], st);
      lcdWriteIfChanged(row, line);
    } else {
      lcdWriteIfChanged(row, "                    ");
    }
  }
}

void renderStatusScreen() {
  unsigned long now = millis();
  if (now-lastStatusRefresh < STATUS_REFRESH_MS) return;
  lastStatusRefresh = now;
  const char *ms = robotMode==MODE_MANUAL?"Manual":robotMode==MODE_AUTO?"Auto  ":"Idle  ";
  const char *cs = detectedColor==COLOR_RED?"Red  ":detectedColor==COLOR_GREEN?"Green":
                   detectedColor==COLOR_BLUE?"Blue ":"None ";
  char l0[21],l1[21],l2[21];
  snprintf(l0,21,"STATUS:%-13s", robotRunning?"RUNNING":"IDLE   ");
  snprintf(l1,21,"Mode:%-15s",   ms);
  snprintf(l2,21,"Color:%-14s",  cs);
  lcdWriteIfChanged(0,l0); lcdWriteIfChanged(1,l1);
  lcdWriteIfChanged(2,l2); lcdWriteIfChanged(3,"[Select]=Stop+Back  ");
}

// ─────────────────────────────────────────────
//  STATE TRANSITIONS
// ─────────────────────────────────────────────
void goTo(UIState next, int cursor=0) {
  uiState=next; cursorPos=cursor; lcdDirty=true;
  clearShadow(); lcd.clear(); memset(prevLine,0,sizeof(prevLine));
}

void doBack() {
  if (uiState==UI_STATUS_SCREEN) { robotRunning=false; robotMode=MODE_IDLE; }
  if (uiState==UI_WARMUP)        { inWarmup=false; warmupStep=WU_SERVO_BASE; memset(warmupDone,false,sizeof(warmupDone)); }
  goTo(BACK_TARGET[uiState]);
}

// ─────────────────────────────────────────────
//  UI HANDLER
// ─────────────────────────────────────────────
void handleUI() {
  unsigned long now = millis();
  if (now-lastCursorBlink>=CURSOR_BLINK_MS) {
    lastCursorBlink=now; cursorVisible=!cursorVisible; lcdDirty=true;
  }

  // Physical HOME
  if (joy.home) {
    robotRunning=false; robotMode=MODE_IDLE; inWarmup=false;
    warmupStep=WU_SERVO_BASE; memset(warmupDone,false,sizeof(warmupDone));
    goTo(UI_MAIN_MENU); return;
  }
  // Physical BACK
  if (joy.back) { doBack(); return; }

  switch (uiState) {

    // ── MAIN MENU  (0=Start, 1=Settings — no Back item)
    case UI_MAIN_MENU:
      if (joy.up   && cursorPos>0) { cursorPos--; lcdDirty=true; }
      if (joy.down && cursorPos<1) { cursorPos++; lcdDirty=true; }
      if (joy.select) {
        if      (cursorPos==0) goTo(UI_START_MENU);
        else if (cursorPos==1) goTo(UI_SETTINGS_MENU);
      }
      if (lcdDirty) { renderMainMenu(); lcdDirty=false; }
      break;

    // ── START MENU  (0=Mode, 1=WarmUp, 2=Back)
    case UI_START_MENU:
      if (joy.up   && cursorPos>0) { cursorPos--; lcdDirty=true; }
      if (joy.down && cursorPos<2) { cursorPos++; lcdDirty=true; }
      if (joy.select) {
        if      (cursorPos==0) { goTo(UI_MODE_MENU); }
        else if (cursorPos==1) {
          warmupStep=WU_SERVO_BASE; memset(warmupDone,false,sizeof(warmupDone));
          warmupTimer=millis(); inWarmup=true; goTo(UI_WARMUP);
        }
        else                   { doBack(); }   // ← "< Back" selected
      }
      if (lcdDirty) { renderStartMenu(); lcdDirty=false; }
      break;

    // ── MODE MENU  (0=Manual, 1=Auto, 2=Back)
    case UI_MODE_MENU:
      if (joy.up   && cursorPos>0) { cursorPos--; lcdDirty=true; }
      if (joy.down && cursorPos<2) { cursorPos++; lcdDirty=true; }
      if (joy.select) {
        if      (cursorPos==0) { robotMode=MODE_MANUAL; robotRunning=true; goTo(UI_STATUS_SCREEN); }
        else if (cursorPos==1) { robotMode=MODE_AUTO;   robotRunning=true; goTo(UI_STATUS_SCREEN); }
        else                   { doBack(); }   // ← "< Back" selected
      }
      if (lcdDirty) { renderModeMenu(); lcdDirty=false; }
      break;

    // ── WARM-UP  (no cursor items; Select when done = Back)
    case UI_WARMUP:
      renderWarmupMenu();
      if (joy.select && warmupStep==WU_DONE) doBack();
      break;

    // ── SETTINGS MENU  (0=ColorSort, 1=ColorFilter, 2=Back)
    case UI_SETTINGS_MENU:
      if (joy.up   && cursorPos>0) { cursorPos--; lcdDirty=true; }
      if (joy.down && cursorPos<2) { cursorPos++; lcdDirty=true; }
      if (joy.select) {
        if      (cursorPos==0) { colorSortEnabled=!colorSortEnabled; lcdDirty=true; }
        else if (cursorPos==1) { if (colorSortEnabled) goTo(UI_COLOR_SELECT); }
        else                   { doBack(); }   // ← "< Back" selected
      }
      if (lcdDirty) { renderSettingsMenu(); lcdDirty=false; }
      break;

    // ── COLOR SELECT  (0=Red, 1=Green, 2=Blue, 3=Back)
    case UI_COLOR_SELECT:
      if (joy.up   && cursorPos>0) { cursorPos--; lcdDirty=true; }
      if (joy.down && cursorPos<3) { cursorPos++; lcdDirty=true; }
      if (joy.select) {
        if      (cursorPos==0) { sortRed   =!sortRed;   lcdDirty=true; }
        else if (cursorPos==1) { sortGreen =!sortGreen; lcdDirty=true; }
        else if (cursorPos==2) { sortBlue  =!sortBlue;  lcdDirty=true; }
        else                   { doBack(); }   // ← "< Back" selected
      }
      if (lcdDirty) { renderColorSelect(); lcdDirty=false; }
      break;

    // ── STATUS SCREEN  (Select = stop robot + back)
    case UI_STATUS_SCREEN:
      renderStatusScreen();
      if (joy.select) doBack();
      break;
  }
}

// ─────────────────────────────────────────────
//  WARM-UP STATE MACHINE
// ─────────────────────────────────────────────
void handleWarmup() {
  if (!inWarmup||warmupStep==WU_DONE) return;
  unsigned long now=millis();
  if (now-warmupTimer<WARMUP_STEP_MS) return;
  switch (warmupStep) {
    case WU_SERVO_BASE:    servoBase.write(90); warmupDone[WU_SERVO_BASE]=true; warmupStep=WU_SERVO_LINKS; break;
    case WU_SERVO_LINKS:   servoLink1.write(90); servoLink2.write(90); servoLink3.write(90); warmupDone[WU_SERVO_LINKS]=true; warmupStep=WU_SERVO_GRIPPER; break;
    case WU_SERVO_GRIPPER: servoGrip1.write(90); servoGrip2.write(90); warmupDone[WU_SERVO_GRIPPER]=true; warmupStep=WU_CONVEYOR; break;
    case WU_CONVEYOR:      digitalWrite(PIN_RELAY_MOTOR,HIGH); warmupDone[WU_CONVEYOR]=true; warmupStep=WU_ULTRASONIC; break;
    case WU_ULTRASONIC:    warmupDone[WU_ULTRASONIC]=true; digitalWrite(PIN_RELAY_MOTOR,LOW); warmupStep=WU_COLOR_SENSOR; break;
    case WU_COLOR_SENSOR:  warmupDone[WU_COLOR_SENSOR]=true; warmupStep=WU_DONE; inWarmup=false; break;
    default: break;
  }
  warmupTimer=now; clearShadow();
}

// ─────────────────────────────────────────────
//  SENSORS
// ─────────────────────────────────────────────
float measureUltrasonic(uint8_t trig, uint8_t echo) {
  digitalWrite(trig,LOW); delayMicroseconds(2);
  digitalWrite(trig,HIGH); delayMicroseconds(10); digitalWrite(trig,LOW);
  return pulseIn(echo,HIGH,30000)*0.0343f/2.0f;
}
void handleSensors() {
  unsigned long now=millis();
  if (now-lastSensorRead<SENSOR_INTERVAL_MS) return;
  lastSensorRead=now;
  distX=measureUltrasonic(PIN_ULTRA_X_TRIG,PIN_ULTRA_X_ECHO);
  distZ=measureUltrasonic(PIN_ULTRA_Z_TRIG,PIN_ULTRA_Z_ECHO);
  digitalWrite(PIN_COLOR_S2,LOW);  long rFreq=pulseIn(PIN_COLOR_OUT,LOW,100000);
  digitalWrite(PIN_COLOR_S2,HIGH); long gFreq=pulseIn(PIN_COLOR_OUT,LOW,100000);
  digitalWrite(PIN_COLOR_S2,LOW);  long bFreq=pulseIn(PIN_COLOR_OUT,LOW,100000);
  if      (rFreq<gFreq&&rFreq<bFreq&&rFreq>0) detectedColor=COLOR_RED;
  else if (gFreq<rFreq&&gFreq<bFreq&&gFreq>0) detectedColor=COLOR_GREEN;
  else if (bFreq<rFreq&&bFreq<gFreq&&bFreq>0) detectedColor=COLOR_BLUE;
  else                                          detectedColor=COLOR_NONE;
}

// ─────────────────────────────────────────────
//  AUTO CONTROL
// ─────────────────────────────────────────────
enum AutoStep { AUTO_IDLE,AUTO_APPROACH,AUTO_PICK,AUTO_SORT,AUTO_RETURN };
AutoStep autoStep=AUTO_IDLE; unsigned long autoTimer=0;
void handleAutoControl() {
  if (robotMode!=MODE_AUTO||!robotRunning) return;
  unsigned long now=millis();
  switch(autoStep) {
    case AUTO_IDLE:     if(distX>0&&distX<10.0f){autoStep=AUTO_APPROACH;autoTimer=now;} break;
    case AUTO_APPROACH: if(now-autoTimer>=600){servoLink1.write(60);servoLink2.write(100);autoStep=AUTO_PICK;autoTimer=now;} break;
    case AUTO_PICK:     if(now-autoTimer>=800){servoGrip1.write(45);servoGrip2.write(135);autoStep=AUTO_SORT;autoTimer=now;} break;
    case AUTO_SORT:     if(now-autoTimer>=500){
                          bool s=!colorSortEnabled||(detectedColor==COLOR_RED&&sortRed)||
                                 (detectedColor==COLOR_GREEN&&sortGreen)||(detectedColor==COLOR_BLUE&&sortBlue);
                          servoBase.write(s?150:30);autoStep=AUTO_RETURN;autoTimer=now;} break;
    case AUTO_RETURN:   if(now-autoTimer>=800){servoGrip1.write(90);servoGrip2.write(90);
                          if(now-autoTimer>=1200){servoBase.write(90);servoLink1.write(90);servoLink2.write(90);autoStep=AUTO_IDLE;}} break;
  }
}

// ─────────────────────────────────────────────
//  MANUAL CONTROL
// ─────────────────────────────────────────────
static int manBase=90,manLink1=90,manLink2=90,manLink3=90,manGrip1=90,manGrip2=90;
#define MANUAL_STEP 2
#define MANUAL_INTERVAL 20
unsigned long lastManualMove=0;
void handleManualControl() {
  if (robotMode!=MODE_MANUAL||!robotRunning) return;
  unsigned long now=millis();
  if (now-lastManualMove>=MANUAL_INTERVAL) {
    lastManualMove=now;
    if      (joy.vrx<(JOY_CENTRE-JOY_DEADZONE)){manBase=constrain(manBase-MANUAL_STEP,0,180);servoBase.write(manBase);}
    else if (joy.vrx>(JOY_CENTRE+JOY_DEADZONE)){manBase=constrain(manBase+MANUAL_STEP,0,180);servoBase.write(manBase);}
    if      (joy.vry<(JOY_CENTRE-JOY_DEADZONE)){manLink1=constrain(manLink1+MANUAL_STEP,0,180);servoLink1.write(manLink1);}
    else if (joy.vry>(JOY_CENTRE+JOY_DEADZONE)){manLink1=constrain(manLink1-MANUAL_STEP,0,180);servoLink1.write(manLink1);}
  }
  if (Serial2.available()) {
    String cmd=Serial2.readStringUntil('\n'); cmd.trim();
    if      (cmd.startsWith("B"))  {manBase  =constrain(cmd.substring(1).toInt(),0,180);servoBase.write(manBase);}
    else if (cmd.startsWith("L1")) {manLink1 =constrain(cmd.substring(2).toInt(),0,180);servoLink1.write(manLink1);}
    else if (cmd.startsWith("L2")) {manLink2 =constrain(cmd.substring(2).toInt(),0,180);servoLink2.write(manLink2);}
    else if (cmd.startsWith("L3")) {manLink3 =constrain(cmd.substring(2).toInt(),0,180);servoLink3.write(manLink3);}
    else if (cmd.startsWith("G1")) {manGrip1 =constrain(cmd.substring(2).toInt(),0,180);servoGrip1.write(manGrip1);}
    else if (cmd.startsWith("G2")) {manGrip2 =constrain(cmd.substring(2).toInt(),0,180);servoGrip2.write(manGrip2);}
    else if (cmd=="STOP")          {robotRunning=false;}
  }
}

// ─────────────────────────────────────────────
//  STATUS LEDs
// ─────────────────────────────────────────────
void updateStatusLEDs() {
  uint8_t b=0xFF;
  if (robotRunning) { b&=~(1<<PCF_LED_STATUS0); if(robotMode==MODE_AUTO) b&=~(1<<PCF_LED_STATUS1); }
  if (detectedColor==COLOR_RED)   b&=~(1<<PCF_LED_RED);
  if (detectedColor==COLOR_GREEN) b&=~(1<<PCF_LED_GREEN);
  if (detectedColor==COLOR_BLUE)  b&=~(1<<PCF_LED_BLUE);
  pcf8574Write(b);
}

// ─────────────────────────────────────────────
//  SETUP
// ─────────────────────────────────────────────
void setup() {
  Serial.begin(115200); Serial2.begin(9600);
  Wire.begin(21,22);
  lcd.init(); lcd.backlight(); lcd.clear(); memset(prevLine,0,sizeof(prevLine));

  ESP32PWM::allocateTimer(0); ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2); ESP32PWM::allocateTimer(3);
  auto as=[](Servo&s,int p){s.setPeriodHertz(50);s.attach(p,500,2400);};
  as(servoBase,PIN_SERVO_BASE); as(servoLink1,PIN_SERVO_LINK1);
  as(servoLink2,PIN_SERVO_LINK2); as(servoLink3,PIN_SERVO_LINK3);
  as(servoGrip1,PIN_SERVO_GRIP1); as(servoGrip2,PIN_SERVO_GRIP2);
  servoBase.write(90); servoLink1.write(90); servoLink2.write(90);
  servoLink3.write(90); servoGrip1.write(90); servoGrip2.write(90);

  pinMode(PIN_RELAY_MOTOR,OUTPUT);  digitalWrite(PIN_RELAY_MOTOR,LOW);
  pinMode(PIN_ULTRA_X_TRIG,OUTPUT); pinMode(PIN_ULTRA_X_ECHO,INPUT);
  pinMode(PIN_ULTRA_Z_TRIG,OUTPUT); pinMode(PIN_ULTRA_Z_ECHO,INPUT);
  pinMode(PIN_COLOR_S0,OUTPUT); pinMode(PIN_COLOR_S1,OUTPUT);
  pinMode(PIN_COLOR_S2,OUTPUT); pinMode(PIN_COLOR_OUT,INPUT);
  digitalWrite(PIN_COLOR_S0,HIGH); digitalWrite(PIN_COLOR_S1,LOW);
  pinMode(PIN_JOY_SW,INPUT_PULLUP);
  analogSetPinAttenuation(PIN_JOY_VRX,ADC_11db);
  analogSetPinAttenuation(PIN_JOY_VRY,ADC_11db);
  pcf8574Write(0xFF);
  lcdDirty=true;
  Serial.println("[ROBOTIZER] Ready.");
}

// ─────────────────────────────────────────────
//  MAIN LOOP
// ─────────────────────────────────────────────
void loop() {
  readJoystick();
  handleUI();
  handleWarmup();
  handleSensors();
  handleAutoControl();
  handleManualControl();
  updateStatusLEDs();
}