#include <ESP32Servo.h>

#define TRIG     26
#define ECHO     34

#define SERVO1   15
#define SERVO2   18

#define B_ON     32 
#define B_OFF    25  
#define B_TAM    33  
#define B_CAL    14
#define B_SEL    12 
#define B_ACEPT  13  

#define MOTOR    27

#define LED_MAX1 5
#define LED_MIN2 16
#define LED_MAX2 17
#define LED_SET  2

float distance;
bool sistemaEncendido = false;
bool modoTamano = false;
bool modoCalibracion = false;

int conteoTotal = 0;
int conteoServo1 = 0;
int conteoServo2 = 0;

Servo servo1;
Servo servo2;

bool botonOnPrev    = HIGH, botonOffPrev = HIGH, botonTamPrev = HIGH;
bool botonCalPrev   = HIGH;
bool botonSelPrev   = HIGH;
bool botonAceptPrev = HIGH;

unsigned long lastButtonTime = 0;
const unsigned long debounceDelay = 150; 

float min1 = 2.1, max1 = 2.97;
float min2 = 5.06, max2 = 6;

float leerDistancia() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  long duracion = pulseIn(ECHO, HIGH, 30000);
  return duracion * 0.0343 / 2;
}

void mostrarSeleccion(int index) {
  switch (index) {
    case 0: Serial.println("Calibrando mínimo 1"); break;
    case 1: Serial.println("Calibrando máximo 1"); break;
    case 2: Serial.println("Calibrando mínimo 2"); break;
    case 3: Serial.println("Calibrando máximo 2"); break;
  }
}

void mostrarValoresEstado() {
  Serial.print("min1: "); Serial.println(min1, 2);
  Serial.print("max1: "); Serial.println(max1, 2);
  Serial.print("min2: "); Serial.println(min2, 2);
  Serial.print("max2: "); Serial.println(max2, 2);
}

void modoCalibracionLoop() {
  static bool seleccionDesbloqueada = false;
  static int seleccionIndex = -1;
  static unsigned long lastShowTime = 0;
  const unsigned long showInterval = 1000; 

  static unsigned long ledSetTime = 0;
  static bool ledSetActive = false;

  bool botonSel   = digitalRead(B_SEL);
  bool botonAcept = digitalRead(B_ACEPT);

  digitalWrite(LED_MAX1, LOW);
  digitalWrite(LED_MIN2, LOW);
  digitalWrite(LED_MAX2, LOW);

  if (seleccionDesbloqueada) {
    if (seleccionIndex == 1)      digitalWrite(LED_MAX1, HIGH);      
    else if (seleccionIndex == 2) digitalWrite(LED_MIN2, HIGH);
    else if (seleccionIndex == 3) digitalWrite(LED_MAX2, HIGH); 
    // min1 no prende ningún LED
  }

  if (!seleccionDesbloqueada) {
    if (botonSel == LOW && botonSelPrev == HIGH) {
      seleccionDesbloqueada = true;
      seleccionIndex = 0;
      mostrarSeleccion(seleccionIndex);
      lastShowTime = millis();
      delay(200);
    }
    botonSelPrev = botonSel;
    return;
  }

  if (botonSel == LOW && botonSelPrev == HIGH) {
    seleccionIndex = (seleccionIndex + 1) % 4;
    mostrarSeleccion(seleccionIndex);
    lastShowTime = millis();
    delay(200);
  }
  botonSelPrev = botonSel;

  if (millis() - lastShowTime > showInterval) {
    mostrarValoresEstado();
    lastShowTime = millis();
  }

  if (botonAcept == LOW && botonAceptPrev == HIGH) {
    float resultado = leerDistancia();
    switch (seleccionIndex) {
      case 0: min1 = resultado; break;
      case 1: max1 = resultado; break;
      case 2: min2 = resultado; break;
      case 3: max2 = resultado; break;
    }
    Serial.print("Valor calibrado: "); Serial.println(resultado, 2);
    mostrarValoresEstado();

    digitalWrite(LED_SET, HIGH);
    ledSetActive = true;
    ledSetTime = millis();
    delay(300);
  }
  botonAceptPrev = botonAcept;

  if (ledSetActive && millis() - ledSetTime > 100) {
    digitalWrite(LED_SET, LOW);
    ledSetActive = false;
  }
}

void procesarTamano() {
  distance = leerDistancia();

  Serial.print("Distancia: ");
  Serial.print(distance, 2);
  Serial.println(" cm");

  if (distance >= min1 && distance <= max1) {
    servo1.write(52);
    servo2.write(0);
    conteoServo1++;
    conteoTotal++;
    Serial.print("Objeto de rango 1 detectado. Conteo: ");
    Serial.println(conteoServo1);
    delay(3000);
    servo1.write(0);
  } else if (distance >= min2 && distance <= max2) {
    servo1.write(0);
    servo2.write(130);
    conteoServo2++;
    conteoTotal++;
    Serial.print("Objeto de rango 2 detectado. Conteo: ");
    Serial.println(conteoServo2);
    delay(4000);
    servo2.write(0);
  } else {
    servo1.write(0);
    servo2.write(0);
  }
}

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

  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  pinMode(B_ON,    INPUT_PULLUP);
  pinMode(B_OFF,   INPUT_PULLUP);
  pinMode(B_TAM,   INPUT_PULLUP);
  pinMode(B_CAL,   INPUT_PULLUP);
  pinMode(B_SEL,   INPUT_PULLUP);
  pinMode(B_ACEPT, INPUT_PULLUP);

  pinMode(MOTOR, OUTPUT);
  digitalWrite(MOTOR, LOW); 

  pinMode(LED_MAX1, OUTPUT);
  pinMode(LED_MIN2, OUTPUT);
  pinMode(LED_MAX2, OUTPUT);
  pinMode(LED_SET,  OUTPUT);
  digitalWrite(LED_MAX1, LOW);
  digitalWrite(LED_MIN2, LOW);
  digitalWrite(LED_MAX2, LOW);
  digitalWrite(LED_SET,  LOW);

  servo1.attach(SERVO1);
  servo2.attach(SERVO2);
  servo1.write(0);
  servo2.write(0);

  Serial.println("Sistema listo. Presiona ON para iniciar.");
}

void loop() {
  bool botonOn    = digitalRead(B_ON);
  bool botonOff   = digitalRead(B_OFF);
  bool botonTam   = digitalRead(B_TAM);
  bool botonCal   = digitalRead(B_CAL);

  unsigned long now = millis();

  // Encender sistema
  if (botonOn == LOW && botonOnPrev == HIGH && now - lastButtonTime > debounceDelay) {
    sistemaEncendido = true;
    Serial.println("Sistema ENCENDIDO");
    lastButtonTime = now;
  }
  botonOnPrev = botonOn;

  // Apagar sistema
  if (botonOff == LOW && botonOffPrev == HIGH && now - lastButtonTime > debounceDelay) {
    sistemaEncendido = false;
    modoTamano = false;
    modoCalibracion = false;
    Serial.println("Sistema APAGADO");
    servo1.write(0);
    servo2.write(0);
    digitalWrite(MOTOR, LOW); 
    digitalWrite(LED_MAX1, LOW);
    digitalWrite(LED_MIN2, LOW);
    digitalWrite(LED_MAX2, LOW);
    digitalWrite(LED_SET,  LOW);
    lastButtonTime = now;
  }
  botonOffPrev = botonOff;

  if (sistemaEncendido) {
    // Activar/desactivar modo calibración
    if (botonCal == LOW && botonCalPrev == HIGH && now - lastButtonTime > debounceDelay) {
      modoCalibracion = !modoCalibracion;
      modoTamano = false;
      Serial.println(modoCalibracion ? "Modo Calibración ACTIVADO. Presiona SEL para elegir qué calibrar." : "Modo Calibración DESACTIVADO.");
      digitalWrite(LED_MAX1, LOW);
      digitalWrite(LED_MIN2, LOW);
      digitalWrite(LED_MAX2, LOW);
      digitalWrite(LED_SET,  LOW);
      lastButtonTime = now;
    }
    botonCalPrev = botonCal;

    if (modoCalibracion) {
      digitalWrite(MOTOR, LOW); 
      modoCalibracionLoop();    
    } else {
      digitalWrite(MOTOR, HIGH);

      if (botonTam == LOW && botonTamPrev == HIGH && now - lastButtonTime > debounceDelay) {
        modoTamano = !modoTamano; 
        Serial.print("Modo Tamaño ");
        Serial.println(modoTamano ? "ACTIVADO" : "DESACTIVADO");
        lastButtonTime = now;
      }
      botonTamPrev = botonTam;

      if (modoTamano) {
        procesarTamano();
      }
    }
  } else {
    digitalWrite(MOTOR, LOW); 
    servo1.write(0);
    servo2.write(0);
    digitalWrite(LED_MAX1, LOW);
    digitalWrite(LED_MIN2, LOW);
    digitalWrite(LED_MAX2, LOW);
    digitalWrite(LED_SET,  LOW);
  }

  delay(20);
}