#include <Arduino.h>
#include <WiFi.h>
#include <WebSocketsServer.h>

// ===== Credenciales Wi-Fi (MODIFICAR) =====
const char* ssid = "TU_REDU_WIFI";
const char* password = "TU_CONTRASENA";

// ===== Servidor WebSocket =====
WebSocketsServer webSocket(81); // Puerto 81 para WS

// ===== Pines de Hardware =====
const int IN1 = 13; const int IN2 = 14;
const int IN3 = 27; const int IN4 = 26;
const int ENA = 25; const int ENB = 33;

const int freqPWM = 1000;
const int resolucionPWM = 8;
const int velocidadBase = 180; 

#define ENC_IZQ_C1 18
#define ENC_IZQ_C2 19
#define ENC_DER_C1 32
#define ENC_DER_C2 23

const int TRIG_IZQ = 5;  const int ECHO_IZQ = 4;
const int TRIG_DER = 21; const int ECHO_DER = 22;

// ===== Variables Compartidas y de Control IoT =====
volatile long pulsosIzq = 0;
volatile long pulsosDer = 0;
const int pulsosPorVuelta = 200; 
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;

volatile float distanciaIzqActual = 999.0;
volatile float distanciaDerActual = 999.0;

float distanciaUmbral = 25.0; 
bool exploracionActiva = false;
unsigned long ultimoEnvioWS = 0;

bool ejecutandoManiobraEvasion = false;
long pulsosEvasionObjetivo = 0;

// ===== Geometría y Máquina de Estados =====
const float diametroRueda = 4.5; 
const float distanciaEntreRuedas = 18.0; 
const float circunferenciaRueda = PI * diametroRueda; 

enum Movimiento { ADELANTE, DERECHA, IZQUIERDA, DETENIDO };

struct Comando {
  Movimiento tipo;
  long pulsosObjetivo;
};

long calcularPulsosGiro(float grados) {
  float arco = (PI * distanciaEntreRuedas) * (grados / 360.0); 
  return (long)((arco / circunferenciaRueda) * pulsosPorVuelta);
}

Comando rutina[] = {
  {ADELANTE,  (long)((20.0 / (PI * 4.5)) * 200)}, 
  {DERECHA,   calcularPulsosGiro(90.0)}, 
  {ADELANTE,  (long)((20.0 / (PI * 4.5)) * 200)}, 
  {IZQUIERDA, calcularPulsosGiro(90.0)}  
};

const int totalPasos = sizeof(rutina) / sizeof(rutina[0]);
int indiceRutina = 0;

// ===== Tarea FreeRTOS (Lectura Asíncrona de Sensores, Núcleo 0) =====
void tareaSensoresUltrasonicos(void *pvParameters) {
  pinMode(TRIG_IZQ, OUTPUT); pinMode(ECHO_IZQ, INPUT);
  pinMode(TRIG_DER, OUTPUT); pinMode(ECHO_DER, INPUT);
  const unsigned long timeoutUS = 20000; 

  while (true) {
    digitalWrite(TRIG_IZQ, LOW); delayMicroseconds(2);
    digitalWrite(TRIG_IZQ, HIGH); delayMicroseconds(10);
    digitalWrite(TRIG_IZQ, LOW);
    long duracionIzq = pulseIn(ECHO_IZQ, HIGH, timeoutUS);
    distanciaIzqActual = (duracionIzq == 0) ? 999.0 : duracionIzq * 0.034 / 2.0;
    vTaskDelay(pdMS_TO_TICKS(30)); 

    digitalWrite(TRIG_DER, LOW); delayMicroseconds(2);
    digitalWrite(TRIG_DER, HIGH); delayMicroseconds(10);
    digitalWrite(TRIG_DER, LOW);
    long duracionDer = pulseIn(ECHO_DER, HIGH, timeoutUS);
    distanciaDerActual = (duracionDer == 0) ? 999.0 : duracionDer * 0.034 / 2.0;
    vTaskDelay(pdMS_TO_TICKS(30)); 
  }
}

// ===== Interrupciones =====
void IRAM_ATTR isrEncoderIzq() {
  portENTER_CRITICAL_ISR(&mux);
  if (digitalRead(ENC_IZQ_C2) == HIGH) pulsosIzq++; else pulsosIzq--;
  portEXIT_CRITICAL_ISR(&mux);
}

void IRAM_ATTR isrEncoderDer() {
  portENTER_CRITICAL_ISR(&mux);
  if (digitalRead(ENC_DER_C2) == HIGH) pulsosDer++; else pulsosDer--;
  portEXIT_CRITICAL_ISR(&mux);
}

void resetEncoders() {
  portENTER_CRITICAL(&mux);
  pulsosIzq = 0; pulsosDer = 0;
  portEXIT_CRITICAL(&mux);
}

void aplicarMovimiento(Movimiento mov) {
  switch(mov) {
    case ADELANTE:
      digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
      digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
      break;
    case DERECHA:
      digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
      digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
      break;
    case IZQUIERDA:
      digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);
      digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
      break;
    case DETENIDO:
      digitalWrite(IN1, HIGH); digitalWrite(IN2, HIGH);
      digitalWrite(IN3, HIGH); digitalWrite(IN4, HIGH);
      ledcWrite(ENA, 0); ledcWrite(ENB, 0);
      break;
  }
}

// ===== Control de Eventos WebSocket =====
void eventoWebSocket(uint8_t num, WStype_t type, uint8_t * payload, size_t length) {
  if (type == WStype_TEXT) {
    String mensaje = (char*)payload;
    if (mensaje == "START") {
      exploracionActiva = true;
      resetEncoders();
      aplicarMovimiento(rutina[indiceRutina].tipo);
      Serial.println("Comando WS: INICIAR");
    } 
    else if (mensaje == "STOP") {
      exploracionActiva = false;
      ejecutandoManiobraEvasion = false;
      aplicarMovimiento(DETENIDO);
      Serial.println("Comando WS: DETENER");
    }
    else if (mensaje.startsWith("THRESH:")) {
      String valor = mensaje.substring(7);
      distanciaUmbral = valor.toFloat();
      Serial.print("Comando WS: Umbral = "); Serial.println(distanciaUmbral);
    }
  }
}

// ===== Motor Principal de Navegación (Núcleo 1) =====
void actualizarNavegacion() {
  if (!exploracionActiva) return; 

  // 1. MANIOBRA DE EVASIÓN
  if (ejecutandoManiobraEvasion) {
    long pIzq, pDer;
    portENTER_CRITICAL(&mux); pIzq = abs(pulsosIzq); pDer = abs(pulsosDer); portEXIT_CRITICAL(&mux);

    if (pIzq >= pulsosEvasionObjetivo || pDer >= pulsosEvasionObjetivo) {
      ejecutandoManiobraEvasion = false;
      resetEncoders();
      aplicarMovimiento(rutina[indiceRutina].tipo);
    } else {
      ledcWrite(ENA, velocidadBase); ledcWrite(ENB, velocidadBase);
    }
    return; 
  }

  // 2. DETECCIÓN REACTIVA
  if (distanciaIzqActual < distanciaUmbral || distanciaDerActual < distanciaUmbral) {
    ejecutandoManiobraEvasion = true;
    pulsosEvasionObjetivo = calcularPulsosGiro(35.0); 
    
    if (distanciaIzqActual < distanciaUmbral && distanciaDerActual >= distanciaUmbral) {
      aplicarMovimiento(DERECHA); 
    } else if (distanciaDerActual < distanciaUmbral && distanciaIzqActual >= distanciaUmbral) {
      aplicarMovimiento(IZQUIERDA); 
    } else {
      aplicarMovimiento(DERECHA); 
    }
    resetEncoders();
    return; 
  }

  // 3. CAPA DELIBERATIVA
  Comando cmdActual = rutina[indiceRutina];
  long pIzq, pDer;
  portENTER_CRITICAL(&mux); pIzq = abs(pulsosIzq); pDer = abs(pulsosDer); portEXIT_CRITICAL(&mux);

  if (pIzq >= cmdActual.pulsosObjetivo || pDer >= cmdActual.pulsosObjetivo) {
    indiceRutina = (indiceRutina + 1) % totalPasos;
    resetEncoders();
    aplicarMovimiento(rutina[indiceRutina].tipo);
    return; 
  }

  if (cmdActual.tipo == ADELANTE) {
    float Kp = 1.5; 
    long error = pIzq - pDer;
    int ajuste = error * Kp;
    ledcWrite(ENA, constrain(velocidadBase - ajuste, 0, 255));
    ledcWrite(ENB, constrain(velocidadBase + ajuste, 0, 255));
  } else {
    ledcWrite(ENA, velocidadBase); ledcWrite(ENB, velocidadBase);
  }
}

// ===== Inicialización =====
void setup() {
  Serial.begin(115200);

  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  ledcAttach(ENA, freqPWM, resolucionPWM); ledcAttach(ENB, freqPWM, resolucionPWM);
  aplicarMovimiento(DETENIDO);
  
  pinMode(ENC_IZQ_C1, INPUT_PULLUP); pinMode(ENC_IZQ_C2, INPUT_PULLUP);
  pinMode(ENC_DER_C1, INPUT_PULLUP); pinMode(ENC_DER_C2, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC_IZQ_C1), isrEncoderIzq, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_DER_C1), isrEncoderDer, RISING);

  // Iniciar WiFi
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  Serial.print("Conectando a WiFi");
  while (WiFi.status() != WL_CONNECTED) { delay(500); Serial.print("."); }
  Serial.println("\n--- ROBOT EN LINEA ---"); 
  Serial.print("IP ASIGNADA: "); 
  Serial.println(WiFi.localIP());

  // Iniciar WebSockets
  webSocket.begin();
  webSocket.onEvent(eventoWebSocket);

  // Tarea de Sensores en Núcleo 0
  xTaskCreatePinnedToCore(tareaSensoresUltrasonicos, "SensoresUS", 2048, NULL, 1, NULL, 0);
}

// ===== Loop Principal (Núcleo 1) =====
void loop() {
  webSocket.loop();
  actualizarNavegacion();

  // Enviar Telemetría (No bloqueante, cada 150ms)
  if (millis() - ultimoEnvioWS > 150) {
    ultimoEnvioWS = millis();
    
    String json = "{\"izq\":";
    json += (distanciaIzqActual == 999.0) ? "\"Fuera de rango\"" : String(distanciaIzqActual, 1);
    json += ",\"der\":";
    json += (distanciaDerActual == 999.0) ? "\"Fuera de rango\"" : String(distanciaDerActual, 1);
    json += ",\"evadiendo\":";
    json += ejecutandoManiobraEvasion ? "true" : "false";
    json += "}";

    webSocket.broadcastTXT(json);
  }
}