/*
 * ============================================================================
 * ESP32 ROBOT - ROBOT EDUCATIVO GLaDOx
 * ============================================================================
 *
 * Proyecto: Plataforma Robótica para Enseñanza de Física en Educación Media
 * Autores: Miguel Angel Luna Garcia, Cristian David Álvarez Cardona
 * Institución: Universidad Tecnológica de Pereira - Ingeniería Mecatrónica
 * Versión: 1.0 (Bluetooth)
 * Fecha: Noviembre 2025
 *
 * Descripción:
 * Este código controla un ESP32 que maneja los servomotores del robot.
 * Busca y se conecta automáticamente al ESP32 controlador vía Bluetooth
 * y mueve los servos a las posiciones especificadas.
 *
 * Hardware requerido:
 * - ESP32 (DevKit V1 o similar)
 * - 4x Servomotores MG995 (11 kg·cm torque)
 * - Fuente de alimentación 5V/3A (para servos)
 * - Conexiones: Base(GPIO4), Hombro(GPIO5), Codo(GPIO18), Muñeca(GPIO19)
 * - Bluetooth activado
 *
 * Licencia: GPL v3 - Uso educativo y académico
 * ============================================================================
 */

#include <ESP32Servo.h>  // Biblioteca para control de servos en ESP32
#include <BluetoothSerial.h>  // Biblioteca para Bluetooth Serial

// ============================================================================
// DEFINICIONES DE HARDWARE
// ============================================================================

// Pines para servomotores
#define PIN_SERVO_BASE     4
#define PIN_SERVO_HOMBRO   5
#define PIN_SERVO_CODO     18
#define PIN_SERVO_MUNECA   19

// LED integrado para indicación
#define PIN_LED_STATUS     2

// Dirección MAC del Controlador (obtenida del escáner BT)
#define MAC_CONTROLADOR "cc:db:a7:3e:da:ee"

// Límites angulares de seguridad
#define LIMITE_BASE_MIN        0
#define LIMITE_BASE_MAX        180
#define LIMITE_HOMBRO_MIN      0
#define LIMITE_HOMBRO_MAX      180
#define LIMITE_CODO_MIN        0
#define LIMITE_CODO_MAX        180
#define LIMITE_MUNECA_MIN      0
#define LIMITE_MUNECA_MAX      180

// Parámetros de movimiento suave (ajusta para más rapidez/suavidad)
#define MOVE_STEP_DEG           4    // Incremento angular por paso
#define MOVE_DELAY_MS           6    // Retardo entre pasos
#define DEAD_BAND_DEG           1    // Umbral para ignorar micro-movimientos

// Posiciones HOME
#define HOME_BASE              90
#define HOME_HOMBRO            90
#define HOME_CODO              90
#define HOME_MUNECA            90

// ============================================================================
// OBJETOS
// ============================================================================

Servo servoBase;
Servo servoHombro;
Servo servoCodo;
Servo servoMuneca;

BluetoothSerial btSerial;  // Objeto para Bluetooth Serial

// ============================================================================
// VARIABLES GLOBALES
// ============================================================================

// Posiciones actuales
int posicionActualBase = HOME_BASE;
int posicionActualHombro = HOME_HOMBRO;
int posicionActualCodo = HOME_CODO;
int posicionActualMuneca = HOME_MUNECA;

// ============================================================================
// FUNCIÓN SETUP
// ============================================================================

void setup() {
  // Inicializar comunicación serial para debugging
  Serial.begin(115200);

  // Inicializar Bluetooth Serial como CLIENTE
  btSerial.begin("GLaDOx_Robot", true);  // true = modo maestro/cliente
  btSerial.setTimeout(50);  // Evita bloqueos largos al leer comandos
  Serial.println("========================================");
  Serial.println("Bluetooth CLIENTE iniciado: GLaDOx_Robot");
  Serial.println("Buscando GLaDOx_Controller...");
  Serial.println("========================================");

  // Configurar LED
  pinMode(PIN_LED_STATUS, OUTPUT);

  // Parpadeo inicial
  for (int i = 0; i < 3; i++) {
    digitalWrite(PIN_LED_STATUS, HIGH);
    delay(200);
    digitalWrite(PIN_LED_STATUS, LOW);
    delay(200);
  }

  // Adjuntar servos con configuración ESP32
  // ESP32Servo permite hasta 16 canales PWM
  servoBase.attach(PIN_SERVO_BASE);
  servoHombro.attach(PIN_SERVO_HOMBRO);
  servoCodo.attach(PIN_SERVO_CODO);
  servoMuneca.attach(PIN_SERVO_MUNECA);

  delay(500);

  // Ir a posición HOME
  moverAPosicionHome();

  // Intentar conectar al controlador usando dirección MAC
  Serial.println("Conectando a GLaDOx_Controller...");
  Serial.print("Direccion MAC: ");
  Serial.println(MAC_CONTROLADOR);
  
  // Convertir string MAC a uint8_t array
  uint8_t macAddress[6];
  sscanf(MAC_CONTROLADOR, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
         &macAddress[0], &macAddress[1], &macAddress[2],
         &macAddress[3], &macAddress[4], &macAddress[5]);
  
  // Reintentar conexión hasta 10 veces
  bool conectado = false;
  for (int intento = 1; intento <= 10 && !conectado; intento++) {
    Serial.print("Intento ");
    Serial.print(intento);
    Serial.print("/10: ");
    
    conectado = btSerial.connect(macAddress);
    
    if (conectado) {
      Serial.println("EXITO!");
      Serial.println("========================================");
      Serial.println("CONECTADO a GLaDOx_Controller!");
      Serial.println("========================================");
      // Parpadeo rápido para indicar conexión exitosa
      for (int i = 0; i < 5; i++) {
        digitalWrite(PIN_LED_STATUS, HIGH);
        delay(100);
        digitalWrite(PIN_LED_STATUS, LOW);
        delay(100);
      }
    } else {
      Serial.println("Fallo.");
      delay(2000);  // Esperar 2s entre intentos
    }
  }
  
  if (!conectado) {
    Serial.println("========================================");
    Serial.println("ERROR: No se pudo conectar despues de 10 intentos.");
    Serial.println("Verifica:");
    Serial.println("1. El Controlador esta encendido");
    Serial.println("2. El Bluetooth del Controlador esta activo");
    Serial.println("3. Ambos ESP32 estan cerca (< 10m)");
    Serial.println("========================================");
    Serial.println("El sistema seguira reintentando...");
  }

  // Mensaje de inicio
  Serial.println("ESP32 Robot GLaDOx - Inicializado");
  Serial.println("Esperando comandos via Bluetooth...");
}

// ============================================================================
// FUNCIÓN LOOP
// ============================================================================

void loop() {
  // Verificar si la conexión Bluetooth está activa
  if (!btSerial.connected()) {
    // Intentar reconectar
    Serial.println("========================================");
    Serial.println("Conexion perdida. Reintentando conexion...");
    Serial.println("========================================");
    digitalWrite(PIN_LED_STATUS, LOW);
    
    // Convertir string MAC a uint8_t array
    uint8_t macAddress[6];
    sscanf(MAC_CONTROLADOR, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
           &macAddress[0], &macAddress[1], &macAddress[2],
           &macAddress[3], &macAddress[4], &macAddress[5]);
    
    // Reintentar 3 veces antes de esperar más tiempo
    bool reconectado = false;
    for (int i = 1; i <= 3 && !reconectado; i++) {
      Serial.print("Intento de reconexion ");
      Serial.print(i);
      Serial.print("/3: ");
      
      reconectado = btSerial.connect(macAddress);
      
      if (reconectado) {
        Serial.println("Reconectado exitosamente!");
        // Parpadeo para indicar reconexión
        for (int j = 0; j < 3; j++) {
          digitalWrite(PIN_LED_STATUS, HIGH);
          delay(100);
          digitalWrite(PIN_LED_STATUS, LOW);
          delay(100);
        }
      } else {
        Serial.println("Fallo.");
        delay(1000);
      }
    }
    
    if (!reconectado) {
      Serial.println("No se pudo reconectar. Reintentando en 10s...");
      delay(10000);  // Esperar 10s antes de reintentar
    }
    return;
  }

  // LED encendido indica conexión activa
  digitalWrite(PIN_LED_STATUS, HIGH);
  
  // Verificar si hay datos Bluetooth
  if (btSerial.available() > 0) {
    procesarComandoBluetooth();
  }

  delay(10);  // Pequeño delay para estabilidad
}

// ============================================================================
// FUNCIONES DE MOVIMIENTO
// ============================================================================

void moverServoSuave(Servo &servo, int posicionFinal, int &posicionActual,
                     int limiteMin, int limiteMax) {
  if (posicionFinal < limiteMin || posicionFinal > limiteMax) {
    Serial.println("ERROR: Posicion fuera de limites");
    return;
  }

  if (abs(posicionFinal - posicionActual) <= DEAD_BAND_DEG) {
    return;  // Movimiento muy pequeño, se omite
  }

  if (posicionActual < posicionFinal) {
    for (int pos = posicionActual; pos <= posicionFinal; pos += MOVE_STEP_DEG) {
      servo.write(pos);
      delay(MOVE_DELAY_MS);
    }
  } else if (posicionActual > posicionFinal) {
    for (int pos = posicionActual; pos >= posicionFinal; pos -= MOVE_STEP_DEG) {
      servo.write(pos);
      delay(MOVE_DELAY_MS);
    }
  }

  servo.write(posicionFinal);
  posicionActual = posicionFinal;
}

void moverAPosicionHome() {
  Serial.println("Moviendo a HOME...");
  moverServoSuave(servoBase, HOME_BASE, posicionActualBase,
                  LIMITE_BASE_MIN, LIMITE_BASE_MAX);
  moverServoSuave(servoHombro, HOME_HOMBRO, posicionActualHombro,
                  LIMITE_HOMBRO_MIN, LIMITE_HOMBRO_MAX);
  moverServoSuave(servoCodo, HOME_CODO, posicionActualCodo,
                  LIMITE_CODO_MIN, LIMITE_CODO_MAX);
  moverServoSuave(servoMuneca, HOME_MUNECA, posicionActualMuneca,
                  LIMITE_MUNECA_MIN, LIMITE_MUNECA_MAX);
  Serial.println("HOME alcanzado");
}

// ============================================================================
// PROCESAMIENTO DE COMANDOS BLUETOOTH
// ============================================================================

void procesarComandoBluetooth() {
  String comando = btSerial.readStringUntil('\n');
  comando.trim();

  if (comando.length() == 0) return;

  Serial.print("Comando recibido via BT: ");
  Serial.println(comando);

  // Parsear formato: B<base>,H<hombro>,C<codo>,M<muneca>
  int bIndex = comando.indexOf('B');
  int hIndex = comando.indexOf('H');
  int cIndex = comando.indexOf('C');
  int mIndex = comando.indexOf('M');

  if (bIndex >= 0 && hIndex > bIndex && cIndex > hIndex && mIndex > cIndex) {
    // Extraer valores
    String baseStr = comando.substring(bIndex + 1, hIndex);
    String hombroStr = comando.substring(hIndex + 1, cIndex);
    String codoStr = comando.substring(cIndex + 1, mIndex);
    String munecaStr = comando.substring(mIndex + 1);

    int base = baseStr.toInt();
    int hombro = hombroStr.toInt();
    int codo = codoStr.toInt();
    int muneca = munecaStr.toInt();

    // Mover servos
    moverServoSuave(servoBase, base, posicionActualBase,
                    LIMITE_BASE_MIN, LIMITE_BASE_MAX);
    moverServoSuave(servoHombro, hombro, posicionActualHombro,
                    LIMITE_HOMBRO_MIN, LIMITE_HOMBRO_MAX);
    moverServoSuave(servoCodo, codo, posicionActualCodo,
                    LIMITE_CODO_MIN, LIMITE_CODO_MAX);
    moverServoSuave(servoMuneca, muneca, posicionActualMuneca,
                    LIMITE_MUNECA_MIN, LIMITE_MUNECA_MAX);

    Serial.println("Movimiento completado");

    // Parpadeo LED
    digitalWrite(PIN_LED_STATUS, HIGH);
    delay(100);
    digitalWrite(PIN_LED_STATUS, LOW);
  } else {
    Serial.println("Formato de comando invalido");
  }
}

// ============================================================================
// FIN DEL CÓDIGO ROBOT
// ============================================================================