/*
 * ============================================================================
 * ESP32 CONTROLADOR - 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 actúa como controlador remoto para el robot.
 * Lee 4 encoders rotatorios infinitos y controla los ángulos de los servos
 * de manera relativa (incremento/decremento) vía comunicación Bluetooth Serial.
 * El controlador actúa como SERVIDOR Bluetooth esperando conexión del robot.
 *
 * Hardware requerido:
 * - ESP32 (DevKit V1 o similar)
 * - 4x Encoders rotatorios (con pulsos A y B)
 * - Conexiones: Encoder1(A=GPIO 4, B=GPIO 5), Encoder2(A=12, B=13),
 *               Encoder3(A=14, B=15), Encoder4(A=16, B=17)
 * - Bluetooth activado
 *
 * Licencia: GPL v3 - Uso educativo y académico
 * ============================================================================
 */

#include <ESP32Encoder.h>  // Biblioteca para encoders en ESP32
#include <BluetoothSerial.h>  // Biblioteca para Bluetooth Serial

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

// Pines para encoders (A y B para cada encoder)
#define ENC1_A  4
#define ENC1_B  5
#define ENC2_A  12
#define ENC2_B  13
#define ENC3_A  14
#define ENC3_B  15
#define ENC4_A  16
#define ENC4_B  17

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

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

ESP32Encoder encoder1;
ESP32Encoder encoder2;
ESP32Encoder encoder3;
ESP32Encoder encoder4;

BluetoothSerial btSerial;  // Objeto para Bluetooth Serial

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

// Valores actuales de los encoders (conteos absolutos)
long valorEncoder1 = 0;
long valorEncoder2 = 0;
long valorEncoder3 = 0;
long valorEncoder4 = 0;

// Ángulos actuales de los servos (control relativo)
int anguloBase = 90;     // Ángulo actual base (0-360°)
int anguloHombro = 90;   // Ángulo actual hombro (30-150°)
int anguloCodo = 90;     // Ángulo actual codo (20-160°)
int anguloMuneca = 90;   // Ángulo actual muñeca (0-180°)

// Factores de sensibilidad (cuánto cambia el ángulo por pulso del encoder)
// Para tu encoder: ~80 pulsos por vuelta (calculado de 320°/4)
// Para 1 vuelta = 180°: SENSIBILIDAD = 180 / 80 ≈ 2.25 (usa 2 para ~160°/vuelta)
// Para 1 vuelta = 90°: SENSIBILIDAD = 1
#define SENSIBILIDAD_BASE    2.25   // Ajustado para ~160° por vuelta completa
#define SENSIBILIDAD_HOMBRO  2.25
#define SENSIBILIDAD_CODO    2.25
#define SENSIBILIDAD_MUNECA  2.25

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

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

  // Inicializar Bluetooth Serial como SERVIDOR
  btSerial.begin("GLaDOx_Controller");
  Serial.println("========================================");
  Serial.println("Bluetooth SERVIDOR iniciado: GLaDOx_Controller");
  Serial.println("Esperando conexion del Robot...");
  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);
  }

  // Configurar encoders con full quadrature para mejor estabilidad
  // attachFullQuad cuenta 1 pulso por paso completo (menos sensible a ruido)
  encoder1.attachFullQuad(ENC1_A, ENC1_B);
  encoder2.attachFullQuad(ENC2_A, ENC2_B);
  encoder3.attachFullQuad(ENC3_A, ENC3_B);
  encoder4.attachFullQuad(ENC4_A, ENC4_B);

  // Es crucial para encoders tipo KY-040, que son propensos a jitter (ruido de señal)
  // setFilter(1023) proporciona el efecto máximo de debouncing por hardware
  encoder1.setFilter(1023); 
  encoder2.setFilter(1023);
  encoder3.setFilter(1023);
  encoder4.setFilter(1023);

  // Establecer conteo inicial en 0
  encoder1.setCount(0);
  encoder2.setCount(0);
  encoder3.setCount(0);
  encoder4.setCount(0);

  // Mensaje de inicio
  Serial.println("ESP32 Controlador GLaDOx - Inicializado");
  Serial.println("Control RELATIVO: Gira encoders para cambiar ángulos.");
  Serial.print("Sensibilidad: ~");
  Serial.print(80 * SENSIBILIDAD_BASE);
  Serial.println("° por vuelta completa.");
  Serial.println("Encoders listos para control");
  Serial.println("Esperando que el Robot se conecte...");
}

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

void loop() {
   // Verificar si hay conexión Bluetooth activa
  if (!btSerial.hasClient()) {
    // No hay cliente conectado, indicar con parpadeo rápido
    static unsigned long ultimoParpadeo = 0;
    if (millis() - ultimoParpadeo > 300) {
      digitalWrite(PIN_LED_STATUS, !digitalRead(PIN_LED_STATUS));
      ultimoParpadeo = millis();
    }
    delay(50);
    return;  // No procesar encoders sin conexión
  }
  
  // Indicar conexión activa con LED encendido
  digitalWrite(PIN_LED_STATUS, HIGH);
  
  // Leer valores de encoders
  // Nota: Con fullQuad, se cuenta 1 pulso por paso completo del encoder
  // Esto reduce el bouncing y hace el control más estable
  long nuevoValor1 = encoder1.getCount();
  long nuevoValor2 = encoder2.getCount();
  long nuevoValor3 = encoder3.getCount();
  long nuevoValor4 = encoder4.getCount();

  // Calcular cambios (diferencia desde la última lectura)
  long delta1 = nuevoValor1 - valorEncoder1;
  long delta2 = nuevoValor2 - valorEncoder2;
  long delta3 = nuevoValor3 - valorEncoder3;
  long delta4 = nuevoValor4 - valorEncoder4;

  // Actualizar ángulos con control relativo
  anguloBase += delta1 * SENSIBILIDAD_BASE;
  anguloHombro += delta2 * SENSIBILIDAD_HOMBRO;
  anguloCodo += delta3 * SENSIBILIDAD_CODO;
  anguloMuneca += delta4 * SENSIBILIDAD_MUNECA;

  // Restringir a límites válidos
  anguloBase = constrain(anguloBase, 0, 360);
  anguloHombro = constrain(anguloHombro, 0, 180);
  anguloCodo = constrain(anguloCodo, 0, 180);
  anguloMuneca = constrain(anguloMuneca, 0, 180);

  // Actualizar valores anteriores
  valorEncoder1 = nuevoValor1;
  valorEncoder2 = nuevoValor2;
  valorEncoder3 = nuevoValor3;
  valorEncoder4 = nuevoValor4;

  // Enviar datos si hay cambios significativos (>0 para cualquier cambio)
  if (abs(delta1) > 0 || abs(delta2) > 0 || abs(delta3) > 0 || abs(delta4) > 0) {

    // Formato: B<base>,H<hombro>,C<codo>,M<muneca>
    btSerial.print("B");
    btSerial.print(anguloBase);
    btSerial.print(",H");
    btSerial.print(anguloHombro);
    btSerial.print(",C");
    btSerial.print(anguloCodo);
    btSerial.print(",M");
    btSerial.println(anguloMuneca);

    // Debug por serial
    Serial.print("Enviado: B");
    Serial.print(anguloBase);
    Serial.print(",H");
    Serial.print(anguloHombro);
    Serial.print(",C");
    Serial.print(anguloCodo);
    Serial.print(",M");
    Serial.println(anguloMuneca);

    // Parpadeo LED para indicar envío
    digitalWrite(PIN_LED_STATUS, HIGH);
    delay(50);
    digitalWrite(PIN_LED_STATUS, LOW);
  }

  delay(50);  // Delay reducido para respuesta más rápida
}

// ============================================================================
// FIN DEL CÓDIGO CONTROLADOR
// ============================================================================