#include <Adafruit_NeoPixel.h>
#include <ESP32Servo.h>

volatile int estadoEmocional = 0; // 0: Normal/Azul, 1: Alerta/Rojo

// --- CONFIGURACIÓN DEL LED ---
#define PIN_LED 13
#define NUMPIXELS 8
Adafruit_NeoPixel anillo(NUMPIXELS, PIN_LED, NEO_GRB + NEO_KHZ800);
TaskHandle_t TareaLED;

// --- CONFIGURACIÓN DE SERVOS ---
Servo mandibula;
Servo ojosX; // Eje X (Izquierda/Derecha)
Servo ojosY; // Eje Y (Arriba/Abajo)

#define PIN_MANDIBULA 4
#define PIN_OJOS_X 5
#define PIN_OJOS_Y 15

int bocaCerrada = 0;
int bocaAbierta = 35;
int centroOjosX = 90; // 90 es mirar al frente
int centroOjosY = 90; // 90 es nivelado

// --- CONFIGURACIÓN SENSOR DISTANCIA ---
#define TRIG_PIN 18
#define ECHO_PIN 19

void setup() {
  Serial.begin(115200);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  anillo.begin();
  anillo.clear();
  anillo.show();

  mandibula.attach(PIN_MANDIBULA, 500, 2400);
  ojosX.attach(PIN_OJOS_X, 500, 2400);
  ojosY.attach(PIN_OJOS_Y, 500, 2400); // Iniciamos el eje Y
  
  mandibula.write(bocaCerrada);
  ojosX.write(centroOjosX);
  ojosY.write(centroOjosY);

  xTaskCreatePinnedToCore(animacionLED, "TareaLED", 10000, NULL, 1, &TareaLED, 0);
}

void loop() {
  long distancia = medirDistancia();

  // REFLEJO: Si hay algo a menos de 10 cm
  if (distancia > 0 && distancia < 10) {
    estadoEmocional = 1; // ROJO
    
    // Movimiento evasivo en DOS ejes
    ojosX.write(135); // Miro hacia un lado
    ojosY.write(45);  // Bajo la mirada (evasión)
    
    simularHabla(6); 
    delay(1000);     
    
  } else {
    estadoEmocional = 0; // AZUL
    ojosX.write(centroOjosX); // Vuelvo al centro X
    ojosY.write(centroOjosY); // Vuelvo al centro Y
  }

  delay(100); 
}

long medirDistancia() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duracion = pulseIn(ECHO_PIN, HIGH);
  return duracion * 0.034 / 2; 
}

void simularHabla(int silabas) {
  for (int i = 0; i < silabas; i++) {
    for (int pos = bocaCerrada; pos <= bocaAbierta; pos += 2) {
      mandibula.write(pos); delay(10);
    }
    for (int pos = bocaAbierta; pos >= bocaCerrada; pos -= 2) {
      mandibula.write(pos); delay(10);
    }
  }
}

void animacionLED( void * pvParameters ) {
  for(;;) {
    if (estadoEmocional == 0) {
      for(int i=0; i<NUMPIXELS; i++) anillo.setPixelColor(i, anillo.Color(0, 50, 255));
      anillo.show();
    } 
    else if (estadoEmocional == 1) {
      for(int i=0; i<NUMPIXELS; i++) anillo.setPixelColor(i, anillo.Color(255, 0, 0));
      anillo.show();
      vTaskDelay(100 / portTICK_PERIOD_MS);
      anillo.clear();
      anillo.show();
      vTaskDelay(100 / portTICK_PERIOD_MS);
    }
    vTaskDelay(50 / portTICK_PERIOD_MS);
  }
}