#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>

// ---- LCD I2C ----
LiquidCrystal_I2C lcd(0x27, 16, 2);

// ---- Ultrasonidos ----
#define TRIG_F 5
#define ECHO_F 34
#define TRIG_L 19
#define ECHO_L 35
#define TRIG_R 23
#define ECHO_R 39

// ---- Servos ----
Servo servoBrazo, servoPinza, servoCompuerta, servoSeguro;

#define SERVO_BRAZO     13
#define SERVO_PINZA     12
#define SERVO_COMPUERTA 14
#define SERVO_SEGURO    27

// ---- Configuración de rangos ----
const int RANGO_PICK_MIN = 5;   // cm
const int RANGO_PICK_MAX = 15;  // cm
bool modoAuto = true;

// ---- Función para medir distancia ----
long distCM(int trig, int echo) {
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long d = pulseIn(echo, HIGH, 30000);
  if (d == 0) return -1;
  return (long)(d * 0.034 / 2);
}

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);

  // LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Proyecto LAIKA");

  // Pines ultrasónicos
  pinMode(TRIG_F, OUTPUT); pinMode(ECHO_F, INPUT);
  pinMode(TRIG_L, OUTPUT); pinMode(ECHO_L, INPUT);
  pinMode(TRIG_R, OUTPUT); pinMode(ECHO_R, INPUT);

  // Servos
  servoBrazo.attach(SERVO_BRAZO, 500, 2400);
  servoPinza.attach(SERVO_PINZA, 500, 2400);
  servoCompuerta.attach(SERVO_COMPUERTA, 500, 2400);
  servoSeguro.attach(SERVO_SEGURO, 500, 2400);

  // Posición inicial
  servoBrazo.write(90);
  servoPinza.write(90);
  servoCompuerta.write(0);  // Compuerta cerrada
  servoSeguro.write(0);     // Seguro activado
}

void loop() {
  long df = distCM(TRIG_F, ECHO_F);
  long dl = distCM(TRIG_L, ECHO_L);
  long dr = distCM(TRIG_R, ECHO_R);

  // Mostrar en LCD
  lcd.setCursor(0, 1);
  lcd.printf("F:%3ld L:%3ld R:%3ld", df, dl, dr);

  // Lógica de recolección automática
  if (modoAuto && df >= RANGO_PICK_MIN && df <= RANGO_PICK_MAX) {
    Serial.println("[AUTO] Objeto en rango: PICK");

    // Bajar brazo y cerrar pinza
    servoBrazo.write(120); delay(400);
    servoPinza.write(30);  delay(400);

    // Abrir compuerta y desactivar seguro
    servoSeguro.write(90); delay(300);
    servoCompuerta.write(90); delay(500);

    // Soltar objeto
    servoPinza.write(90); delay(300);

    // Cerrar compuerta y activar seguro
    servoCompuerta.write(0); delay(500);
    servoSeguro.write(0); delay(300);

    // Regresar brazo
    servoBrazo.write(90); delay(300);
  }

  delay(250);
}