#include 
#include 

// =====================================================
// PROTOCOLO NEXTION / ESP32
// =====================================================
// Nextion -> ESP32
//   \n
//      motorPct: 0..100
//      dir: 0 = CRUSH, 1 = REVERSE
//      irisPct: 0..100
//
//   \n   -> REMOVE CUP (pulso/evento)
//   \n   -> RETURN CUP (pulso/evento)
//
// ESP32 -> Nextion
//   SOLO comandos válidos de Nextion ASCII + 0xFF 0xFF 0xFF
//   Ejemplo mínimo:
//      vaDoor.val=0   -> puerta cerrada
//      vaDoor.val=1   -> puerta abierta

// =====================================================
// PINES
// =====================================================

// --- Motor DC con IBT-2 ---
const uint8_t PIN_RPWM = 13;
const uint8_t PIN_LPWM = 14;

// --- Sensor de puerta ---
const uint8_t PIN_PUERTA = 27;
const bool PUERTA_CERRADA_NIVEL = LOW;

// --- Servo compuerta / iris ---
const uint8_t PIN_SERVO = 26;

// --- Stepper 1 (DRV8825) ---
// Eje Z (recorrido corto, final de carrera 1)
const uint8_t M1_STEP_PIN  = 18;
const uint8_t M1_DIR_PIN   = 19;
const uint8_t M1_LIMIT_PIN = 32;

// --- Stepper 2 (DRV8825) ---
// Eje Y (recorrido largo, final de carrera 2)
const uint8_t M2_STEP_PIN  = 21;
const uint8_t M2_DIR_PIN   = 22;
const uint8_t M2_LIMIT_PIN = 33;

// --- Nextion UART ---
const uint8_t NEXTION_RX_PIN = 23;   // ESP32 RX <- TX Nextion
const uint8_t NEXTION_TX_PIN = 25;   // ESP32 TX -> RX Nextion
const uint32_t NEXTION_BAUD  = 9600;

// UART2 para Nextion
HardwareSerial NextionSerial(2);

// =====================================================
// CALIBRACION FINAL
// =====================================================
bool SERVO_AUMENTAR_ANGULO_ABRE = false;
int  SERVO_ANGULO_CERRADO = 180;
int  SERVO_ANGULO_ABIERTO = 0;

const bool LIMIT1_ACTIVE_LEVEL = LOW;
const bool LIMIT2_ACTIVE_LEVEL = LOW;

int8_t M1_HOME_DIR = 1;
int8_t M2_HOME_DIR = 1;

// =====================================================
// MICROSTEPPING 1/16
// =====================================================
const long MICROSTEP_FACTOR = 16;

// Valores calibrados originalmente en paso completo
const long M1_RANGE_STEPS_FULL_BASE         = 700;
const long M2_RANGE_STEPS_FULL_BASE         = 3600;
const long M2_RETURN_CUP_Y_STEPS_FULL_BASE  = 300;

// Escalados
long M1_RANGE_STEPS = M1_RANGE_STEPS_FULL_BASE * MICROSTEP_FACTOR; // 11200
long M2_RANGE_STEPS = M2_RANGE_STEPS_FULL_BASE * MICROSTEP_FACTOR; // 57600
long M2_RETURN_CUP_Y_STEPS = M2_RETURN_CUP_Y_STEPS_FULL_BASE * MICROSTEP_FACTOR; // 5760

// =====================================================
// CONFIG PWM MOTOR DC
// =====================================================
const uint32_t PWM_FREQ_MOTOR = 20000;
const uint8_t  PWM_RES_MOTOR  = 8;
const uint16_t PWM_MAX_MOTOR  = (1 << PWM_RES_MOTOR) - 1;

// =====================================================
// CONFIG PWM SERVO
// =====================================================
const uint32_t PWM_FREQ_SERVO = 50;
const uint8_t  PWM_RES_SERVO  = 16;
const uint32_t PWM_MAX_SERVO  = (1UL << PWM_RES_SERVO) - 1;
const uint32_t SERVO_PERIODO_US = 20000;

int SERVO_MIN_US = 500;
int SERVO_MAX_US = 2500;

// Servo un poco más rápido que antes, pero aún suave
unsigned long INTERVALO_SERVO_MS = 25;
int PASO_SERVO = 1;

// =====================================================
// MOTOR DC / PUERTA
// =====================================================
unsigned long TIEMPO_CAMBIO_SENTIDO_MS = 1000;
unsigned long INTERVALO_RAMPA_MS       = 20;
uint8_t       PASO_RAMPA               = 2;

unsigned long DEBOUNCE_PUERTA_MS       = 60;
unsigned long INTERVALO_REPORTE_MS     = 1000;

// =====================================================
// CONFIG STEPPERS CON ACCELSTEPPER
// MÁS LENTO PARA EVITAR PÉRDIDA DE PASOS
// =====================================================
const float M1_MAX_SPEED = 400.0f * MICROSTEP_FACTOR;   // 6400
const float M2_MAX_SPEED = 400.0f * MICROSTEP_FACTOR;   // 6400

const float M1_ACCEL = 700.0f * MICROSTEP_FACTOR;       // 11200
const float M2_ACCEL = 700.0f * MICROSTEP_FACTOR;       // 11200

// Homing normal
const float M1_HOME_FAST_SPEED = 120.0f * MICROSTEP_FACTOR;  // 1920
const float M2_HOME_FAST_SPEED = 120.0f * MICROSTEP_FACTOR;  // 1920
const float M1_HOME_SLOW_SPEED = 40.0f  * MICROSTEP_FACTOR;  // 640
const float M2_HOME_SLOW_SPEED = 40.0f  * MICROSTEP_FACTOR;  // 640
const float M1_HOME_BACKOFF_SPEED = 80.0f * MICROSTEP_FACTOR; // 1280
const float M2_HOME_BACKOFF_SPEED = 80.0f * MICROSTEP_FACTOR; // 1280

// Homing de arranque aún más silencioso
const float STARTUP_HOME_FAST_SPEED   = 90.0f  * MICROSTEP_FACTOR; // 1440
const float STARTUP_HOME_SLOW_SPEED   = 30.0f  * MICROSTEP_FACTOR; // 480
const float STARTUP_HOME_BACKOFF_SPEED = 60.0f * MICROSTEP_FACTOR; // 960

const long HOME_SEARCH_STEPS = 200000L;
const long HOME_RELEASE_MAX_STEPS = 5000L;

// Si inviertes esto, recalibra homeDir
bool M1_INVERT_DIR = false;
bool M2_INVERT_DIR = false;

// =====================================================
// NEXTION RX BUFFER
// =====================================================
char nextionRxBuf[80];
uint8_t nextionRxIdx = 0;

// =====================================================
// SERIAL USB BUFFER
// =====================================================
char bufferRx[40];
uint8_t idxRx = 0;

// =====================================================
// SOLICITUDES Y ESTADOS APLICADOS (HMI / CONTROL)
// =====================================================
uint8_t requestedMotorPct = 0;
uint8_t requestedDir      = 0;   // 0=CRUSH, 1=REVERSE
uint8_t requestedIrisPct  = 0;

uint8_t appliedMotorPct   = 0;
uint8_t appliedDir        = 0;
uint8_t appliedIrisPct    = 0;

bool cupRemovePulse = false;
bool cupReturnPulse = false;

uint8_t lastIssuedMotorPct = 255;
uint8_t lastIssuedDir      = 255;
uint8_t lastIssuedIrisPct  = 255;

// feedback Nextion
int lastDoorFeedbackSent = -1;
unsigned long tNextionFeedback = 0;
unsigned long INTERVALO_NEXTION_FEEDBACK_MS = 250;

// =====================================================
// ESTADOS MOTOR DC
// =====================================================
enum Direccion {
  DIR_STOP = 0,
  DIR_D,
  DIR_I
};

Direccion dirActual = DIR_STOP;
uint8_t velocidadActual = 0;
uint8_t velocidadObjetivo = 0;

Direccion dirPendiente = DIR_STOP;
uint8_t velocidadPendiente = 0;
bool inversionEnProceso = false;
bool esperandoPausaInversion = false;

Direccion dirUsuario = DIR_STOP;
uint8_t velocidadUsuario = 0;

unsigned long tRampa = 0;
unsigned long tEsperaInversion = 0;

// =====================================================
// ESTADO PUERTA
// =====================================================
bool puertaCerradaRaw = false;
bool puertaCerradaRawAnterior = false;
bool puertaCerradaEstable = false;

unsigned long tCambioPuerta = 0;
unsigned long tReporte = 0;

// =====================================================
// ESTADO SERVO
// =====================================================
int servoAnguloActual = 0;
int servoAnguloObjetivo = 0;
unsigned long tServo = 0;

// =====================================================
// STEPPERS
// =====================================================
AccelStepper stepper1(AccelStepper::DRIVER, M1_STEP_PIN, M1_DIR_PIN);
AccelStepper stepper2(AccelStepper::DRIVER, M2_STEP_PIN, M2_DIR_PIN);

enum HomePhase {
  HOME_IDLE = 0,
  HOME_RELEASE,
  HOME_SEEK_FAST,
  HOME_BACKOFF,
  HOME_SEEK_SLOW
};

struct StepperAxis {
  AccelStepper* stepper;

  uint8_t stepPin;
  uint8_t dirPin;
  uint8_t limitPin;
  bool limitActiveLevel;

  bool invertDir;
  int8_t homeDir;     // +1 significa que DIR lógico positivo va hacia home
  long rangeSteps;    // 0..range en coordenada máquina

  bool jogMode = false;
  bool homed = false;

  long targetMachinePos = 0;

  float maxSpeed = 1000.0f;
  float accel = 1000.0f;

  // homing silencioso
  HomePhase homePhase = HOME_IDLE;
  float homeFastSpeed = 1000.0f;
  float homeSlowSpeed = 300.0f;
  float homeBackoffSpeed = 600.0f;
  long homePhaseStartPos = 0;
};

StepperAxis axis1;
StepperAxis axis2;

// =====================================================
// SECUENCIAS AUTOMATICAS
// =====================================================
enum AutoSequenceState {
  AUTOSEQ_NONE = 0,
  AUTOSEQ_STARTUP_HOME_Z,
  AUTOSEQ_STARTUP_HOME_Y,
  AUTOSEQ_REMOVE_HOME_Z,
  AUTOSEQ_REMOVE_HOME_Y,
  AUTOSEQ_REMOVE_MOVE_Y_MAX,
  AUTOSEQ_RETURN_HOME_Z,
  AUTOSEQ_RETURN_MOVE_Y_NEAR_HOME,
  AUTOSEQ_RETURN_MOVE_Z_MAX
};

AutoSequenceState autoSeqState = AUTOSEQ_STARTUP_HOME_Z;
bool autoSeqStepStarted = false;
bool startupHomingDone = false;

// =====================================================
// UTILS
// =====================================================
uint8_t minU8(uint8_t a, uint8_t b) {
  return (a < b) ? a : b;
}

int clampInt(int x, int lo, int hi) {
  if (x < lo) return lo;
  if (x > hi) return hi;
  return x;
}

long clampLong(long x, long lo, long hi) {
  if (x < lo) return lo;
  if (x > hi) return hi;
  return x;
}

const char* textoDireccion(Direccion d) {
  switch (d) {
    case DIR_D:    return "D";
    case DIR_I:    return "I";
    case DIR_STOP: return "STOP";
    default:       return "?";
  }
}

bool esSentidoValido(Direccion d) {
  return (d == DIR_D || d == DIR_I);
}

void toUpperInPlace(char* s) {
  for (size_t i = 0; s[i] != '\0'; i++) {
    s[i] = (char)toupper((unsigned char)s[i]);
  }
}

long mapLongClamped(long x, long in_min, long in_max, long out_min, long out_max) {
  if (in_max == in_min) return out_min;

  if (in_min < in_max) {
    if (x < in_min) x = in_min;
    if (x > in_max) x = in_max;
  } else {
    if (x > in_min) x = in_min;
    if (x < in_max) x = in_max;
  }

  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

bool autoSequenceActive() {
  return autoSeqState != AUTOSEQ_NONE;
}

// =====================================================
// NEXTION HELPERS
// =====================================================
void sendNextionCommand(const String &cmd) {
  NextionSerial.print(cmd);
  NextionSerial.write(0xFF);
  NextionSerial.write(0xFF);
  NextionSerial.write(0xFF);
}

void updateHmiFeedback() {
  unsigned long ahora = millis();
  if (ahora - tNextionFeedback < INTERVALO_NEXTION_FEEDBACK_MS) return;
  tNextionFeedback = ahora;

  int doorVal = puertaCerradaEstable ? 0 : 1;

  if (doorVal != lastDoorFeedbackSent) {
    sendNextionCommand(String("vaDoor.val=") + String(doorVal));
    lastDoorFeedbackSent = doorVal;
  }
}

// =====================================================
// PARSER NEXTION
// =====================================================
void processHmiFrame(const String &msg) {
  if (msg.length() < 3) return;

  if (msg.startsWith("", &motorPct, &dir, &irisPct) == 3) {
      motorPct = clampInt(motorPct, 0, 100);
      dir      = clampInt(dir, 0, 1);
      irisPct  = clampInt(irisPct, 0, 100);

      requestedMotorPct = (uint8_t)motorPct;
      requestedDir      = (uint8_t)dir;
      requestedIrisPct  = (uint8_t)irisPct;

      Serial.printf("[HMI] H motor=%u dir=%u iris=%u\n",
                    requestedMotorPct, requestedDir, requestedIrisPct);
    } else {
      Serial.printf("[HMI] Frame H invalido: %s\n", msg.c_str());
    }
    return;
  }

  if (msg.startsWith("", &evt) == 1) {
      if (evt == 1) {
        cupRemovePulse = true;
        Serial.println("[HMI] Evento REMOVE CUP");
      } else if (evt == 2) {
        cupReturnPulse = true;
        Serial.println("[HMI] Evento RETURN CUP");
      } else {
        Serial.printf("[HMI] Evento C desconocido: %d\n", evt);
      }
    } else {
      Serial.printf("[HMI] Frame C invalido: %s\n", msg.c_str());
    }
    return;
  }

  Serial.printf("[HMI] Frame ignorado: %s\n", msg.c_str());
}

void readNextionFrames() {
  while (NextionSerial.available() > 0) {
    char ch = (char)NextionSerial.read();

    if (ch == '\r') continue;

    if (ch == '\n') {
      if (nextionRxIdx > 0) {
        nextionRxBuf[nextionRxIdx] = '\0';
        processHmiFrame(String(nextionRxBuf));
        nextionRxIdx = 0;
      }
    } else {
      if (nextionRxIdx < sizeof(nextionRxBuf) - 1) {
        nextionRxBuf[nextionRxIdx++] = ch;
      } else {
        nextionRxIdx = 0;
      }
    }
  }
}

// =====================================================
// MOTOR DC
// =====================================================
uint16_t porcentajeADuty(uint8_t porcentaje) {
  if (porcentaje > 100) porcentaje = 100;
  return (uint16_t)((porcentaje * PWM_MAX_MOTOR) / 100);
}

void escribirPuente(Direccion dir, uint8_t velPercent) {
  uint16_t duty = porcentajeADuty(velPercent);

  switch (dir) {
    case DIR_D:
      ledcWrite(PIN_RPWM, duty);
      ledcWrite(PIN_LPWM, 0);
      break;

    case DIR_I:
      ledcWrite(PIN_RPWM, 0);
      ledcWrite(PIN_LPWM, duty);
      break;

    case DIR_STOP:
    default:
      ledcWrite(PIN_RPWM, 0);
      ledcWrite(PIN_LPWM, 0);
      break;
  }
}

void apagarMotorPorSeguridad() {
  inversionEnProceso = false;
  esperandoPausaInversion = false;

  dirPendiente = DIR_STOP;
  velocidadPendiente = 0;

  dirActual = DIR_STOP;
  velocidadActual = 0;
  velocidadObjetivo = 0;

  escribirPuente(DIR_STOP, 0);
}

void detenerMotorNormal() {
  inversionEnProceso = false;
  esperandoPausaInversion = false;

  dirPendiente = DIR_STOP;
  velocidadPendiente = 0;

  velocidadObjetivo = 0;
}

void iniciarInversion(Direccion nuevaDir, uint8_t nuevaVel) {
  dirPendiente = nuevaDir;
  velocidadPendiente = nuevaVel;

  inversionEnProceso = true;
  esperandoPausaInversion = false;
  velocidadObjetivo = 0;

  Serial.printf("Inversion solicitada. Frenando a 0 y esperando %lu ms.\n",
                TIEMPO_CAMBIO_SENTIDO_MS);
}

void procesarSolicitudMotor(Direccion nuevaDir, uint8_t nuevaVel) {
  if (nuevaVel > 100) {
    Serial.println("Error: velocidad fuera de rango (0-100)");
    return;
  }

  if (nuevaVel == 0) {
    dirUsuario = DIR_STOP;
    velocidadUsuario = 0;
  } else {
    dirUsuario = nuevaDir;
    velocidadUsuario = nuevaVel;
  }

  if (!puertaCerradaEstable) {
    apagarMotorPorSeguridad();

    if (nuevaVel == 0) {
      Serial.println("Puerta abierta. Motor detenido y configuracion en STOP.");
    } else {
      Serial.printf("Puerta abierta. Configuracion guardada para reanudar luego: %s%u\n",
                    textoDireccion(dirUsuario), velocidadUsuario);
    }
    return;
  }

  if (nuevaVel == 0) {
    Serial.println("Parada solicitada.");
    detenerMotorNormal();
    return;
  }

  if (dirActual == DIR_STOP && velocidadActual == 0 && !inversionEnProceso) {
    dirActual = nuevaDir;
    velocidadObjetivo = nuevaVel;
    Serial.printf("Arranque: %s%u\n", textoDireccion(nuevaDir), nuevaVel);
    return;
  }

  if (!inversionEnProceso && dirActual == nuevaDir) {
    velocidadObjetivo = nuevaVel;
    Serial.printf("Nueva velocidad: %s%u\n", textoDireccion(nuevaDir), nuevaVel);
    return;
  }

  if (!inversionEnProceso && esSentidoValido(dirActual) && nuevaDir != dirActual) {
    iniciarInversion(nuevaDir, nuevaVel);
    return;
  }

  if (inversionEnProceso) {
    dirPendiente = nuevaDir;
    velocidadPendiente = nuevaVel;
    Serial.printf("Destino pendiente actualizado: %s%u\n",
                  textoDireccion(nuevaDir), nuevaVel);
  }
}

void actualizarRampaMotor() {
  if (!puertaCerradaEstable) return;

  unsigned long ahora = millis();
  if (ahora - tRampa < INTERVALO_RAMPA_MS) return;
  tRampa = ahora;

  if (inversionEnProceso && !esperandoPausaInversion) {
    if (velocidadActual > velocidadObjetivo) {
      uint8_t diferencia = (uint8_t)(velocidadActual - velocidadObjetivo);
      uint8_t delta = minU8(PASO_RAMPA, diferencia);
      velocidadActual -= delta;
      escribirPuente(dirActual, velocidadActual);
    }

    if (velocidadActual == 0) {
      escribirPuente(DIR_STOP, 0);
      dirActual = DIR_STOP;
      esperandoPausaInversion = true;
      tEsperaInversion = millis();
      Serial.println("Motor en 0. Iniciando pausa de inversion...");
    }
    return;
  }

  if (inversionEnProceso && esperandoPausaInversion) {
    if (millis() - tEsperaInversion >= TIEMPO_CAMBIO_SENTIDO_MS) {
      dirActual = dirPendiente;
      velocidadObjetivo = velocidadPendiente;

      inversionEnProceso = false;
      esperandoPausaInversion = false;

      Serial.printf("Inversion completada. Nuevo objetivo: %s%u\n",
                    textoDireccion(dirActual), velocidadObjetivo);
    }
    return;
  }

  if (velocidadActual < velocidadObjetivo) {
    uint8_t faltante = (uint8_t)(velocidadObjetivo - velocidadActual);
    uint8_t delta = minU8(PASO_RAMPA, faltante);
    velocidadActual += delta;
    escribirPuente(dirActual, velocidadActual);
  }
  else if (velocidadActual > velocidadObjetivo) {
    uint8_t diferencia = (uint8_t)(velocidadActual - velocidadObjetivo);
    uint8_t delta = minU8(PASO_RAMPA, diferencia);
    velocidadActual -= delta;

    if (velocidadActual == 0 && velocidadObjetivo == 0) {
      escribirPuente(DIR_STOP, 0);
      dirActual = DIR_STOP;
    } else {
      escribirPuente(dirActual, velocidadActual);
    }
  }

  appliedMotorPct = velocidadActual;
  appliedDir = (dirActual == DIR_I) ? 1 : 0;
}

// =====================================================
// SERVO / IRIS
// =====================================================
uint32_t servoPulseUsToDuty(uint32_t pulseUs) {
  return (pulseUs * PWM_MAX_SERVO) / SERVO_PERIODO_US;
}

void escribirServoAngulo(int angulo) {
  angulo = clampInt(angulo, 0, 180);
  long pulseUs = mapLongClamped(angulo, 0, 180, SERVO_MIN_US, SERVO_MAX_US);
  uint32_t duty = servoPulseUsToDuty((uint32_t)pulseUs);
  ledcWrite(PIN_SERVO, duty);
}

void moverServoA(int angulo) {
  servoAnguloObjetivo = clampInt(angulo, 0, 180);
  Serial.printf("Servo objetivo -> %d grados\n", servoAnguloObjetivo);
}

int irisPctToServoAngle(uint8_t pct) {
  return (int)mapLongClamped((long)pct, 0, 100, SERVO_ANGULO_CERRADO, SERVO_ANGULO_ABIERTO);
}

uint8_t servoAngleToIrisPct(int angle) {
  return (uint8_t)clampLong(
    mapLongClamped((long)angle, SERVO_ANGULO_CERRADO, SERVO_ANGULO_ABIERTO, 0, 100),
    0, 100
  );
}

void abrirCompuerta() {
  requestedIrisPct = 100;
  moverServoA(SERVO_ANGULO_ABIERTO);
  Serial.printf("Compuerta ABRIR -> %d grados\n", SERVO_ANGULO_ABIERTO);
}

void cerrarCompuerta() {
  requestedIrisPct = 0;
  moverServoA(SERVO_ANGULO_CERRADO);
  Serial.printf("Compuerta CERRAR -> %d grados\n", SERVO_ANGULO_CERRADO);
}

void actualizarServo() {
  unsigned long ahora = millis();
  if (ahora - tServo < INTERVALO_SERVO_MS) return;
  tServo = ahora;

  if (servoAnguloActual < servoAnguloObjetivo) {
    servoAnguloActual += PASO_SERVO;
    if (servoAnguloActual > servoAnguloObjetivo) servoAnguloActual = servoAnguloObjetivo;
    escribirServoAngulo(servoAnguloActual);
  }
  else if (servoAnguloActual > servoAnguloObjetivo) {
    servoAnguloActual -= PASO_SERVO;
    if (servoAnguloActual < servoAnguloObjetivo) servoAnguloActual = servoAnguloObjetivo;
    escribirServoAngulo(servoAnguloActual);
  }

  appliedIrisPct = servoAngleToIrisPct(servoAnguloActual);
}

// =====================================================
// PUERTA
// =====================================================
bool leerPuertaCrudaCerrada() {
  return (digitalRead(PIN_PUERTA) == PUERTA_CERRADA_NIVEL);
}

void reanudarUltimaConfiguracion() {
  if (!puertaCerradaEstable) return;

  if (!esSentidoValido(dirUsuario) || velocidadUsuario == 0) {
    Serial.println("Puerta cerrada. No hay configuracion previa para reanudar.");
    return;
  }

  dirActual = dirUsuario;
  velocidadObjetivo = velocidadUsuario;

  Serial.printf("Puerta cerrada. Reanudando ultima configuracion: %s%u\n",
                textoDireccion(dirUsuario), velocidadUsuario);
}

void manejarEventoPuertaAbierta() {
  Serial.println("EVENTO: PUERTA ABIERTA -> APAGADO FORZADO");
  apagarMotorPorSeguridad();
}

void manejarEventoPuertaCerrada() {
  Serial.println("EVENTO: PUERTA CERRADA");
  reanudarUltimaConfiguracion();
}

void actualizarPuerta() {
  bool lecturaActual = leerPuertaCrudaCerrada();

  if (lecturaActual != puertaCerradaRawAnterior) {
    puertaCerradaRawAnterior = lecturaActual;
    tCambioPuerta = millis();
  }

  if (millis() - tCambioPuerta >= DEBOUNCE_PUERTA_MS) {
    puertaCerradaRaw = lecturaActual;

    if (puertaCerradaEstable != puertaCerradaRaw) {
      puertaCerradaEstable = puertaCerradaRaw;

      if (puertaCerradaEstable) {
        manejarEventoPuertaCerrada();
      } else {
        manejarEventoPuertaAbierta();
      }
    }
  }
}

// =====================================================
// APLICACION DE SOLICITUDES HMI
// =====================================================
void applyCrusherMotor() {
  Direccion reqDir = (requestedDir == 0) ? DIR_D : DIR_I;
  procesarSolicitudMotor(reqDir, requestedMotorPct);
}

void applyIris() {
  int targetAngle = irisPctToServoAngle(requestedIrisPct);
  moverServoA(targetAngle);
}

void applyRequestedStateIfChanged() {
  if (requestedMotorPct != lastIssuedMotorPct || requestedDir != lastIssuedDir) {
    applyCrusherMotor();
    lastIssuedMotorPct = requestedMotorPct;
    lastIssuedDir = requestedDir;
  }

  if (requestedIrisPct != lastIssuedIrisPct) {
    applyIris();
    lastIssuedIrisPct = requestedIrisPct;
  }
}

// =====================================================
// STEPPERS CON ACCELSTEPPER
// =====================================================
bool axisLimitActive(StepperAxis &ax) {
  return digitalRead(ax.limitPin) == ax.limitActiveLevel;
}

// Convierte posición máquina (0=home, positivo alejándose)
// a posición lógica de AccelStepper.
long axisLogicalFromMachine(const StepperAxis &ax, long machinePos) {
  return (ax.homeDir == 1) ? -machinePos : machinePos;
}

// Convierte posición lógica de AccelStepper a posición máquina.
long axisMachineFromLogical(const StepperAxis &ax, long logicalPos) {
  return (ax.homeDir == 1) ? -logicalPos : logicalPos;
}

long axisMachinePosition(const StepperAxis &ax) {
  return axisMachineFromLogical(ax, ax.stepper->currentPosition());
}

bool axisIsBusy(const StepperAxis &ax) {
  return (ax.homePhase != HOME_IDLE) || ax.jogMode || (ax.stepper->distanceToGo() != 0);
}

float axisLogicalSpeedTowardHome(const StepperAxis &ax, float speedAbs) {
  return (ax.homeDir == 1) ? +fabs(speedAbs) : -fabs(speedAbs);
}

float axisLogicalSpeedAwayFromHome(const StepperAxis &ax, float speedAbs) {
  return -axisLogicalSpeedTowardHome(ax, speedAbs);
}

void axisConfigureMove(StepperAxis &ax, float maxSpeed, float accel) {
  ax.stepper->setMaxSpeed(maxSpeed);
  ax.stepper->setAcceleration(accel);
}

void axisStopImmediate(StepperAxis &ax) {
  long pos = ax.stepper->currentPosition();
  ax.stepper->moveTo(pos);
  ax.homePhase = HOME_IDLE;
  ax.jogMode = false;
  ax.targetMachinePos = axisMachinePosition(ax);
}

void axisSetAtHome(StepperAxis &ax) {
  ax.stepper->setCurrentPosition(0);
  ax.stepper->moveTo(0);
  ax.homePhase = HOME_IDLE;
  ax.jogMode = false;
  ax.homed = true;
  ax.targetMachinePos = 0;
}

void axisStartQuietHome(StepperAxis &ax, const char* nombre,
                        float fastSpeed, float slowSpeed, float backoffSpeed) {
  ax.homeFastSpeed = fastSpeed;
  ax.homeSlowSpeed = slowSpeed;
  ax.homeBackoffSpeed = backoffSpeed;
  ax.jogMode = false;
  ax.homed = false;

  // durante homing usamos runSpeed(), no movimiento acelerado
  ax.stepper->moveTo(ax.stepper->currentPosition());

  if (axisLimitActive(ax)) {
    ax.homePhase = HOME_RELEASE;
    ax.stepper->setSpeed(axisLogicalSpeedAwayFromHome(ax, ax.homeBackoffSpeed));
    ax.homePhaseStartPos = ax.stepper->currentPosition();
    Serial.printf("%s -> HOME silencioso: liberando switch...\n", nombre);
  } else {
    ax.homePhase = HOME_SEEK_FAST;
    ax.stepper->setSpeed(axisLogicalSpeedTowardHome(ax, ax.homeFastSpeed));
    ax.homePhaseStartPos = ax.stepper->currentPosition();
    Serial.printf("%s -> HOME silencioso: búsqueda rápida...\n", nombre);
  }
}

void axisStartHome(StepperAxis &ax, const char* nombre) {
  axisStartQuietHome(ax, nombre, ax.homeFastSpeed, ax.homeSlowSpeed, ax.homeBackoffSpeed);
}

void axisStartAbsolute(StepperAxis &ax, const char* nombre, long machineTarget) {
  if (!ax.homed) {
    Serial.printf("%s -> primero haz HOME\n", nombre);
    return;
  }

  machineTarget = clampLong(machineTarget, 0, ax.rangeSteps);
  long logicalTarget = axisLogicalFromMachine(ax, machineTarget);

  axisConfigureMove(ax, ax.maxSpeed, ax.accel);
  ax.stepper->moveTo(logicalTarget);
  ax.targetMachinePos = machineTarget;
  ax.homePhase = HOME_IDLE;
  ax.jogMode = false;

  Serial.printf("%s -> movimiento absoluto a %ld\n", nombre, machineTarget);
}

void axisStartRelative(StepperAxis &ax, const char* nombre, long delta) {
  if (!ax.homed) {
    Serial.printf("%s -> primero haz HOME\n", nombre);
    return;
  }

  long currentMachine = axisMachinePosition(ax);
  long targetMachine = clampLong(currentMachine + delta, 0, ax.rangeSteps);

  if (targetMachine == currentMachine) {
    Serial.printf("%s -> ya esta en el limite permitido\n", nombre);
    return;
  }

  axisStartAbsolute(ax, nombre, targetMachine);
}

void axisStartJog(StepperAxis &ax, const char* nombre, int8_t coordDir) {
  if (!ax.homed) {
    Serial.printf("%s -> primero haz HOME\n", nombre);
    return;
  }

  axisConfigureMove(ax, ax.maxSpeed, ax.accel);

  long farTarget = (coordDir > 0) ? ax.rangeSteps : 0;
  long logicalTarget = axisLogicalFromMachine(ax, farTarget);

  ax.stepper->moveTo(logicalTarget);
  ax.targetMachinePos = farTarget;
  ax.homePhase = HOME_IDLE;
  ax.jogMode = true;

  Serial.printf("%s -> JOG %s\n", nombre, (coordDir > 0 ? "+" : "-"));
}

void axisUpdateHomeQuiet(StepperAxis &ax, const char* nombre) {
  switch (ax.homePhase) {
    case HOME_IDLE:
      return;

    case HOME_RELEASE:
      ax.stepper->runSpeed();
      if (!axisLimitActive(ax)) {
        ax.homePhase = HOME_SEEK_FAST;
        ax.stepper->setSpeed(axisLogicalSpeedTowardHome(ax, ax.homeFastSpeed));
        ax.homePhaseStartPos = ax.stepper->currentPosition();
        Serial.printf("%s -> switch liberado, búsqueda rápida.\n", nombre);
      } else {
        long delta = labs(ax.stepper->currentPosition() - ax.homePhaseStartPos);
        if (delta > HOME_RELEASE_MAX_STEPS) {
          ax.homePhase = HOME_IDLE;
          Serial.printf("%s -> ERROR liberando switch.\n", nombre);
        }
      }
      return;

    case HOME_SEEK_FAST:
      ax.stepper->runSpeed();
      if (axisLimitActive(ax)) {
        ax.homePhase = HOME_BACKOFF;
        ax.stepper->setSpeed(axisLogicalSpeedAwayFromHome(ax, ax.homeBackoffSpeed));
        ax.homePhaseStartPos = ax.stepper->currentPosition();
        Serial.printf("%s -> switch tocado, retroceso.\n", nombre);
      } else {
        long delta = labs(ax.stepper->currentPosition() - ax.homePhaseStartPos);
        if (delta > HOME_SEARCH_STEPS) {
          ax.homePhase = HOME_IDLE;
          Serial.printf("%s -> ERROR no encontró home.\n", nombre);
        }
      }
      return;

    case HOME_BACKOFF:
      ax.stepper->runSpeed();
      if (!axisLimitActive(ax)) {
        ax.homePhase = HOME_SEEK_SLOW;
        ax.stepper->setSpeed(axisLogicalSpeedTowardHome(ax, ax.homeSlowSpeed));
        ax.homePhaseStartPos = ax.stepper->currentPosition();
        Serial.printf("%s -> búsqueda lenta final.\n", nombre);
      } else {
        long delta = labs(ax.stepper->currentPosition() - ax.homePhaseStartPos);
        if (delta > HOME_RELEASE_MAX_STEPS) {
          ax.homePhase = HOME_IDLE;
          Serial.printf("%s -> ERROR en backoff.\n", nombre);
        }
      }
      return;

    case HOME_SEEK_SLOW:
      ax.stepper->runSpeed();
      if (axisLimitActive(ax)) {
        axisSetAtHome(ax);
        Serial.printf("%s -> HOME completo, posición = 0\n", nombre);
      } else {
        long delta = labs(ax.stepper->currentPosition() - ax.homePhaseStartPos);
        if (delta > HOME_RELEASE_MAX_STEPS) {
          ax.homePhase = HOME_IDLE;
          Serial.printf("%s -> ERROR en búsqueda lenta.\n", nombre);
        }
      }
      return;
  }
}

void axisUpdate(StepperAxis &ax, const char* nombre) {
  if (ax.homePhase != HOME_IDLE) {
    axisUpdateHomeQuiet(ax, nombre);
    return;
  }

  long currentMachine = axisMachinePosition(ax);

  if (currentMachine < 0) {
    ax.stepper->setCurrentPosition(axisLogicalFromMachine(ax, 0));
    ax.stepper->moveTo(axisLogicalFromMachine(ax, 0));
    currentMachine = 0;
  }

  if (currentMachine > ax.rangeSteps) {
    ax.stepper->setCurrentPosition(axisLogicalFromMachine(ax, ax.rangeSteps));
    ax.stepper->moveTo(axisLogicalFromMachine(ax, ax.rangeSteps));
    currentMachine = ax.rangeSteps;
  }

  ax.stepper->run();

  if (ax.jogMode) {
    long p = axisMachinePosition(ax);

    if (p <= 0 && ax.targetMachinePos == 0) {
      ax.stepper->setCurrentPosition(axisLogicalFromMachine(ax, 0));
      ax.stepper->moveTo(axisLogicalFromMachine(ax, 0));
      ax.jogMode = false;
      Serial.printf("%s -> jog detenido en home\n", nombre);
      return;
    }

    if (p >= ax.rangeSteps && ax.targetMachinePos == ax.rangeSteps) {
      ax.stepper->setCurrentPosition(axisLogicalFromMachine(ax, ax.rangeSteps));
      ax.stepper->moveTo(axisLogicalFromMachine(ax, ax.rangeSteps));
      ax.jogMode = false;
      Serial.printf("%s -> jog detenido en maximo\n", nombre);
      return;
    }
  }
}

// =====================================================
// SECUENCIAS AUTOMATICAS
// =====================================================
void startRemoveCupSequence() {
  if (autoSequenceActive()) return;
  autoSeqState = AUTOSEQ_REMOVE_HOME_Z;
  autoSeqStepStarted = false;
  Serial.println("[AUTO] Iniciando REMOVE CUP");
}

void startReturnCupSequence() {
  if (autoSequenceActive()) return;
  autoSeqState = AUTOSEQ_RETURN_HOME_Z;
  autoSeqStepStarted = false;
  Serial.println("[AUTO] Iniciando RETURN CUP");
}

void updateAutoSequence() {
  switch (autoSeqState) {
    case AUTOSEQ_NONE:
      break;

    case AUTOSEQ_STARTUP_HOME_Z:
      if (!autoSeqStepStarted) {
        Serial.println("[AUTO] Startup: seteo suave eje Z");
        axisStartQuietHome(axis1, "Z/M1",
                           STARTUP_HOME_FAST_SPEED,
                           STARTUP_HOME_SLOW_SPEED,
                           STARTUP_HOME_BACKOFF_SPEED);
        autoSeqStepStarted = true;
      }
      if (!axisIsBusy(axis1)) {
        if (!axis1.homed) {
          Serial.println("[AUTO] ERROR: home startup Z falló.");
          autoSeqState = AUTOSEQ_NONE;
        } else {
          autoSeqState = AUTOSEQ_STARTUP_HOME_Y;
          autoSeqStepStarted = false;
        }
      }
      break;

    case AUTOSEQ_STARTUP_HOME_Y:
      if (!autoSeqStepStarted) {
        Serial.println("[AUTO] Startup: seteo suave eje Y");
        axisStartQuietHome(axis2, "Y/M2",
                           STARTUP_HOME_FAST_SPEED,
                           STARTUP_HOME_SLOW_SPEED,
                           STARTUP_HOME_BACKOFF_SPEED);
        autoSeqStepStarted = true;
      }
      if (!axisIsBusy(axis2)) {
        if (!axis2.homed) {
          Serial.println("[AUTO] ERROR: home startup Y falló.");
          autoSeqState = AUTOSEQ_NONE;
        } else {
          startupHomingDone = true;
          autoSeqState = AUTOSEQ_NONE;
          autoSeqStepStarted = false;
          Serial.println("[AUTO] Startup completo. Ambos ejes en origen.");
        }
      }
      break;

    case AUTOSEQ_REMOVE_HOME_Z:
      if (!autoSeqStepStarted) {
        axisStartHome(axis1, "Z/M1");
        autoSeqStepStarted = true;
      }
      if (!axisIsBusy(axis1)) {
        if (!axis1.homed) {
          Serial.println("[AUTO] ERROR: home Z en REMOVE CUP falló.");
          autoSeqState = AUTOSEQ_NONE;
        } else {
          autoSeqState = AUTOSEQ_REMOVE_HOME_Y;
          autoSeqStepStarted = false;
        }
      }
      break;

    case AUTOSEQ_REMOVE_HOME_Y:
      if (!autoSeqStepStarted) {
        axisStartHome(axis2, "Y/M2");
        autoSeqStepStarted = true;
      }
      if (!axisIsBusy(axis2)) {
        if (!axis2.homed) {
          Serial.println("[AUTO] ERROR: home Y en REMOVE CUP falló.");
          autoSeqState = AUTOSEQ_NONE;
        } else {
          autoSeqState = AUTOSEQ_REMOVE_MOVE_Y_MAX;
          autoSeqStepStarted = false;
        }
      }
      break;

    case AUTOSEQ_REMOVE_MOVE_Y_MAX:
      if (!autoSeqStepStarted) {
        axisStartAbsolute(axis2, "Y/M2", axis2.rangeSteps);
        autoSeqStepStarted = true;
      }
      if (!axisIsBusy(axis2)) {
        autoSeqState = AUTOSEQ_NONE;
        autoSeqStepStarted = false;
        Serial.println("[AUTO] REMOVE CUP completado.");
      }
      break;

    case AUTOSEQ_RETURN_HOME_Z:
      if (!autoSeqStepStarted) {
        axisStartHome(axis1, "Z/M1");
        autoSeqStepStarted = true;
      }
      if (!axisIsBusy(axis1)) {
        if (!axis1.homed) {
          Serial.println("[AUTO] ERROR: home Z en RETURN CUP falló.");
          autoSeqState = AUTOSEQ_NONE;
        } else {
          autoSeqState = AUTOSEQ_RETURN_MOVE_Y_NEAR_HOME;
          autoSeqStepStarted = false;
        }
      }
      break;

    case AUTOSEQ_RETURN_MOVE_Y_NEAR_HOME:
      if (!autoSeqStepStarted) {
        axisStartAbsolute(axis2, "Y/M2", M2_RETURN_CUP_Y_STEPS);
        autoSeqStepStarted = true;
      }
      if (!axisIsBusy(axis2)) {
        autoSeqState = AUTOSEQ_RETURN_MOVE_Z_MAX;
        autoSeqStepStarted = false;
      }
      break;

    case AUTOSEQ_RETURN_MOVE_Z_MAX:
      if (!autoSeqStepStarted) {
        // aquí Z se aleja del final y sube al máximo
        axisStartAbsolute(axis1, "Z/M1", axis1.rangeSteps);
        autoSeqStepStarted = true;
      }
      if (!axisIsBusy(axis1)) {
        autoSeqState = AUTOSEQ_NONE;
        autoSeqStepStarted = false;
        Serial.println("[AUTO] RETURN CUP completado.");
      }
      break;
  }
}

// =====================================================
// CUP EVENTS
// =====================================================
void handleCupRemove() {
  if (!cupRemovePulse) return;
  if (!startupHomingDone) return;
  if (autoSequenceActive()) return;

  cupRemovePulse = false;
  startRemoveCupSequence();
}

void handleCupReturn() {
  if (!cupReturnPulse) return;
  if (!startupHomingDone) return;
  if (autoSequenceActive()) return;

  cupReturnPulse = false;
  startReturnCupSequence();
}

// =====================================================
// REPORTE GENERAL
// =====================================================
void reportarEstadoSistema() {
  if (millis() - tReporte < INTERVALO_REPORTE_MS) return;
  tReporte = millis();

  long pos1 = axisMachinePosition(axis1);
  long pos2 = axisMachinePosition(axis2);

  Serial.printf(
    "PUERTA:%s | REQ_MOTOR:%u/%u | APP_MOTOR:%u/%u | REQ_IRIS:%u | APP_IRIS:%u | SERVO:%d/%d | "
    "FC1:%s POS1:%ld/%ld HOMED1:%d BUSY1:%d | "
    "FC2:%s POS2:%ld/%ld HOMED2:%d BUSY2:%d | AUTOSEQ:%d\n",
    puertaCerradaEstable ? "CERRADA" : "ABIERTA",
    requestedMotorPct, requestedDir,
    appliedMotorPct, appliedDir,
    requestedIrisPct, appliedIrisPct,
    servoAnguloActual, servoAnguloObjetivo,
    axisLimitActive(axis1) ? "ACT" : "LIB",
    pos1, axis1.rangeSteps, axis1.homed ? 1 : 0, axisIsBusy(axis1) ? 1 : 0,
    axisLimitActive(axis2) ? "ACT" : "LIB",
    pos2, axis2.rangeSteps, axis2.homed ? 1 : 0, axisIsBusy(axis2) ? 1 : 0,
    (int)autoSeqState
  );
}

// =====================================================
// COMANDOS SERIAL DEBUG
// =====================================================
void imprimirAyuda() {
  Serial.println("===== COMANDOS =====");
  Serial.println("Motor DC:");
  Serial.println("  D100");
  Serial.println("  I50");
  Serial.println("  D0");
  Serial.println("");
  Serial.println("Servo:");
  Serial.println("  ABRIR");
  Serial.println("  CERRAR");
  Serial.println("  ANG90");
  Serial.println("  IRIS60");
  Serial.println("");
  Serial.println("Steppers:");
  Serial.println("  HOME1");
  Serial.println("  HOME2");
  Serial.println("  P1+1000");
  Serial.println("  P1-500");
  Serial.println("  P2+1000");
  Serial.println("  P2-500");
  Serial.println("  J1+ / J1-");
  Serial.println("  J2+ / J2-");
  Serial.println("  STOP1");
  Serial.println("  STOP2");
  Serial.println("");
  Serial.println("Secuencias:");
  Serial.println("  REMOVECUP");
  Serial.println("  RETURNCUP");
  Serial.println("");
  Serial.println("General:");
  Serial.println("  ESTADO");
  Serial.println("  AYUDA");
}

void imprimirEstadoUnaVez() {
  long pos1 = axisMachinePosition(axis1);
  long pos2 = axisMachinePosition(axis2);

  Serial.printf(
    "PUERTA:%s | REQ_MOTOR:%u/%u | APP_MOTOR:%u/%u | REQ_IRIS:%u | APP_IRIS:%u | SERVO:%d/%d | "
    "FC1:%s POS1:%ld/%ld HOMED1:%d BUSY1:%d | "
    "FC2:%s POS2:%ld/%ld HOMED2:%d BUSY2:%d | AUTOSEQ:%d\n",
    puertaCerradaEstable ? "CERRADA" : "ABIERTA",
    requestedMotorPct, requestedDir,
    appliedMotorPct, appliedDir,
    requestedIrisPct, appliedIrisPct,
    servoAnguloActual, servoAnguloObjetivo,
    axisLimitActive(axis1) ? "ACT" : "LIB",
    pos1, axis1.rangeSteps, axis1.homed ? 1 : 0, axisIsBusy(axis1) ? 1 : 0,
    axisLimitActive(axis2) ? "ACT" : "LIB",
    pos2, axis2.rangeSteps, axis2.homed ? 1 : 0, axisIsBusy(axis2) ? 1 : 0,
    (int)autoSeqState
  );
}

void procesarComando(const char* cmdOriginal) {
  if (cmdOriginal == nullptr || cmdOriginal[0] == '\0') return;

  char cmd[40];
  strncpy(cmd, cmdOriginal, sizeof(cmd) - 1);
  cmd[sizeof(cmd) - 1] = '\0';
  toUpperInPlace(cmd);

  if (strcmp(cmd, "ABRIR") == 0) {
    abrirCompuerta();
    return;
  }

  if (strcmp(cmd, "CERRAR") == 0) {
    cerrarCompuerta();
    return;
  }

  if (strcmp(cmd, "REMOVECUP") == 0) {
    cupRemovePulse = true;
    return;
  }

  if (strcmp(cmd, "RETURNCUP") == 0) {
    cupReturnPulse = true;
    return;
  }

  if (strncmp(cmd, "IRIS", 4) == 0) {
    char* endPtr = nullptr;
    long pct = strtol(cmd + 4, &endPtr, 10);

    if (endPtr == cmd + 4 || *endPtr != '\0') {
      Serial.println("Formato invalido. Usa IRIS60");
      return;
    }

    pct = clampLong(pct, 0, 100);
    requestedIrisPct = (uint8_t)pct;
    applyIris();
    lastIssuedIrisPct = requestedIrisPct;
    return;
  }

  if (strncmp(cmd, "ANG", 3) == 0) {
    char* endPtr = nullptr;
    long ang = strtol(cmd + 3, &endPtr, 10);

    if (endPtr == cmd + 3 || *endPtr != '\0') {
      Serial.println("Formato invalido. Usa ANG90");
      return;
    }

    if (ang < 0 || ang > 180) {
      Serial.println("Angulo invalido. 0 a 180.");
      return;
    }

    moverServoA((int)ang);
    return;
  }

  if (autoSequenceActive()) {
    if (strcmp(cmd, "HOME1") == 0 || strcmp(cmd, "HOME2") == 0 ||
        strcmp(cmd, "J1+") == 0 || strcmp(cmd, "J1-") == 0 ||
        strcmp(cmd, "J2+") == 0 || strcmp(cmd, "J2-") == 0 ||
        strcmp(cmd, "STOP1") == 0 || strcmp(cmd, "STOP2") == 0 ||
        strncmp(cmd, "P1", 2) == 0 || strncmp(cmd, "P2", 2) == 0) {
      Serial.println("Secuencia automática activa. Espera a que termine.");
      return;
    }
  }

  if (strcmp(cmd, "HOME1") == 0) {
    axisStartHome(axis1, "M1");
    return;
  }

  if (strcmp(cmd, "HOME2") == 0) {
    axisStartHome(axis2, "M2");
    return;
  }

  if (strcmp(cmd, "J1+") == 0) {
    axisStartJog(axis1, "M1", +1);
    return;
  }

  if (strcmp(cmd, "J1-") == 0) {
    axisStartJog(axis1, "M1", -1);
    return;
  }

  if (strcmp(cmd, "J2+") == 0) {
    axisStartJog(axis2, "M2", +1);
    return;
  }

  if (strcmp(cmd, "J2-") == 0) {
    axisStartJog(axis2, "M2", -1);
    return;
  }

  if (strcmp(cmd, "STOP1") == 0) {
    axisStopImmediate(axis1);
    Serial.println("M1 detenido");
    return;
  }

  if (strcmp(cmd, "STOP2") == 0) {
    axisStopImmediate(axis2);
    Serial.println("M2 detenido");
    return;
  }

  if (strncmp(cmd, "P1", 2) == 0) {
    char* endPtr = nullptr;
    long delta = strtol(cmd + 2, &endPtr, 10);

    if (endPtr == cmd + 2 || *endPtr != '\0') {
      Serial.println("Formato invalido. Usa P1+1000 o P1-500");
      return;
    }

    axisStartRelative(axis1, "M1", delta);
    return;
  }

  if (strncmp(cmd, "P2", 2) == 0) {
    char* endPtr = nullptr;
    long delta = strtol(cmd + 2, &endPtr, 10);

    if (endPtr == cmd + 2 || *endPtr != '\0') {
      Serial.println("Formato invalido. Usa P2+1000 o P2-500");
      return;
    }

    axisStartRelative(axis2, "M2", delta);
    return;
  }

  if (cmd[0] == 'D' || cmd[0] == 'I') {
    char* endPtr = nullptr;
    long valor = strtol(cmd + 1, &endPtr, 10);

    if (endPtr == cmd + 1 || *endPtr != '\0') {
      Serial.println("Formato invalido. Ejemplos: D50, I100, D0");
      return;
    }

    if (valor < 0 || valor > 100) {
      Serial.println("Valor invalido. Debe estar entre 0 y 100");
      return;
    }

    requestedMotorPct = (uint8_t)valor;
    requestedDir      = (cmd[0] == 'D') ? 0 : 1;
    applyCrusherMotor();
    lastIssuedMotorPct = requestedMotorPct;
    lastIssuedDir = requestedDir;
    return;
  }

  if (strcmp(cmd, "ESTADO") == 0) {
    imprimirEstadoUnaVez();
    return;
  }

  if (strcmp(cmd, "AYUDA") == 0) {
    imprimirAyuda();
    return;
  }

  Serial.println("Comando no reconocido. Escribe AYUDA.");
}

void leerSerialNoBloqueante() {
  while (Serial.available() > 0) {
    char ch = Serial.read();

    if (ch == '\n' || ch == '\r') {
      if (idxRx > 0) {
        bufferRx[idxRx] = '\0';
        procesarComando(bufferRx);
        idxRx = 0;
      }
    } else if (ch == ' ' || ch == '\t') {
      // ignorar
    } else {
      if (idxRx < sizeof(bufferRx) - 1) {
        bufferRx[idxRx++] = ch;
      } else {
        idxRx = 0;
        Serial.println("Comando demasiado largo");
      }
    }
  }
}

// =====================================================
// SETUP AXIS
// =====================================================
void setupAxis(
  StepperAxis &ax,
  AccelStepper* stepperObj,
  uint8_t stepPin,
  uint8_t dirPin,
  uint8_t limitPin,
  bool limitActiveLevel,
  bool invertDir,
  int8_t homeDir,
  float runSpeed,
  float runAccel,
  long rangeSteps,
  float homeFast,
  float homeSlow,
  float homeBackoff
) {
  ax.stepper = stepperObj;
  ax.stepPin = stepPin;
  ax.dirPin = dirPin;
  ax.limitPin = limitPin;
  ax.limitActiveLevel = limitActiveLevel;
  ax.invertDir = invertDir;
  ax.homeDir = homeDir;
  ax.maxSpeed = runSpeed;
  ax.accel = runAccel;
  ax.rangeSteps = rangeSteps;
  ax.homeFastSpeed = homeFast;
  ax.homeSlowSpeed = homeSlow;
  ax.homeBackoffSpeed = homeBackoff;

  pinMode(ax.limitPin, INPUT);

  ax.stepper->setPinsInverted(ax.invertDir, false, false);
  ax.stepper->setMaxSpeed(ax.maxSpeed);
  ax.stepper->setAcceleration(ax.accel);
  ax.stepper->setMinPulseWidth(4);
  ax.stepper->setCurrentPosition(0);
  ax.targetMachinePos = 0;
  ax.homePhase = HOME_IDLE;
}

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

  NextionSerial.begin(NEXTION_BAUD, SERIAL_8N1, NEXTION_RX_PIN, NEXTION_TX_PIN);

  bool ok1 = ledcAttach(PIN_RPWM, PWM_FREQ_MOTOR, PWM_RES_MOTOR);
  bool ok2 = ledcAttach(PIN_LPWM, PWM_FREQ_MOTOR, PWM_RES_MOTOR);
  bool ok3 = ledcAttach(PIN_SERVO, PWM_FREQ_SERVO, PWM_RES_SERVO);

  if (!ok1 || !ok2 || !ok3) {
    Serial.println("Error configurando PWM LEDC");
    while (true) {}
  }

  pinMode(PIN_PUERTA, INPUT_PULLUP);

  bool estadoInicialPuerta = leerPuertaCrudaCerrada();
  puertaCerradaRaw = estadoInicialPuerta;
  puertaCerradaRawAnterior = estadoInicialPuerta;
  puertaCerradaEstable = estadoInicialPuerta;

  setupAxis(axis1, &stepper1, M1_STEP_PIN, M1_DIR_PIN, M1_LIMIT_PIN,
            LIMIT1_ACTIVE_LEVEL, M1_INVERT_DIR, M1_HOME_DIR,
            M1_MAX_SPEED, M1_ACCEL, M1_RANGE_STEPS,
            M1_HOME_FAST_SPEED, M1_HOME_SLOW_SPEED, M1_HOME_BACKOFF_SPEED);

  setupAxis(axis2, &stepper2, M2_STEP_PIN, M2_DIR_PIN, M2_LIMIT_PIN,
            LIMIT2_ACTIVE_LEVEL, M2_INVERT_DIR, M2_HOME_DIR,
            M2_MAX_SPEED, M2_ACCEL, M2_RANGE_STEPS,
            M2_HOME_FAST_SPEED, M2_HOME_SLOW_SPEED, M2_HOME_BACKOFF_SPEED);

  tCambioPuerta = millis();
  tReporte = 0;
  tRampa = millis();
  tServo = millis();
  tNextionFeedback = 0;

  servoAnguloActual = clampInt(SERVO_ANGULO_CERRADO, 0, 180);
  servoAnguloObjetivo = servoAnguloActual;
  escribirServoAngulo(servoAnguloActual);

  requestedIrisPct = 0;
  appliedIrisPct = 0;

  apagarMotorPorSeguridad();

  lastDoorFeedbackSent = -1;

  autoSeqState = AUTOSEQ_STARTUP_HOME_Z;
  autoSeqStepStarted = false;
  startupHomingDone = false;

  Serial.println("Sistema listo.");
  Serial.println("UART Nextion lista en GPIO23(RX) y GPIO25(TX)");
  Serial.println("Steppers con AccelStepper.");
  Serial.println("Home silencioso con runSpeed(): liberar -> buscar -> retroceder -> buscar lento.");
  Serial.println("Microstepping 1/16 aplicado a ambos ejes.");
  imprimirAyuda();
  Serial.printf("Estado inicial puerta: %s\n",
                puertaCerradaEstable ? "CERRADA" : "ABIERTA");
  Serial.printf("Servo inicial: %d grados\n", servoAnguloActual);
  Serial.printf("M1_RANGE_STEPS = %ld\n", M1_RANGE_STEPS);
  Serial.printf("M2_RANGE_STEPS = %ld\n", M2_RANGE_STEPS);
  Serial.printf("M2_RETURN_CUP_Y_STEPS = %ld\n", M2_RETURN_CUP_Y_STEPS);
}

void loop() {
  leerSerialNoBloqueante();
  readNextionFrames();
  actualizarPuerta();
  updateAutoSequence();
  applyRequestedStateIfChanged();
  actualizarRampaMotor();
  actualizarServo();
  handleCupRemove();
  handleCupReturn();
  axisUpdate(axis1, "M1");
  axisUpdate(axis2, "M2");
  updateHmiFeedback();
  reportarEstadoSistema();