#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "freertos/task.h"
#include <Arduino.h>
#include <cmath>

// ==========================================
// PROTOCOLO UART
// ==========================================
//
// RX (RPi → ESP32):
//   M:s1,f1,s2,f2,s3,f3,s4,f4\n
//       s = signo  → -1 | 0 | 1
//       f = fracción 0.000–1.000 de VEL_MAX_TICKS del motor
//   S:\n  → Stop inmediato
//   C:\n  → Iniciar autocalibración
//
// TX (ESP32 → RPi):
//   T:t1,t2,t3,t4\n  → Odometría acumulada (ticks con signo)
//   K:v1,v2,v3,v4\n  → VEL_MAX_TICKS de cada motor (ticks/s) — tras calibrar
//   D:...\n           → Debug (solo por USB/Serial)
//
// ==========================================

// ─── FLAGS ────────────────────────────────────────────────────────────────────
volatile bool iniciar_calibracion = false;
volatile bool en_calibracion      = false;

// ─── PINES ────────────────────────────────────────────────────────────────────
const int PIN_RX_RPI  = 44, PIN_TX_RPI = 43;
const int PIN_STBY_1  = 40, PIN_STBY_2 = 7;

const int PIN_M1_IN1 = 15, PIN_M1_IN2 = 16, PIN_M1_PWM = 17, PIN_ENC1 = 9;
const int PIN_M2_IN1 = 6,  PIN_M2_IN2 = 5,  PIN_M2_PWM = 4,  PIN_ENC2 = 10;
const int PIN_M3_IN1 = 41, PIN_M3_IN2 = 42, PIN_M3_PWM = 2,  PIN_ENC3 = 36;
const int PIN_M4_IN1 = 39, PIN_M4_IN2 = 38, PIN_M4_PWM = 37, PIN_ENC4 = 35;

const int FREC_PWM = 1000, RES_PWM = 8;

const int IN1[] = {PIN_M1_IN1, PIN_M2_IN1, PIN_M3_IN1, PIN_M4_IN1};
const int IN2[] = {PIN_M1_IN2, PIN_M2_IN2, PIN_M3_IN2, PIN_M4_IN2};
const int PWM[] = {PIN_M1_PWM, PIN_M2_PWM, PIN_M3_PWM, PIN_M4_PWM};
const int ENC[] = {PIN_ENC1,   PIN_ENC2,   PIN_ENC3,   PIN_ENC4};

// ─── ENCODERS ─────────────────────────────────────────────────────────────────
volatile unsigned long pulsos[4] = {0, 0, 0, 0};
void IRAM_ATTR isr_E1() { pulsos[0]++; }
void IRAM_ATTR isr_E2() { pulsos[1]++; }
void IRAM_ATTR isr_E3() { pulsos[2]++; }
void IRAM_ATTR isr_E4() { pulsos[3]++; }

long global_ticks[4] = {0, 0, 0, 0};
SemaphoreHandle_t mutexDatos;

// ─── VELOCIDAD MÁXIMA CALIBRADA ───────────────────────────────────────────────
// En ticks/s. La RPi la pide con C: y la recibe en K:
float VEL_MAX_TICKS[4] = {200.0f, 200.0f, 200.0f, 200.0f};

// Banda muerta por motor = 3% de VEL_MAX (se recalcula tras calibrar)
float BANDA_MUERTA[4]  = {6.0f, 6.0f, 6.0f, 6.0f};

// ==========================================
// CLASE PID
// ==========================================
class PID {
public:
  float kp, ki, kff;
  float error_acumulado = 0.0f;
  float setpoint        = 0.0f;   // ticks/s, con signo

  PID(float _kp, float _ki, float _kff) : kp(_kp), ki(_ki), kff(_kff) {}

  // vel_actual: ticks/s con signo
  // banda    : umbral bajo el cual el error se ignora (ticks/s)
  int calcularPWM(float vel_actual, float dt, float banda) {
    if (fabsf(setpoint) < 1.0f) {
      error_acumulado = 0.0f;
      return 0;
    }

    float error = setpoint - vel_actual;

    // Banda muerta solo en régimen estacionario (motor ya girando)
    if (fabsf(error) < banda && fabsf(vel_actual) > 5.0f)
      error = 0.0f;

    error_acumulado += error * dt;
    float lim = 200.0f / (ki + 0.0001f);
    error_acumulado = constrain(error_acumulado, -lim, lim);

    float salida = kp * error
                 + ki * error_acumulado
                 + copysignf(fabsf(setpoint) * kff, setpoint);

    return (int)constrain(salida, -255.0f, 255.0f);
  }

  void reset() { error_acumulado = 0.0f; setpoint = 0.0f; }
};

PID pid[4] = {
  PID(2.0f, 1.5f, 0.0f),   // M1 FL
  PID(2.0f, 1.5f, 0.0f),   // M2 FR
  PID(2.0f, 1.5f, 0.0f),   // M3 RL
  PID(2.0f, 1.5f, 0.0f),   // M4 RR
};

// ─── MOTOR ────────────────────────────────────────────────────────────────────
void moverMotor(int idx, int val) {
  if (abs(val) < 15) {
    digitalWrite(IN1[idx], LOW);
    digitalWrite(IN2[idx], LOW);
    ledcWrite(PWM[idx], 0);
    return;
  }
  if (val > 0) { digitalWrite(IN1[idx], HIGH); digitalWrite(IN2[idx], LOW); }
  else         { digitalWrite(IN1[idx], LOW);  digitalWrite(IN2[idx], HIGH); val = -val; }
  ledcWrite(PWM[idx], val);
}

void pararTodos() {
  for (int i = 0; i < 4; i++) moverMotor(i, 0);
}

// ─── LECTURA ATÓMICA DE ENCODERS ──────────────────────────────────────────────
// portENTER_CRITICAL_ISR bloquea ambos cores en ESP32 (a diferencia de noInterrupts)
static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;

inline void leerPulsos(unsigned long dest[4]) {
  portENTER_CRITICAL(&mux);
  for (int i = 0; i < 4; i++) dest[i] = pulsos[i];
  portEXIT_CRITICAL(&mux);
}

// ==========================================
// TAREA: CONTROL DE MOTORES (CORE 1)
// ==========================================
void tareaControlMotores(void *pvParameters) {
  unsigned long p_ant[4]  = {0, 0, 0, 0};
  static float  v_f[4]    = {0, 0, 0, 0};
  const float   alpha      = 0.35f;
  const float   dt         = 0.05f;
  const TickType_t xFrec   = pdMS_TO_TICKS(50);
  TickType_t xUltimo       = xTaskGetTickCount();

  for (;;) {

    // ────────────────────────────────────────────────────────────────────────
    //  AUTOCALIBRACIÓN
    // ────────────────────────────────────────────────────────────────────────
    if (iniciar_calibracion) {
      en_calibracion = true;
      for (int i = 0; i < 4; i++) pid[i].reset();
      pararTodos();
      vTaskDelay(pdMS_TO_TICKS(500));

      unsigned long p_base[4];
      leerPulsos(p_base);
      for (int i = 0; i < 4; i++) p_ant[i] = p_base[i];

      // — Paso 1: fricción estática ————————————————————————————————————————
      int friccion[4] = {0, 0, 0, 0};
      for (int p = 20; p <= 130; p += 2) {
        for (int i = 0; i < 4; i++)
          if (friccion[i] == 0) {
            digitalWrite(IN1[i], LOW);
            digitalWrite(IN2[i], HIGH);
            ledcWrite(PWM[i], p);
          }
        vTaskDelay(pdMS_TO_TICKS(60));

        unsigned long p_act[4];
        leerPulsos(p_act);
        bool todos = true;
        for (int i = 0; i < 4; i++) {
          if (friccion[i] == 0) {
            if ((p_act[i] - p_ant[i]) > 1) friccion[i] = p;
            else                            todos = false;
          }
          p_ant[i] = p_act[i];
        }
        if (todos) break;
      }

      // — Paso 2: velocidad máxima (kff y VEL_MAX_TICKS) ——————————————————
      const int TEST_PWM = 200;
      for (int i = 0; i < 4; i++) {
        digitalWrite(IN1[i], LOW);
        digitalWrite(IN2[i], HIGH);
        ledcWrite(PWM[i], TEST_PWM);
      }
      vTaskDelay(pdMS_TO_TICKS(800));   // estabilización

      unsigned long p_ss1[4]; leerPulsos(p_ss1);
      vTaskDelay(pdMS_TO_TICKS(300));
      unsigned long p_ss2[4]; leerPulsos(p_ss2);

      for (int i = 0; i < 4; i++) {
        float vel_medida = (float)(p_ss2[i] - p_ss1[i]) / 0.3f;  // ticks/s

        // VEL_MAX extrapolada a PWM 255
        VEL_MAX_TICKS[i] = (vel_medida > 10.0f)
            ? vel_medida * (255.0f / (float)(TEST_PWM - friccion[i]))
            : 200.0f;
        VEL_MAX_TICKS[i] = constrain(VEL_MAX_TICKS[i], 50.0f, 1000.0f);

        // kff: PWM por ticks/s
        pid[i].kff = (vel_medida > 10.0f)
            ? (float)(TEST_PWM - friccion[i]) / vel_medida
            : 2.0f;
        pid[i].kff = constrain(pid[i].kff, 0.1f, 8.0f);

        // Banda muerta = 3% de VEL_MAX
        BANDA_MUERTA[i] = VEL_MAX_TICKS[i] * 0.03f;

        pid[i].reset();
      }

      pararTodos();

      // Reporte por USB
      Serial.println("=== CALIBRACION COMPLETA ===");
      for (int i = 0; i < 4; i++)
        Serial.printf("M%d  fric=%d  vel_max=%.1f  kff=%.3f  banda=%.1f\n",
          i+1, friccion[i], VEL_MAX_TICKS[i], pid[i].kff, BANDA_MUERTA[i]);

      // ── Trama K: → RPi para que actualice su VEL_MAX ──────────────────────
      // Tanto por USB como por Serial1 (UART RPi)
      char buf[80];
      snprintf(buf, sizeof(buf), "K:%.1f,%.1f,%.1f,%.1f\n",
               VEL_MAX_TICKS[0], VEL_MAX_TICKS[1],
               VEL_MAX_TICKS[2], VEL_MAX_TICKS[3]);
      Serial.print(buf);
      Serial1.print(buf);

      leerPulsos(p_ant);
      iniciar_calibracion = false;
      en_calibracion      = false;
    }
    // ────────────────────────────────────────────────────────────────────────

    // Leer encoders
    unsigned long p_act[4];
    leerPulsos(p_act);

    unsigned long d_p[4];
    for (int i = 0; i < 4; i++) {
      d_p[i]  = p_act[i] - p_ant[i];
      p_ant[i] = p_act[i];
    }

    // Dirección del setpoint para acumular odometría con signo
    int d_f[4];
    for (int i = 0; i < 4; i++)
      d_f[i] = (pid[i].setpoint >  1.0f) ?  1 :
               (pid[i].setpoint < -1.0f) ? -1 : 0;

    if (xSemaphoreTake(mutexDatos, pdMS_TO_TICKS(5)) == pdTRUE) {
      for (int i = 0; i < 4; i++)
        global_ticks[i] += (long)d_p[i] * d_f[i];
      xSemaphoreGive(mutexDatos);
    }

    // Velocidad filtrada con EMA
    for (int i = 0; i < 4; i++) {
      float v_cruda = ((float)d_p[i] * (float)d_f[i]) / dt;
      v_f[i] = alpha * v_cruda + (1.0f - alpha) * v_f[i];
    }

    // PID y actuación
    for (int i = 0; i < 4; i++)
      moverMotor(i, pid[i].calcularPWM(v_f[i], dt, BANDA_MUERTA[i]));

    // Debug cada 500ms (10 ciclos × 50ms)
    static int dbg = 0;
    if (++dbg >= 10) {
      dbg = 0;
      Serial.printf("D:SP=%.0f,%.0f,%.0f,%.0f V=%.0f,%.0f,%.0f,%.0f\n",
        pid[0].setpoint, pid[1].setpoint, pid[2].setpoint, pid[3].setpoint,
        v_f[0], v_f[1], v_f[2], v_f[3]);
    }

    vTaskDelayUntil(&xUltimo, xFrec);
  }
}

// ==========================================
// PARSEO DE TRAMA (Core 0)
// ==========================================
void procesarTrama(const String &t) {
  if (t.length() < 2) return;
  char cmd = t.charAt(0);

  if (cmd == 'C') { iniciar_calibracion = true; return; }
  if (cmd == 'S') { for (int i = 0; i < 4; i++) pid[i].reset(); return; }

  if (cmd == 'M' && t.length() > 2) {
    // Formato: M:s1,f1,s2,f2,s3,f3,s4,f4
    // s = -1, 0, o 1   |   f = 0.000–1.000
    int    s[4]; float f[4];
    if (sscanf(t.c_str() + 2,
               "%d,%f,%d,%f,%d,%f,%d,%f",
               &s[0], &f[0], &s[1], &f[1],
               &s[2], &f[2], &s[3], &f[3]) != 8)
      return;

    for (int i = 0; i < 4; i++) {
      f[i] = constrain(f[i], 0.0f, 1.0f);
      // Setpoint en ticks/s con signo aplicado directamente
      pid[i].setpoint = (float)s[i] * f[i] * VEL_MAX_TICKS[i];
    }
    return;
  }
}

// ==========================================
// TAREA: COMUNICACIÓN (Core 0)
// ==========================================
void tareaComunicacion(void *pvParameters) {
  String tUSB = "", tRPI = "";
  unsigned long uEnvio           = 0;
  unsigned long ultimaVezRecibido = millis();

  for (;;) {
    // Telemetría cada 40ms
    if (millis() - uEnvio >= 40) {
      uEnvio = millis();
      long tk[4];
      if (xSemaphoreTake(mutexDatos, pdMS_TO_TICKS(5)) == pdTRUE) {
        for (int i = 0; i < 4; i++) tk[i] = global_ticks[i];
        xSemaphoreGive(mutexDatos);
      }
      char buf[64];
      snprintf(buf, sizeof(buf), "T:%ld,%ld,%ld,%ld\n", tk[0], tk[1], tk[2], tk[3]);
      Serial.print(buf);
      Serial1.print(buf);
    }

    // Lectura USB
    while (Serial.available()) {
      char c = Serial.read();
      if (c == '\n') {
        tUSB.trim();
        if (tUSB.length()) { procesarTrama(tUSB); ultimaVezRecibido = millis(); }
        tUSB = "";
      } else { tUSB += c; }
    }

    // Lectura RPi
    while (Serial1.available()) {
      char c = Serial1.read();
      if (c == '\n') {
        tRPI.trim();
        if (tRPI.length()) { procesarTrama(tRPI); ultimaVezRecibido = millis(); }
        tRPI = "";
      } else { tRPI += c; }
    }

    // Freno de emergencia (ignora calibración en curso)
    // 800ms da margen para lags normales del SO de la RPi
    if (!en_calibracion && millis() - ultimaVezRecibido > 800)
      for (int i = 0; i < 4; i++) pid[i].reset();

    vTaskDelay(pdMS_TO_TICKS(2));
  }
}

// ==========================================
// SETUP
// ==========================================
void setup() {
  Serial.begin(115200);
  Serial1.begin(921600, SERIAL_8N1, PIN_RX_RPI, PIN_TX_RPI);

  pinMode(PIN_STBY_1, OUTPUT); digitalWrite(PIN_STBY_1, HIGH);
  pinMode(PIN_STBY_2, OUTPUT); digitalWrite(PIN_STBY_2, HIGH);

  for (int i = 0; i < 4; i++) {
    pinMode(IN1[i], OUTPUT);
    pinMode(IN2[i], OUTPUT);
    ledcAttach(PWM[i], FREC_PWM, RES_PWM);
    pinMode(ENC[i], INPUT);
  }

  attachInterrupt(digitalPinToInterrupt(ENC[0]), isr_E1, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC[1]), isr_E2, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC[2]), isr_E3, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC[3]), isr_E4, RISING);

  mutexDatos = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(tareaControlMotores, "Ctrl", 4096, NULL, 2, NULL, 1);
  xTaskCreatePinnedToCore(tareaComunicacion,   "Com",  4096, NULL, 1, NULL, 0);
}

void loop() { vTaskDelay(pdMS_TO_TICKS(10)); }