#include <WiFi.h>
#include <WebServer.h>
#include <WebSocketsServer.h>
#include <Update.h>
#include <time.h>
#include <TMC2209.h>
#include <FlexyStepper.h>
#include <Wire.h>
#include <SparkFun_ADS122C04_ADC_Arduino_Library.h>
#include <math.h>
#include <Preferences.h>

#define VERBO 0

// -------------------- Declaraciones que ya usas --------------------
void externalADC_init();
void setXY();
void trackSunByTime();
void nomegusta(int32_t varerr, int32_t error);
void leetodo();
int32_t current(volatile uint32_t data);

// --- Auto-Tune ---
void at_recommend_thresholds(bool applyNow);
void handleRecomputeAutoTune();
void handleApplyAutoTune();
void handleSetAutoApply();

// -------------------- WiFi --------------------
// Local configuration; see wifi_credentials.example.h.
#include "wifi_credentials.h"

// -------------------- NTP (time.h) --------------------
const char* ntpServer = "pool.ntp.org";
// Aguascalientes típico: UTC-6 sin DST (ajústalo si quieres DST)
const long gmtOffset_sec = -6 * 3600;
const int  daylightOffset_sec = 0;

// -------------------- Web --------------------
WebServer server(80);
WebSocketsServer webSocket = WebSocketsServer(81);

// -------------------- Log --------------------
#define MAX_LOG_SIZE 100
String logBuffer[MAX_LOG_SIZE];
int logIndex = 0;

// -------------------- Motores / ADC (tu base) --------------------
volatile int adcDataAvailable = 0;
volatile uint32_t adch[3];
volatile bool VocIsc = 0;

volatile float SmoothData, SmoothData2, SmoothVolt;
volatile float LPF_Beta = 0.85;

SFE_ADS122C04 mySensor;
HardwareSerial serial_stream = Serial0;

const int MOTOR_X_STEP_PIN = 3;
const int MOTOR_X_DIRECTION_PIN = 2;
const int MOTOR_Y_STEP_PIN = 13;
const int MOTOR_Y_DIRECTION_PIN = 12;

const long SERIAL_BAUD_RATE = 250000;
const uint8_t REPLY_DELAY = 15;

const uint8_t RUN_CURRENT_PERCENT0 = 18;
const int32_t VELOCITY0 = 4200;
const uint8_t STALL_GUARD_THRESHOLD0 = 200;

const uint8_t RUN_CURRENT_PERCENT1 = 25;
const int32_t VELOCITY1 = 4200;
const uint8_t STALL_GUARD_THRESHOLD1 = 200;

FlexyStepper stepperX;
FlexyStepper stepperY;

TMC2209 stepper_driver_0;
const TMC2209::SerialAddress SERIAL_ADDRESS_1 = TMC2209::SERIAL_ADDRESS_1;
TMC2209 stepper_driver_1;
const TMC2209::SerialAddress SERIAL_ADDRESS_3 = TMC2209::SERIAL_ADDRESS_3;

const int32_t LIMITX = -104000;
const int32_t LIMITY = -106000;
const int32_t hist = 100;

volatile int32_t cicleMAX = 3;

volatile int32_t cicles = 0;
volatile int32_t errmax = 1500;
volatile int32_t delayyy = 2;

volatile int32_t min_errmax = 100;
volatile int32_t abrupt_threshold = 9000;
volatile int32_t small_threshold = 3000;
volatile int32_t reset_threshold = 25000;
volatile float   peak_proximity = 0.015;

volatile int32_t posX = 0;
volatile int32_t posY = 0;


// === NVS persistence for posX/posY ===
Preferences prefs;                       // manejador NVS
volatile int32_t lastSavedX = 0;         // último X guardado en NVS
volatile int32_t lastSavedY = 0;         // último Y guardado en NVS
const int32_t NVS_THRESHOLD = 1000;      // guardar cuando cambie ±1000 pasos
unsigned long lastNvsCheckMs = 0;
const unsigned long NVS_CHECK_PERIOD_MS = 500;
bool nvsEnabled = true; // Control desde la webpage // revisa cada 0.5 s

static inline void maybePersistXY() {
  unsigned long now = millis();
  if (now - lastNvsCheckMs < NVS_CHECK_PERIOD_MS) return;
  lastNvsCheckMs = now;

  
  if (!nvsEnabled) return;
int32_t x = posX;  // posX/posY ya existen en el sketch como volatile int32_t
  int32_t y = posY;

  if (abs(x - lastSavedX) >= NVS_THRESHOLD) {
    prefs.putInt("x", x);
    lastSavedX = x;
#if VERBO
    Serial.printf("[NVS] Guardado X=%ld
", (long)x);
#endif
  }
  if (abs(y - lastSavedY) >= NVS_THRESHOLD) {
    prefs.putInt("y", y);
    lastSavedY = y;
#if VERBO
    Serial.printf("[NVS] Guardado Y=%ld
", (long)y);
#endif
  }
}
volatile int8_t dirX = 1;
volatile int8_t dirY = 1;

volatile bool Xactive = 1;

volatile uint16_t status = 0;

uint8_t contador = 0;
volatile uint32_t rawval[4];

volatile int32_t adc_old = 1000;

volatile uint32_t newADC;
volatile int32_t adc, error, pira, volt;
volatile uint32_t tatras = 0;
volatile uint32_t elapsed = 0;

// -------------------- Solar (Aguascalientes) --------------------
const float LATITUDE   = 21.88;    // °N
const float LONGITUDE  = -102.29;  // °E negativo Oeste
const float TILT_ANGLE = 20.0;     // inclinación mecánica hacia el sur

// --- Parámetros de Calibración con Offset Corregido ---
const double TRACKER_AZIMUTH_BASE   = 136.1;    // Orientación del seguidor
const double AZIMUTH_ANGLE_RANGE    = 80.0;
const double AZIMUTH_OFFSET         = 26720.0;  // <-- ¡¡¡ ÚNICO CAMBIO AQUÍ !!!
const double AZIMUTH_SCALE          = 605.3;
const double ZENITH_ANGLE_LIMIT     = 50.0;
const double ZENITH_MAX_STEPS       = 51500.0;
const double ZENITH_SCALE           = 820.0;

// ==================== AUTO-TUNE BUFFER ====================
#ifndef AT_CAP
#define AT_CAP 180  // ~3 min a 1 Hz
#endif

struct AutoTuneBuf {
  int32_t C[AT_CAP];   // Isc (cuentas ADC de AIN0/1)
  int32_t I[AT_CAP];   // Irradiancia W/m2 (de V2 ajustada)
  uint32_t t[AT_CAP];  // millis
  int head = -1;
  int count = 0;
} atb;

static inline int at_index(int k) { return (k + AT_CAP) % AT_CAP; }

void at_push(int32_t Cval, int32_t Ival, uint32_t now_ms) {
  atb.head = (atb.head + 1) % AT_CAP;
  atb.C[atb.head] = Cval;
  atb.I[atb.head] = Ival;
  atb.t[atb.head] = now_ms;
  if (atb.count < AT_CAP) atb.count++;
}

template<typename T>
float quantile_copy(const T* arr, int n, float q) {
  static T tmp[AT_CAP];
  if (n <= 0) return 0;
  for (int i = 0; i < n; i++) tmp[i] = arr[i];
  for (int i = 1; i < n; i++) { // insertion sort
    T key = tmp[i]; int j = i - 1;
    while (j >= 0 && tmp[j] > key) { tmp[j+1] = tmp[j]; j--; }
    tmp[j+1] = key;
  }
  float idx = (n - 1) * q;
  int i0 = (int)idx;
  int i1 = min(i0 + 1, n - 1);
  float frac = idx - i0;
  return tmp[i0] * (1 - frac) + tmp[i1] * frac;
}

volatile bool at_auto_apply = false;
unsigned long at_last_apply_ms = 0;
const unsigned long AT_PERIOD_MS = 60000UL;

// ==================== UTIL DE LOG ====================
String getTimeStamp() {
  struct tm timeinfo;
  if (!getLocalTime(&timeinfo)) return "[sin hora]";
  char buffer[25];
  strftime(buffer, sizeof(buffer), "[%Y-%m-%d %H:%M:%S]", &timeinfo);
  return String(buffer);
}

void agregarLog(String texto) {
  String entrada = getTimeStamp() + " " + texto;
  Serial.println(entrada);
  logBuffer[logIndex] = entrada;
  logIndex = (logIndex + 1) % MAX_LOG_SIZE;
  webSocket.broadcastTXT("LOG:" + entrada);
}

void limpiarLog() {
  for (int i = 0; i < MAX_LOG_SIZE; i++) logBuffer[i] = "";
  logIndex = 0;
  webSocket.broadcastTXT("LOG:[LOG LIMPIADO]");
  Serial.println("[LOG LIMPIADO]");
}

void sendPositionUpdate() {
  String posMsg = "POS:" + String(-posX) + ":" + String(-posY);
  webSocket.broadcastTXT(posMsg);
}

// ==================== WEBSOCKET ====================
void iniciarWebSocket() {
  webSocket.begin();
  webSocket.onEvent([](uint8_t num, WStype_t type, uint8_t * payload, size_t length) {
    if (type == WStype_CONNECTED) {
      for (int i = 0; i < MAX_LOG_SIZE; i++) {
        int idx = (logIndex + i) % MAX_LOG_SIZE;
        if (logBuffer[idx].length() > 0) webSocket.sendTXT(num, "LOG:" + logBuffer[idx]);
      }
      String posMsg = "POS:" + String(-posX) + ":" + String(-posY);
      webSocket.sendTXT(num, posMsg);
    }
  });
}

// ==================== ADC EXTERNO ====================
void externalADC_init() {
  Wire.begin();
  mySensor.begin(0x40, Wire);
  mySensor.setInputMultiplexer(ADS122C04_MUX_AIN2_AVSS);
  mySensor.setGain(ADS122C04_GAIN_1);
  mySensor.setOperatingMode(ADS122C04_OP_MODE_NORMAL);
  mySensor.setConversionMode(ADS122C04_CONVERSION_MODE_SINGLE_SHOT);
  mySensor.setVoltageReference(ADS122C04_VREF_INTERNAL);
  mySensor.setDataCounter(ADS122C04_DCNT_DISABLE);
  mySensor.setDataIntegrityCheck(ADS122C04_CRC_DISABLED);
  mySensor.setBurnOutCurrent(ADS122C04_BURN_OUT_CURRENT_OFF);
  mySensor.setIDACcurrent(ADS122C04_IDAC_CURRENT_OFF);
  mySensor.setIDAC1mux(ADS122C04_IDAC1_DISABLED);
  mySensor.setIDAC2mux(ADS122C04_IDAC2_DISABLED);
}

// ==================== LECTURA ADC ====================
// AIN0/1: Isc (corto). AIN2: piranómetro. AIN3: Voc.
// Se conmuta con pin 9: HIGH => SC, LOW => Voc.
void leetodo() {
  for (contador = 0; contador < 4; contador++) {
    if (contador == 0) { // Isc (cortocircuito vía conmutación)
      mySensor.setInputMultiplexer(ADS122C04_MUX_AIN1_AVSS);
      digitalWrite(9, HIGH);
    }
    if (contador == 1)   // AIN0 (relacionado con Isc)
      mySensor.setInputMultiplexer(ADS122C04_MUX_AIN0_AVSS);
    if (contador == 2) { // Piranómetro
      mySensor.setInputMultiplexer(ADS122C04_MUX_AIN2_AVSS);
      digitalWrite(9, LOW);
    }
    if (contador == 3) { // Voc
      mySensor.setInputMultiplexer(ADS122C04_MUX_AIN3_AVSS);
    }

    mySensor.start();
    unsigned long start_time = millis();
    boolean drdy = false;
    while (!drdy && (millis() < (start_time + ADS122C04_CONVERSION_TIMEOUT))) {
      delay(5);
      drdy = mySensor.checkDataReady();
    }
    if (!drdy) { Serial.println(F("checkDataReady timed out")); return; }

    rawval[contador] = mySensor.readADC();

    // Diagnóstico V0..V3 (ajustes: V2 ÷2 para W/m2; V3 ×2 por divisor)
    Serial.print(F(",V"));
    Serial.print(contador);
    Serial.print(":");
    float base = ((float)rawval[contador] / 8388607.0f) * 2048.0f; // V
    if (contador == 2) {        // Piranómetro → W/m2 directo
      Serial.print(base / 2.0f);
    } else if (contador == 3) { // Voc (divisor 1/2)
      Serial.print(base * 2.0f);
    } else {
      Serial.print(base);
    }
    delay(50);
  }
}

// ==================== CORRIENTE ====================
int32_t current(volatile uint32_t data) { return (int32_t)data; }

// ==================== FINE-TUNE (tu base con dynamic err) ====================
void nomegusta(int32_t value, int32_t e) {
  int32_t current_change = abs(value - adc_old);

  static int32_t dynamic_errmax = errmax;
  static int32_t max_current = -value;
  static bool    near_peak = false;
  static uint8_t escape_counter = 0;

  if (-value > max_current) {
    max_current = -value;
    near_peak = true;
    dynamic_errmax = min_errmax;
    e = dynamic_errmax;
    escape_counter = 0;
  } else if (-value >= max_current * (1.0 - peak_proximity)) {
    near_peak = true;
    dynamic_errmax = min_errmax;
    e = dynamic_errmax / (cicles + 1);
    if (e < min_errmax) e = min_errmax;
    escape_counter++;
  } else if (current_change > abrupt_threshold && cicles > 0) {
    near_peak = false;
    dynamic_errmax = (dynamic_errmax * 60) / 100;
    if (dynamic_errmax < min_errmax) dynamic_errmax = min_errmax;
    e = dynamic_errmax;
    escape_counter = 0;
  } else if (current_change < small_threshold && cicles > 0) {
    char log_buff[120];
    snprintf(log_buff, sizeof(log_buff),
             "FineTune: dC=%d, dynE=%d, e=%d, near=%d, Cmax=%d",
             current_change, dynamic_errmax, e, near_peak, max_current);
    agregarLog(log_buff);
    sendPositionUpdate();
    escape_counter++;
    return;
  } else if (current_change > reset_threshold) {
    near_peak = false;
    dynamic_errmax = errmax;
    max_current = -value;
    e = dynamic_errmax;
    escape_counter = 0;
  } else {
    e = dynamic_errmax;
    escape_counter++;
  }

  if (escape_counter >= 10) {
    near_peak = false;
    dynamic_errmax = errmax;
    e = dynamic_errmax;
    escape_counter = 0;
  }

  char log_buff[120];
  snprintf(log_buff, sizeof(log_buff),
           "FineTune: dC=%d, dynE=%d, e=%d, near=%d, Cmax=%d",
           current_change, dynamic_errmax, e, near_peak, max_current);
  agregarLog(log_buff);

  if (value < adc_old || cicles == 0) {
    if (cicles == 0 && value > adc_old) { if (Xactive) dirY *= -1; else dirX *= -1; }
    if (Xactive) {
      posX += dirX * e;
      if (posX >= 0) posX = 0;
      if (posX <= LIMITX) posX = LIMITX;
      stepperX.moveToPositionInSteps(posX);
    } else {
      posY += dirY * e;
      if (posY >= 0) posY = 0;
      if (posY <= LIMITY) posY = LIMITY;
      stepperY.moveToPositionInSteps(posY);
    }
  } else {
    if (Xactive) {
      dirX *= -1;
      posX += dirX * e;
      if (posX >= 0) posX = 0;
      if (posX <= LIMITX) posX = LIMITX;
      stepperX.moveToPositionInSteps(posX);
    } else {
      dirY *= -1;
      posY += dirY * e;
      if (posY >= 0) posY = 0;
      if (posY <= LIMITY) posY = LIMITY;
      stepperY.moveToPositionInSteps(posY);
    }
  }

  bool moving = true;
  while (moving) {
    stepperX.processMovement();
    stepperY.processMovement();
    moving = !stepperX.motionComplete() || !stepperY.motionComplete();
    delay(1);
  }

  adc_old = value;
  cicles++;
  if (cicles >= cicleMAX) {
    cicles = 0;
    Xactive = !Xactive;      // alterna eje
    dynamic_errmax = errmax; // reset por eje
    max_current = -value;
    near_peak = false;
    escape_counter = 0;
  }

  sendPositionUpdate();
}

// ==================== TRACK SUN BY TIME (tu versión) ====================
void trackSunByTime() {
  status = 3;

  struct tm timeinfo;
  if (!getLocalTime(&timeinfo)) { agregarLog("Error al obtener hora NTP"); status = 0; return; }

  char time_buff[120];
  snprintf(time_buff, sizeof(time_buff),
           "Timeinfo: year=%d, mon=%d, day=%d, hour=%d, min=%d, sec=%d",
           timeinfo.tm_year + 1900, timeinfo.tm_mon + 1, timeinfo.tm_mday,
           timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec);
  agregarLog(time_buff);

  int year = timeinfo.tm_year + 1900;
  int month = timeinfo.tm_mon + 1;
  int day = timeinfo.tm_mday;
  int hour = timeinfo.tm_hour;
  int minute = timeinfo.tm_min;
  int second = timeinfo.tm_sec;

  int a = (14 - month) / 12;
  int y = year + 4800 - a;
  int m = month + 12 * a - 3;
  double jd0 = day + floor((153 * m + 2) / 5.0) + 365.0 * y + floor(y / 4.0) - floor(y / 100.0) + floor(y / 400.0) - 32045.5;
  // local→UTC (ajusta si usas DST manual)
  double ut_hour = hour + minute / 60.0 + second / 3600.0 + 6.0;
  double jdn = jd0 + ut_hour / 24.0;

  char ut_buff[60];
  snprintf(ut_buff, sizeof(ut_buff), "UT_hour: %.6f", ut_hour);
  agregarLog(ut_buff);

  double t = (jdn - 2451545.0) / 36525.0;
  double mean_anomaly = 357.5291 + 35999.0503 * t - 0.0001537 * t * t;
  mean_anomaly = fmod(mean_anomaly, 360.0) * M_PI / 180.0;
  double C = 1.914602 * sin(mean_anomaly) + 0.019993 * sin(2.0 * mean_anomaly) + 0.000289 * sin(3.0 * mean_anomaly);
  double L = 280.46646 + 36000.76983 * t + 0.0003032 * t * t; L = fmod(L, 360.0) * M_PI / 180.0;
  double ecliptic_long = L + C * M_PI / 180.0;
  double epsilon = (23.4392911 - 0.0130042 * t - 0.000000163 * t * t + 0.0000005036 * t * t * t) * M_PI / 180.0;

  double declination = asin(sin(epsilon) * sin(ecliptic_long));
  double ra = atan2(cos(epsilon) * sin(ecliptic_long), cos(ecliptic_long)); if (ra < 0) ra += 2 * M_PI;

  double D = jdn - 2451545.0;
  double gmst_hours = 6.697374558 + 0.06570982441908 * D + ut_hour * 1.00273790935;
  gmst_hours = fmod(gmst_hours, 24.0); if (gmst_hours < 0) gmst_hours += 24.0;
  double gmst = gmst_hours * 15.0 * M_PI / 180.0;

  double lon_rad = LONGITUDE * M_PI / 180.0;
  double lst = gmst + lon_rad; lst = fmod(lst, 2.0 * M_PI); if (lst < 0) lst += 2.0 * M_PI;

  double ha = lst - ra; if (ha < -M_PI) ha += 2 * M_PI; if (ha > M_PI) ha -= 2 * M_PI;

  double phi = LATITUDE * M_PI / 180.0;

  double sin_alt = sin(phi) * sin(declination) + cos(phi) * cos(declination) * cos(ha);
  sin_alt = constrain(sin_alt, -1.0, 1.0);

  double altitude_rad = asin(sin_alt);
  double altitude = altitude_rad * 180.0 / M_PI;
  double zenith_raw = 90.0 - altitude;

  double cos_alt = cos(altitude_rad);
  if (fabs(cos_alt) < 1e-12) cos_alt = (cos_alt >= 0) ? 1e-12 : -1e-12;

  double sin_az = -cos(declination) * sin(ha) / cos_alt;
  double cos_az = (sin(declination) - sin(phi) * sin_alt) / (cos(phi) * cos_alt);
  double azimuth = atan2(sin_az, cos_az) * 180.0 / M_PI; if (azimuth < 0) azimuth += 360.0;

  double zenith = zenith_raw - TILT_ANGLE;
  if (zenith < 0.0) zenith = 0.0;

  char debug_buff[200];
  snprintf(debug_buff, sizeof(debug_buff),
           "Debug: jdn=%.3f, t=%.6f, ra=%.3f, ha=%.3f, decl=%.3f, sin_alt=%.3f, gmst=%.3f, lst=%.3f, alt=%.3f",
           jdn, t, ra * 180.0 / M_PI, ha * 180.0 / M_PI, declination * 180.0 / M_PI,
           sin_alt, gmst * 180.0 / M_PI, lst * 180.0 / M_PI, altitude);
  agregarLog(debug_buff);

  // --- Lógica de Mapeo Corregida ---

  // Map azimuth → Y (Relación Directa)
  // Calcula el ángulo relativo a la orientación real del seguidor.
  double az_relative = azimuth - TRACKER_AZIMUTH_BASE;
  az_relative = constrain(az_relative, -AZIMUTH_ANGLE_RANGE, AZIMUTH_ANGLE_RANGE);
  int32_t target_posY = -(int32_t)(AZIMUTH_OFFSET + az_relative * AZIMUTH_SCALE);

  // Map zenith → X (Relación INVERSA)
  double zenith_adjusted = 90.0 - altitude;
  zenith_adjusted = constrain(zenith_adjusted, 0.0, ZENITH_ANGLE_LIMIT);
  
  // LÓGICA CORREGIDA: Restamos desde el máximo de pasos para invertir el movimiento.
  // A mayor ángulo (sol más bajo), menor es la cuenta de pasos.
  int32_t target_posX = -(int32_t)(ZENITH_MAX_STEPS - zenith_adjusted * ZENITH_SCALE);


  posX = target_posX; posY = target_posY;
  if (posX >= 0) posX = 0; if (posX <= LIMITX) posX = LIMITX;
  if (posY >= 0) posY = 0; if (posY <= LIMITY) posY = LIMITY;
  stepperX.moveToPositionInSteps(posX);
  stepperY.moveToPositionInSteps(posY);

  char buff[120];
  snprintf(buff, sizeof(buff), "SunPos: Az=%.1f, Zen=%.1f (raw=%.1f), posX=%d, posY=%d",
           azimuth, zenith_adjusted, zenith_raw, posX, posY);
  agregarLog(buff);

  bool moving = true;
  while (moving) {
    stepperX.processMovement();
    stepperY.processMovement();
    moving = !stepperX.motionComplete() || !stepperY.motionComplete();
    delay(1);
  }

  int32_t currentValue = current(rawval[1]);
  nomegusta(-currentValue, errmax);
  sendPositionUpdate();
}

// ==================== HOMING ====================
void setXY() {
  stepper_driver_0.setMicrostepsPerStepPowerOfTwo(3);
  long time = millis();
  uint16_t contador = 0;
  uint32_t s = stepper_driver_0.getStallGuardResult();
  uint32_t resultado = 500;

  stepper_driver_0.moveAtVelocity(VELOCITY0);
  stepper_driver_0.disableInverseMotorDirection();
  delay(100);

  while (millis() - time < 45500 && resultado > 180) {
    s += stepper_driver_0.getStallGuardResult();
    contador++;
    if (contador == 272) {
      resultado = s / 272; s = 0; contador = 0;
#ifdef VERBO
      Serial.print("stall_guard_result = "); Serial.println(resultado);
#endif
    }
  }
  stepper_driver_0.moveAtVelocity(0);
  stepper_driver_0.setStandstillMode(stepper_driver_0.NORMAL);

  stepper_driver_1.moveAtVelocity(VELOCITY1);
  stepper_driver_1.enableInverseMotorDirection();
  delay(100);

  time = millis(); contador = 0;
  s = stepper_driver_1.getStallGuardResult(); resultado = 500;
  while (millis() - time < 45000 && resultado > 185) {
    s += stepper_driver_1.getStallGuardResult();
    contador++;
    if (contador == 272) {
      resultado = s / 272; s = 0; contador = 0;
#ifdef VERBO
      Serial.print("stall_guard_motorY = "); Serial.println(resultado);
#endif
    }
  }
#ifdef VERBO
  Serial.print("stall_guard_result-- = "); Serial.println(resultado);
  Serial.println("MOTOR Y SET");
#endif
  stepper_driver_1.moveAtVelocity(0);
  stepper_driver_1.setStandstillMode(stepper_driver_1.NORMAL);

  stepperX.setCurrentPositionInSteps(0);
  stepperY.setCurrentPositionInSteps(0);

  stepper_driver_0.setMicrostepsPerStepPowerOfTwo(3);
  stepper_driver_1.setMicrostepsPerStepPowerOfTwo(3);

  stepperX.setSpeedInStepsPerSecond(6200);
  stepperX.setAccelerationInStepsPerSecondPerSecond(9000);
  stepperY.setSpeedInStepsPerSecond(6200);
  stepperY.setAccelerationInStepsPerSecondPerSecond(9000);

  posX = 0; posY = 0;
  sendPositionUpdate();
}

// ==================== PÁGINA WEB ====================
void handleRoot() {
  server.sendHeader("Content-Type", "text/html; charset=UTF-8");
  String html = R"rawliteral(
<!DOCTYPE html>
<html>
<head>
<meta charset="UTF-8">
<title>ESP32 Logger & Stepper Control</title>
<style>
  .container { display:flex; flex-wrap:wrap; gap:20px; margin-top:20px; }
  .column { flex:1; min-width:300px; max-width:45%; }
  @media (max-width:800px){ .column{ max-width:100%; } }
  .param-group { margin-bottom:10px; }
  button{ margin:2px 0; }
</style>
<script>
  var ws = new WebSocket("ws://" + location.hostname + ":81/");
  ws.onmessage = function(event){
    var data = event.data;
    if (data.startsWith("LOG:")) {
      var log = document.getElementById("log");
      log.innerHTML += data.substring(4) + "<br>";
      log.scrollTop = log.scrollHeight;
    } else if (data.startsWith("POS:")) {
      var parts = data.split(":");
      if (parts.length === 3) {
        document.getElementById("posXDisplay").innerText = parts[2];
        document.getElementById("posYDisplay").innerText = parts[1];
        updatePlot(parts[2], parts[1]);
      }
    }
  };
  function updatePlot(x, y){
    var canvas = document.getElementById("positionPlot");
    var ctx = canvas.getContext("2d");
    var w = canvas.width, h = canvas.height, m=40, maxX=106000, maxY=104000;
    ctx.clearRect(0,0,w,h);
    ctx.strokeStyle="#ccc"; ctx.lineWidth=1;
    for (var i=0;i<=10;i++){
      var xPos=m+(i*(w-2*m)/10);
      ctx.beginPath(); ctx.moveTo(xPos,m); ctx.lineTo(xPos,h-m); ctx.stroke();
      ctx.fillStyle="#000"; ctx.font="12px Arial"; ctx.fillText(Math.round(i*maxX/10), xPos-10, h-m+15);
      var yPos=m+(i*(h-2*m)/10);
      ctx.beginPath(); ctx.moveTo(m,yPos); ctx.lineTo(w-m,yPos); ctx.stroke();
      ctx.fillText(Math.round((10-i)*maxY/10), m-30, yPos+5);
    }
    ctx.strokeStyle="#000"; ctx.lineWidth=2;
    ctx.beginPath(); ctx.moveTo(m,m); ctx.lineTo(m,h-m); ctx.lineTo(w-m,h-m); ctx.stroke();
    var plotX=m+(x/maxX)*(w-2*m);
    var plotY=h-m-(y/maxY)*(h-2*m);
    ctx.fillStyle="red"; ctx.beginPath(); ctx.arc(plotX,plotY,5,0,2*Math.PI); ctx.fill();
  }
  function sendCommand(s){ fetch('/command?status='+s).catch(console.error); }
  function setCoords(){
    var x=document.getElementById("posX").value;
    var y=document.getElementById("posY").value;
    fetch('/setCoords?x='+y+'&y='+x).catch(console.error);
  }
  // NEW: Set Current Position from inputs (sin homing)
  function setCurrentPos(){
    var x=document.getElementById("posX").value;
    var y=document.getElementById("posY").value;
    if(x===""||y===""){ alert('Fill X and Y'); return; }
    fetch('/command?status=6&x='+y+'&y='+x).catch(console.error);
  }
  function setLPFBeta(){ var v=document.getElementById("lpfBeta").value; fetch('/setLPFBeta?beta='+v); }
  function setErrMax(){ var v=document.getElementById("errMax").value; fetch('/setErrMax?err='+v); }
  function setDelay(){ var v=document.getElementById("delayyy").value; fetch('/setDelay?delay='+v); }
  function setMinErrMax(){ var v=document.getElementById("minErrMax").value; fetch('/setMinErrMax?minErr='+v); }
  function setAbruptThreshold(){ var v=document.getElementById("abruptThreshold").value; fetch('/setAbruptThreshold?abrupt='+v); }
  function setSmallThreshold(){ var v=document.getElementById("smallThreshold").value; fetch('/setSmallThreshold?small='+v); }
  function setResetThreshold(){ var v=document.getElementById("resetThreshold").value; fetch('/setResetThreshold?reset='+v); }
  function setPeakProximity(){ var v=document.getElementById("peakProximity").value; fetch('/setPeakProximity?peak='+v); }
  function setCicleMax(){ var v=document.getElementById("cicleMax").value; fetch('/setCicleMax?cicle='+v); }

  // Auto-Tune UI
  function toggleAutoApply(){ var ck=document.getElementById("autoApply").checked ? 1 : 0; fetch('/setAutoApply?on='+ck); }
  function recomputeAT(){ fetch('/recomputeAutoTune'); }
  function applyAT(){ fetch('/applyAutoTune'); }

  window.onload=function(){
    document.getElementById("lpfBeta").value="0.85";
    document.getElementById("errMax").value="1500";
    document.getElementById("delayyy").value="2";
    document.getElementById("minErrMax").value="100";
    document.getElementById("abruptThreshold").value="9000";
    document.getElementById("smallThreshold").value="3000";
    document.getElementById("resetThreshold").value="25000";
    document.getElementById("peakProximity").value="0.015";
    document.getElementById("cicleMax").value="3";
  };

async function refreshNVS(){
  try{
    const r = await fetch('/getNVS');
    const j = await r.json();
    const b = document.getElementById('btnNVS');
    if (b) b.textContent = 'NVS: ' + (j.nvsEnabled ? 'ON' : 'OFF');
  }catch(e){
    const b = document.getElementById('btnNVS'); if (b) b.textContent='NVS: (error)';
  }
}
async function toggleNVS(){
  try{
    const r = await fetch('/toggleNVS');
    const j = await r.json();
    const b = document.getElementById('btnNVS');
    if (b) b.textContent = 'NVS: ' + (j.nvsEnabled ? 'ON' : 'OFF');
  }catch(e){}
}
window.addEventListener('load', refreshNVS);
</script>
</head>
<body>
  <h2>Real-time Log</h2>
  <div id="log" style="height:300px; overflow:auto; border:1px solid black; background:#f0f0f0; padding:5px;"></div>

  <div style="margin:10px 0;">
  <button id="btnNVS" onclick="toggleNVS()">NVS: (cargando...)</button>
</div>
<div class="container">
    <div class="column">
      <h2>Current Motor Positions</h2>
      <p><strong>X:</strong> <span id="posXDisplay">0</span></p>
      <p><strong>Y:</strong> <span id="posYDisplay">0</span></p>
      <h2>Position Visualization</h2>
      <canvas id="positionPlot" width="400" height="400" style="border:1px solid black;"></canvas>
    </div>

    <div class="column">
      <h2>Motor Control</h2>
      <div>
        <h3>Coordinates (X, Y)</h3>
        <input type="number" id="posX" placeholder="X (0 to 106000)" min="0" max="106000">
        <input type="number" id="posY" placeholder="Y (0 to 104000)" min="0" max="104000">
        <button onclick="setCoords()">Move to Coordinates</button>
        <button onclick="setCurrentPos()">Set Current Position (from inputs)</button>
      </div>

      <div style="margin-top:10px;">
        <h3>Fine-Tuning Parameters</h3>
        <div class="param-group"><label>LPF Beta:</label> <input type="number" id="lpfBeta" step="0.01" min="0" max="1"> <button onclick="setLPFBeta()">Set</button></div>
        <div class="param-group"><label>Error Max:</label> <input type="number" id="errMax" min="0"> <button onclick="setErrMax()">Set</button></div>
        <div class="param-group"><label>Delay (s):</label> <input type="number" id="delayyy" min="0"> <button onclick="setDelay()">Set</button></div>
        <div class="param-group"><label>Min Error Max:</label> <input type="number" id="minErrMax" min="0"> <button onclick="setMinErrMax()">Set</button></div>
        <div class="param-group"><label>Abrupt Threshold:</label> <input type="number" id="abruptThreshold" min="0"> <button onclick="setAbruptThreshold()">Set</button></div>
        <div class="param-group"><label>Small Threshold:</label> <input type="number" id="smallThreshold" min="0"> <button onclick="setSmallThreshold()">Set</button></div>
        <div class="param-group"><label>Reset Threshold:</label> <input type="number" id="resetThreshold" min="0"> <button onclick="setResetThreshold()">Set</button></div>
        <div class="param-group"><label>Peak Proximity (0-1):</label> <input type="number" id="peakProximity" step="0.01" min="0" max="1"> <button onclick="setPeakProximity()">Set</button></div>
        <div class="param-group"><label>Cycle Max:</label> <input type="number" id="cicleMax" min="1"> <button onclick="setCicleMax()">Set</button></div>
      </div>

      <div style="margin-top:10px;">
        <h3>Commands</h3>
        <button onclick="sendCommand(2)">Track Sun</button>
        <button onclick="sendCommand(3)">Fine-Tune</button>
        <button onclick="sendCommand(4)">Set XY (Homing)</button>
        <button onclick="sendCommand(5)">Move to Center</button>
        <button onclick="sendCommand(6)">Set Current Position (legacy)</button>
        <button onclick="sendCommand(10)">View Configuration</button>
        <button onclick="sendCommand(11)">Demo Mode</button>
      </div>

      <div style="margin-top:10px;">
        <h3>Auto-Tune (datos → umbrales)</h3>
        <div class="param-group"><label><input type="checkbox" id="autoApply" onchange="toggleAutoApply()"> Auto-apply cada 60s</label></div>
        <div class="param-group">
          <button onclick="recomputeAT()">Recompute (solo log)</button>
          <button onclick="applyAT()" style="margin-left:8px;">Apply Now</button>
        </div>
      </div>

      <h2>Firmware Update (OTA)</h2>
      <form method="POST" action="/update" enctype="multipart/form-data">
        <input type="file" name="update">
        <input type="submit" value="Upload">
      </form>
      <h2>Options</h2>
      <a href="/download" download="log.txt"><button>Download Log</button></a>
      <a href="/clear"><button style="margin-left:10px;">Clear Log</button></a>
    </div>
  </div>
</body>
</html>
  )rawliteral";
  server.send(200, "text/html", html);
}

// ==================== DESCARGA / LIMPIEZA / OTA ====================
void handleDownload() {
  String txt;
  for (int i = 0; i < MAX_LOG_SIZE; i++) {
    int idx = (logIndex + i) % MAX_LOG_SIZE;
    if (logBuffer[idx].length() > 0) txt += logBuffer[idx] + "\r\n";
  }
  server.send(200, "text/plain", txt);
}

void handleClearLog() {
  limpiarLog();
  server.sendHeader("Location", "/");
  server.send(303);
}

void handleUpdateUpload() {
  HTTPUpload& upload = server.upload();
  if (upload.status == UPLOAD_FILE_START) {
    if (!Update.begin()) Update.printError(Serial);
  } else if (upload.status == UPLOAD_FILE_WRITE) {
    if (Update.write(upload.buf, upload.currentSize) != upload.currentSize) Update.printError(Serial);
  } else if (upload.status == UPLOAD_FILE_END) {
    if (Update.end(true)) { Serial.printf("OTA completa: %u bytes\n", upload.totalSize); }
    else { Update.printError(Serial); }
  }
}

// ==================== ENDPOINTS DE SET ====================
void handleSetCoords() {
  if (server.hasArg("x") && server.hasArg("y")) {
    posX = -server.arg("x").toInt();
    posY = -server.arg("y").toInt();
    agregarLog("Recibi X:" + String(-posX) + ", Y:" + String(-posY));
    stepperX.moveToPositionInSteps(posX);
    stepperY.moveToPositionInSteps(posY);
    sendPositionUpdate();
    server.send(200, "text/plain", "Coordenadas seteadas");
  } else server.send(400, "text/plain", "Faltan parámetros x o y");
}
void handleSetLPFBeta()        { if (server.hasArg("beta")) { LPF_Beta = server.arg("beta").toFloat(); agregarLog("LPF_Beta="+String(LPF_Beta)); server.send(200,"text/plain","LPF_Beta OK"); } else server.send(400,"text/plain","Falta beta"); }
void handleSetErrMax()         { if (server.hasArg("err"))  { errmax   = server.arg("err").toInt();   agregarLog("ErrMax="+String(errmax));     server.send(200,"text/plain","ErrMax OK"); } else server.send(400,"text/plain","Falta err"); }
void handleSetDelay()          { if (server.hasArg("delay")){ delayyy  = server.arg("delay").toInt(); agregarLog("Delay="+String(delayyy));      server.send(200,"text/plain","Delay OK"); } else server.send(400,"text/plain","Falta delay"); }
void handleSetMinErrMax()      { if (server.hasArg("minErr")){min_errmax=server.arg("minErr").toInt(); agregarLog("MinErrMax="+String(min_errmax)); server.send(200,"text/plain","MinErrMax OK"); } else server.send(400,"text/plain","Falta minErr"); }
void handleSetAbruptThreshold(){ if (server.hasArg("abrupt")){abrupt_threshold=server.arg("abrupt").toInt(); agregarLog("AbruptThreshold="+String(abrupt_threshold)); server.send(200,"text/plain","AbruptThreshold OK"); } else server.send(400,"text/plain","Falta abrupt"); }
void handleSetSmallThreshold() { if (server.hasArg("small")) {small_threshold=server.arg("small").toInt(); agregarLog("SmallThreshold="+String(small_threshold)); server.send(200,"text/plain","SmallThreshold OK"); } else server.send(400,"text/plain","Falta small"); }
void handleSetResetThreshold() { if (server.hasArg("reset")) {reset_threshold=server.arg("reset").toInt(); agregarLog("ResetThreshold="+String(reset_threshold)); server.send(200,"text/plain","ResetThreshold OK"); } else server.send(400,"text/plain","Falta reset"); }
void handleSetPeakProximity()  { if (server.hasArg("peak"))  {peak_proximity=server.arg("peak").toFloat(); agregarLog("PeakProximity="+String(peak_proximity)); server.send(200,"text/plain","PeakProximity OK"); } else server.send(400,"text/plain","Falta peak"); }
void handleSetCicleMax()       { if (server.hasArg("cicle")) {cicleMAX=server.arg("cicle").toInt(); agregarLog("CicleMax="+String(cicleMAX)); server.send(200,"text/plain","CicleMax OK"); } else server.send(400,"text/plain","Falta cicle"); }

// ==================== COMANDOS ====================
void handleCommand() {
  if (!server.hasArg("status")) { server.send(400, "text/plain", "Falta status"); return; }
  status = server.arg("status").toInt();
  agregarLog("Ejecutando comando: " + String(status));
  switch (status) {
    case 2: trackSunByTime(); break;
    case 3: status = 3; agregarLog("Iniciando Fine-Tune (nomegusta)"); break;
    case 4: setXY(); sendPositionUpdate(); break;
    case 5:
      posX = LIMITX / 2; posY = LIMITY / 2;
      stepperX.moveToPositionInSteps(posX); stepperY.moveToPositionInSteps(posY);
      agregarLog("Moviendo a centro (X:" + String(-posX) + ", Y:" + String(-posY) + ")");
      sendPositionUpdate(); break;
    case 6: // Set current position (usa x/y si vienen)
      if (server.hasArg("x") && server.hasArg("y")) {
        posX = -server.arg("x").toInt();
        posY = -server.arg("y").toInt();
        stepperX.setCurrentPositionInSteps(posX);
        stepperY.setCurrentPositionInSteps(posY);
        stepper_driver_0.setMicrostepsPerStepPowerOfTwo(3);
        stepper_driver_1.setMicrostepsPerStepPowerOfTwo(3);
        stepperX.setSpeedInStepsPerSecond(4200); stepperX.setAccelerationInStepsPerSecondPerSecond(4000);
        stepperY.setSpeedInStepsPerSecond(4200); stepperY.setAccelerationInStepsPerSecondPerSecond(4000);
        agregarLog("Posición actual seteada: X:" + String(-posX) + ", Y:" + String(-posY));
        sendPositionUpdate();
      } else {
        // si no vienen x/y, no fallamos: usamos los inputs previos (no hay en servidor), así que avisamos
        agregarLog("status=6 sin x/y → Ignorado (usa botón 'Set Current Position (from inputs)')");
      }
      break;
    case 10: {
      TMC2209::Settings s = stepper_driver_0.getSettings();
      agregarLog("Driver0: comm="+String(s.is_communicating)+", setup="+String(s.is_setup)+", en="+String(s.software_enabled)+", μsteps="+String(s.microsteps_per_step)+", irun="+String(s.irun_percent));
      s = stepper_driver_1.getSettings();
      agregarLog("Driver1: comm="+String(s.is_communicating)+", setup="+String(s.is_setup)+", en="+String(s.software_enabled)+", μsteps="+String(s.microsteps_per_step)+", irun="+String(s.irun_percent));
    } break;
    case 11:
      setXY(); agregarLog("Modo demo activo, seteando");
      stepperX.moveToPositionInSteps(LIMITX);
      stepperY.moveToPositionInSteps(LIMITY);
      agregarLog("Modo demo activo, moviendo al final (X:" + String(-posX) + ", Y:" + String(-posY) + ")"); sendPositionUpdate();
      break;
    default: server.send(400, "text/plain", "Comando no válido"); return;
  }
  server.send(200, "text/plain", "Comando ejecutado");
}

// ==================== AUTO-TUNE (recomendaciones) ====================
void at_recommend_thresholds(bool applyNow) {
  if (atb.count < 20) { agregarLog("AT: pocos datos (" + String(atb.count) + "), espera más muestras"); return; }

  const int n = atb.count;
  static int32_t abs_dC[AT_CAP];
  static int32_t dI[AT_CAP];
  int m = 0;
  int64_t sumI = 0, maxC = 0;

  for (int k = 1; k < n; ++k) {
    int i1 = at_index(atb.head - (n - 1 - k));
    int i0 = at_index(atb.head - (n - 1 - (k - 1)));
    int32_t dC = atb.C[i1] - atb.C[i0];
    int32_t dIv = atb.I[i1] - atb.I[i0];
    abs_dC[m] = abs(dC);
    dI[m] = dIv;
    m++;
  }
  for (int k = 0; k < n; ++k) {
    int idx = at_index(atb.head - (n - 1 - k));
    sumI += atb.I[idx];
    if (llabs((long long)atb.C[idx]) > maxC) maxC = llabs((long long)atb.C[idx]);
  }
  float meanI = (float)sumI / (float)n;

  // zona estable por irradiancia
  const float thrI = max(25.0f, 0.03f * meanI);
  static int32_t abs_dC_stable[AT_CAP];
  int ms = 0;
  for (int i = 0; i < m; ++i) if (fabs((float)dI[i]) <= thrI) abs_dC_stable[ms++] = abs_dC[i];
  if (ms < 5) { for (int i = 0; i < m && i < 10; ++i) abs_dC_stable[i] = abs_dC[i]; ms = min(m, 10); }

  int32_t q_small  = (int32_t)lroundf(quantile_copy(abs_dC_stable, ms, 0.70f));
  int32_t q_abrupt = (int32_t)lroundf(quantile_copy(abs_dC,        m,  0.90f));
  int32_t q_reset  = (int32_t)lroundf(quantile_copy(abs_dC,        m,  0.99f));

  int32_t rec_small  = max(5, q_small);
  int32_t rec_abrupt = max(rec_small * 3, q_abrupt);
  int32_t rec_reset  = max(rec_abrupt * 2, q_reset);

  float rec_peak = 0.05f;
  if (maxC > 0) {
    rec_peak = 2.0f * ((float)rec_small / (float)maxC);
    if (rec_peak < 0.01f) rec_peak = 0.01f;
    if (rec_peak > 0.10f) rec_peak = 0.10f;
  }

  char msg[180];
  snprintf(msg, sizeof(msg),
           "AT: meanI=%.1f, q70(dC_estable)=%d, q90(dC)=%d, q99(dC)=%d, max|C|=%lld",
           meanI, q_small, q_abrupt, q_reset, (long long)maxC);
  agregarLog(msg);

  snprintf(msg, sizeof(msg),
           "AT: rec small=%d, abrupt=%d, reset=%d, peak_prox=%.3f",
           rec_small, rec_abrupt, rec_reset, rec_peak);
  agregarLog(msg);

  if (applyNow) {
    small_threshold = rec_small;
    abrupt_threshold = rec_abrupt;
    reset_threshold  = rec_reset;
    peak_proximity   = rec_peak;
    snprintf(msg, sizeof(msg),
             "AT: APLICADOS -> small=%d, abrupt=%d, reset=%d, peak=%.3f",
             small_threshold, abrupt_threshold, reset_threshold, peak_proximity);
    agregarLog(msg);
  }
}

// Endpoints Auto-Tune
void handleRecomputeAutoTune() { at_recommend_thresholds(false); server.send(200, "text/plain", "Recomputed (ver log)"); }
void handleApplyAutoTune()     { at_recommend_thresholds(true);  server.send(200, "text/plain", "Applied (ver log)"); }
void handleSetAutoApply() {
  if (server.hasArg("on")) {
    at_auto_apply = (server.arg("on") == "1");
    agregarLog(String("AT auto-apply: ") + (at_auto_apply ? "ON" : "OFF"));
    server.send(200, "text/plain", "OK");
  } else server.send(400, "text/plain", "Falta 'on'");
}

// ==================== SETUP ====================
void setup() {
  // --- Inicializa NVS y restaura última posición X/Y ---
  prefs.begin("coords", false);
  lastSavedX = prefs.getInt("x", 0);
  lastSavedY = prefs.getInt("y", 0);
  posX = lastSavedX;
  posY = lastSavedY;
#if VERBO
  Serial.printf("[NVS] Restaurado X=%ld Y=%ld\n", (long)posX, (long)posY);
#endif

  Serial.begin(9600);
  serial_stream.begin(SERIAL_BAUD_RATE);

  WiFi.begin(ssid, password);
  Serial.print("Conectando a WiFi");
  while (WiFi.status() != WL_CONNECTED) { delay(500); Serial.print("."); }
  Serial.println("\nWiFi conectado.");
  Serial.print("IP local: "); Serial.println(WiFi.localIP());
  agregarLog("WiFi conectado. IP: " + WiFi.localIP().toString());

  configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);
  struct tm timeinfo;
  if (!getLocalTime(&timeinfo)) { Serial.println("Error al sincronizar NTP."); agregarLog("Error al sincronizar NTP."); }
  else { Serial.println(&timeinfo, "Hora sincronizada: %Y-%m-%d %H:%M:%S"); agregarLog("Hora sincronizada"); }

  pinMode(10, OUTPUT); digitalWrite(10, LOW);
  stepper_driver_0.setup(serial_stream, SERIAL_BAUD_RATE, SERIAL_ADDRESS_1);
  stepper_driver_0.setReplyDelay(REPLY_DELAY);
  stepper_driver_1.setup(serial_stream, SERIAL_BAUD_RATE, SERIAL_ADDRESS_3);
  stepper_driver_1.setReplyDelay(REPLY_DELAY);

  while (!stepper_driver_0.isCommunicating()) {
    Serial.println("Stepper driver 1 not setup and communicating!");
    agregarLog("Stepper driver 1 no configurado");
    stepper_driver_0.setup(serial_stream, SERIAL_BAUD_RATE, SERIAL_ADDRESS_1);
    stepper_driver_0.setReplyDelay(REPLY_DELAY);
    delay(100);
  }
  Serial.println("COM READY"); agregarLog("COM READY");

  stepper_driver_0.moveAtVelocity(0);
  stepper_driver_0.setRunCurrent(RUN_CURRENT_PERCENT0);
  stepper_driver_0.setMicrostepsPerStepPowerOfTwo(3);
  stepper_driver_0.setStallGuardThreshold(STALL_GUARD_THRESHOLD0);
  stepper_driver_0.enableAutomaticCurrentScaling();
  stepper_driver_0.enableCoolStep(0, 2);
  stepper_driver_0.enable();
  stepper_driver_0.setHoldCurrent(2);

  stepper_driver_1.moveAtVelocity(0);
  stepper_driver_1.setRunCurrent(RUN_CURRENT_PERCENT1);
  stepper_driver_1.setMicrostepsPerStepPowerOfTwo(3);
  stepper_driver_1.setStallGuardThreshold(STALL_GUARD_THRESHOLD1);
  stepper_driver_1.enableAutomaticCurrentScaling();
  stepper_driver_1.enableCoolStep(0, 2);
  stepper_driver_1.enable();
  stepper_driver_1.setHoldCurrent(2);

  stepperX.connectToPins(MOTOR_X_STEP_PIN, MOTOR_X_DIRECTION_PIN);
  stepperY.connectToPins(MOTOR_Y_STEP_PIN, MOTOR_Y_DIRECTION_PIN);

  stepper_driver_0.disableInverseMotorDirection();
  stepper_driver_1.enableInverseMotorDirection();
  stepperX.setCurrentPositionInSteps(posX);
  stepperY.setCurrentPositionInSteps(posY);
  stepper_driver_0.setMicrostepsPerStepPowerOfTwo(3);
  stepper_driver_1.setMicrostepsPerStepPowerOfTwo(3);
  stepperX.setSpeedInStepsPerSecond(6200);
  stepperX.setAccelerationInStepsPerSecondPerSecond(9000);
  stepperY.setSpeedInStepsPerSecond(6200);
  stepperY.setAccelerationInStepsPerSecondPerSecond(9000);

  sendPositionUpdate();

  pinMode(9, OUTPUT);
  digitalWrite(9, VocIsc);
  externalADC_init();

  server.on("/", HTTP_GET, handleRoot);
  server.on("/download", HTTP_GET, handleDownload);
  server.on("/clear", HTTP_GET, handleClearLog);
  server.on("/setCoords", HTTP_GET, handleSetCoords);
  server.on("/setLPFBeta", HTTP_GET, handleSetLPFBeta);
  server.on("/setErrMax", HTTP_GET, handleSetErrMax);
  server.on("/setDelay", HTTP_GET, handleSetDelay);
  server.on("/setMinErrMax", HTTP_GET, handleSetMinErrMax);
  server.on("/setAbruptThreshold", HTTP_GET, handleSetAbruptThreshold);
  server.on("/setSmallThreshold", HTTP_GET, handleSetSmallThreshold);
  server.on("/setResetThreshold", HTTP_GET, handleSetResetThreshold);
  server.on("/setPeakProximity", HTTP_GET, handleSetPeakProximity);
  server.on("/setCicleMax", HTTP_GET, handleSetCicleMax);
  server.on("/command", HTTP_GET, handleCommand);
  server.on("/update", HTTP_POST,
    [](){ server.sendHeader("Connection", "close"); server.send(200, "text/plain", Update.hasError() ? "Fallo OTA" : "OTA OK, reiniciando..."); ESP.restart(); },
    handleUpdateUpload
  );
  // Auto-Tune endpoints
  server.on("/recomputeAutoTune", HTTP_GET, handleRecomputeAutoTune);
  server.on("/applyAutoTune",     HTTP_GET, handleApplyAutoTune);
  server.on("/setAutoApply",      HTTP_GET, handleSetAutoApply);
  // --- NVS control endpoints ---
  server.on("/toggleNVS", HTTP_GET, [](){
    nvsEnabled = !nvsEnabled;
    String json = String("{\"nvsEnabled\":") + (nvsEnabled ? "true" : "false") + "}";
    server.sendHeader("Access-Control-Allow-Origin", "*");
    server.send(200, "application/json", json);
  });
  server.on("/getNVS", HTTP_GET, [](){
    String json = String("{\"nvsEnabled\":") + (nvsEnabled ? "true" : "false") + "}";
    server.sendHeader("Access-Control-Allow-Origin", "*");
    server.send(200, "application/json", json);
  });


  server.begin();
  iniciarWebSocket();
  agregarLog("Servidor iniciado como cliente WiFi.");

  Serial.println("Free heap: " + String(ESP.getFreeHeap()));
}

// ==================== LOOP ====================
void loop() {
  // Persistencia condicional de X/Y
  maybePersistXY();

  server.handleClient();
  webSocket.loop();

  static int32_t lastPosX = posX, lastPosY = posY;

  if (millis() / 1000 >= tatras + delayyy) {
    leetodo();
    tatras = millis() / 1000;

    char buff[90];
    sprintf(buff, "X:%d,Y:%d,C:%d,V:%d,P:%d,S:%d,er:%d", -posX, -posY, rawval[1], rawval[3], rawval[2], tatras, error);
    agregarLog(buff);

    // push al buffer de Auto-Tune (Irradiancia desde V2 ajustada)
    float v2_base = ((float)rawval[2] / 8388607.0f) * 2048.0f;
    int32_t I_wm2 = (int32_t)lroundf(v2_base / 2.0f);
    int32_t C_counts = (int32_t)rawval[1];
    at_push(C_counts, I_wm2, millis());

    int32_t varerr = -(int32_t)rawval[1];
    error = errmax; // nomegusta ajusta dinámicamente
    if (status == 3) nomegusta(varerr, error);
  }

  if (posX != lastPosX || posY != lastPosY) {
    sendPositionUpdate();
    lastPosX = posX; lastPosY = posY;
  }

  // Auto-apply periódico
  if (at_auto_apply && (millis() - at_last_apply_ms >= AT_PERIOD_MS)) {
    at_recommend_thresholds(true);
    at_last_apply_ms = millis();
  }
}