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

// --- DECLARACIÓN DE PINES ---

// Servomotores
const int pinSrvBase = 2;
const int pinSrvHombro = 3;
const int pinSrvCodo = 4;
const int pinSrvMuneca = 5;
const int pinSrvRotMuneca = 6;
const int pinSrvTenaza = 7;

// Joysticks (Analógicos y Botones)
const int pinJoy1X = A0; const int pinJoy1Y = A1; const int pinBtnJoy1 = 22;
const int pinJoy2X = A2; const int pinJoy2Y = A3; const int pinBtnJoy2 = 24;
const int pinJoy3X = A4; const int pinJoy3Y = A5; const int pinBtnJoy3 = 26;

// Switches de Seguridad
const int pinSwBase = 28;
const int pinSwTenaza = 30;
const int pinSwMedios = 32;
const int pinSwEmerg = 34;

// LEDs
const int pinLedBaja = 36;
const int pinLedMedia = 38;
const int pinLedAlta = 40;
const int pinLedBase = 42;    // Azul
const int pinLedTenaza = 44;  // Cyan
const int pinLedMedios = 46;  // Blanco
const int pinLedEmergOn = 48; // Rojo
const int pinLedEmergOff = 50;// Verde Claro

// Buzzer y Sensores Ultrasónicos
const int pinBuzzer = 8;
const int pinTrigTenaza = 52; const int pinEchoTenaza = 53;
const int pinTrigEmerg = 10;  const int pinEchoEmerg = 11;

// --- OBJETOS Y VARIABLES GLOBALES ---

Servo srvBase, srvHombro, srvCodo, srvMuneca, srvRotMuneca, srvTenaza;
LiquidCrystal_I2C lcd(0x27, 20, 4); 

// Ángulos actuales de los servos (Inician en 90 grados)
float angBase = 90, angHombro = 90, angCodo = 90;
float angMuneca = 90, angRotMuneca = 90, angTenaza = 90;

// Variables de estado
int velocidad = 2; // 1: Baja, 2: Media, 4: Alta
bool estadoEmergencia = false;
bool obstaculoEmergencia = false;
unsigned long ultimoTiempoUpdate = 0;

void setup() {
  // Inicialización de Pantalla LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Alimentos S.A.");
  lcd.setCursor(0, 1);
  lcd.print("Iniciando sist..");
  delay(2000);
  lcd.clear();

  // Asignación de pines de Servos
  srvBase.attach(pinSrvBase);
  srvHombro.attach(pinSrvHombro);
  srvCodo.attach(pinSrvCodo);
  srvMuneca.attach(pinSrvMuneca);
  srvRotMuneca.attach(pinSrvRotMuneca);
  srvTenaza.attach(pinSrvTenaza);

  // Configuración de Entradas
  pinMode(pinBtnJoy1, INPUT_PULLUP);
  pinMode(pinBtnJoy2, INPUT_PULLUP);
  pinMode(pinBtnJoy3, INPUT_PULLUP);
  
  pinMode(pinSwBase, INPUT);
  pinMode(pinSwTenaza, INPUT);
  pinMode(pinSwMedios, INPUT);
  pinMode(pinSwEmerg, INPUT);

  // Configuración de Salidas (LEDs y Buzzer)
  pinMode(pinLedBaja, OUTPUT); pinMode(pinLedMedia, OUTPUT); pinMode(pinLedAlta, OUTPUT);
  pinMode(pinLedBase, OUTPUT); pinMode(pinLedTenaza, OUTPUT); pinMode(pinLedMedios, OUTPUT);
  pinMode(pinLedEmergOn, OUTPUT); pinMode(pinLedEmergOff, OUTPUT);
  pinMode(pinBuzzer, OUTPUT);

  // Configuración de Sensores Ultrasónicos
  pinMode(pinTrigTenaza, OUTPUT); pinMode(pinEchoTenaza, INPUT);
  pinMode(pinTrigEmerg, OUTPUT); pinMode(pinEchoEmerg, INPUT);

  // Mover a posición inicial
  actualizarServos();
}

void loop() {
  // 1. LECTURA DE SENSORES Y BOTONES
  leerBotonesVelocidad();
  actualizarLedsVelocidad();
  
  long distTenaza = medirDistancia(pinTrigTenaza, pinEchoTenaza);
  long distEmerg = medirDistancia(pinTrigEmerg, pinEchoEmerg);

  // Se considera obstáculo de emergencia si está a menos de 15 cm
  obstaculoEmergencia = (distEmerg > 0 && distEmerg < 15);
  estadoEmergencia = (digitalRead(pinSwEmerg) == HIGH);

  // 2. LÓGICA DE PRIORIDAD (EMERGENCIAS)
  if (estadoEmergencia) {
    // Switch de emergencia activado
    digitalWrite(pinLedEmergOn, HIGH);
    digitalWrite(pinLedEmergOff, LOW);
    digitalWrite(pinBuzzer, HIGH);
    
    // Posiciones fijas requeridas en la rúbrica
    angBase = 120;
    angHombro = 100;
    angCodo = 90;
    angMuneca = 60;
    angRotMuneca = 150;
    angTenaza = 0;
    actualizarServos();
    
    mostrarLCD("EMERGENCIA ACT.", "SISTEMA BLOQUEAD");
  } 
  else if (obstaculoEmergencia) {
    // Sensor ultrasónico de emergencia detecta objeto
    digitalWrite(pinLedEmergOn, HIGH);
    digitalWrite(pinLedEmergOff, LOW);
    digitalWrite(pinBuzzer, HIGH); 
    
    mostrarLCD("ALERTA OBSTACULO", "MOVIMIENTO PAUSA");
  } 
  else {
    // 3. FUNCIONAMIENTO NORMAL
    digitalWrite(pinLedEmergOn, LOW);
    digitalWrite(pinLedEmergOff, HIGH);
    digitalWrite(pinBuzzer, LOW);

    // --- PROCESAMIENTO MATEMÁTICO DE JOYSTICKS ---
    float j1X = (analogRead(pinJoy1X) - 512) / 512.0;
    float j1Y = (analogRead(pinJoy1Y) - 512) / 512.0;
    float j2X = (analogRead(pinJoy2X) - 512) / 512.0;
    float j2Y = (analogRead(pinJoy2Y) - 512) / 512.0;
    float j3X = (analogRead(pinJoy3X) - 512) / 512.0;
    float j3Y = (analogRead(pinJoy3Y) - 512) / 512.0;

    if (fabs(j1X) < 0.12) j1X = 0; if (fabs(j1Y) < 0.12) j1Y = 0;
    if (fabs(j2X) < 0.12) j2X = 0; if (fabs(j2Y) < 0.12) j2Y = 0;
    if (fabs(j3X) < 0.12) j3X = 0; if (fabs(j3Y) < 0.12) j3Y = 0;

    // -- Control de la Base (Primer Grado) --
    if (digitalRead(pinSwBase) == LOW) { // Switch hacia GND = Permite mover
      digitalWrite(pinLedBase, HIGH);
      angBase += j1X * velocidad; 
    } else {
      digitalWrite(pinLedBase, LOW); // Bloqueado
    }

    // -- Control de Grados Medios (2do al 5to) --
    if (digitalRead(pinSwMedios) == LOW) { // Switch hacia GND = Permite mover
      digitalWrite(pinLedMedios, HIGH);
      
      angHombro    += j1Y * velocidad; 
      angCodo      += j2X * velocidad; 
      angMuneca    += j2Y * velocidad; 
      angRotMuneca += j3X * velocidad; 
    } else {
      digitalWrite(pinLedMedios, LOW); // Bloqueado
    }

    // -- Control de la Tenaza (Sexto Grado) --
    if (digitalRead(pinSwTenaza) == LOW) { 
      // Control manual con Joystick Derecho (Eje Y)
      digitalWrite(pinLedTenaza, HIGH);
      angTenaza += j3Y * velocidad;
    } else {
      // Control automático por ultrasonido
      digitalWrite(pinLedTenaza, LOW);
      if (distTenaza > 0 && distTenaza < 10) {
        angTenaza = 180; // Cierra Tenaza
      } else {
        angTenaza = 0;   // Abre Tenaza
      }
    }

    // Restringir rangos físicos seguros de trabajo (0 a 180)
    angBase = constrain(angBase, 0, 180);
    angHombro = constrain(angHombro, 0, 180);
    angCodo = constrain(angCodo, 0, 180);
    angMuneca = constrain(angMuneca, 0, 180);
    angRotMuneca = constrain(angRotMuneca, 0, 180);
    angTenaza = constrain(angTenaza, 0, 180);

    // Enviar las actualizaciones a los actuadores
    actualizarServos();
    
    // Actualizar telemetría normal
    String velStr = (velocidad == 1) ? "Baja" : (velocidad == 2) ? "Med" : "Alta";
    mostrarLCD("Estado: Normal", "Velocidad: " + velStr);
  }
  
  delay(15); 
}

// --- FUNCIONES AUXILIARES ---

void leerBotonesVelocidad() {
  if (digitalRead(pinBtnJoy1) == LOW) { velocidad = 1; }
  if (digitalRead(pinBtnJoy2) == LOW) { velocidad = 2; }
  if (digitalRead(pinBtnJoy3) == LOW) { velocidad = 4; } 
}

void actualizarLedsVelocidad() {
  digitalWrite(pinLedBaja, (velocidad == 1) ? HIGH : LOW);
  digitalWrite(pinLedMedia, (velocidad == 2) ? HIGH : LOW);
  digitalWrite(pinLedAlta, (velocidad == 4) ? HIGH : LOW);
}

void actualizarServos() {
  srvBase.write((int)angBase);
  srvHombro.write((int)angHombro);
  srvCodo.write((int)angCodo);
  srvMuneca.write((int)angMuneca);
  srvRotMuneca.write((int)angRotMuneca);
  srvTenaza.write((int)angTenaza);
}

long medirDistancia(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duracion = pulseIn(echo, HIGH, 30000); 
  if (duracion == 0) return 999; 
  return duracion * 0.034 / 2;
}

void mostrarLCD(String linea1, String linea2) {
  if (millis() - ultimoTiempoUpdate > 250) { 
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print(linea1);
    lcd.setCursor(0, 1);
    lcd.print(linea2);
    ultimoTiempoUpdate = millis();
  }
}