#include 
#include 
#include 
#include "BluetoothSerial.h"

// Configuración WiFi
const char* ssid = "TU_RED_WIFI";
const char* password = "TU_PASSWORD";

WebServer server(80);
BluetoothSerial SerialBT;

// Motores DC (L298N)
#define MOTOR_L_PWM 14
#define MOTOR_L_IN1 27
#define MOTOR_L_IN2 26
#define MOTOR_R_PWM 12
#define MOTOR_R_IN1 25
#define MOTOR_R_IN2 33

// Servomotores del brazo
Servo servoBase, servoHombro, servoCodo, servoGripper;
#define SERVO_BASE 18
#define SERVO_HOMBRO 19
#define SERVO_CODO 21
#define SERVO_GRIPPER 22

// Sensores
#define TRIGGER_PIN 5
#define ECHO_PIN 17
#define MPU_SDA 21
#define MPU_SCL 22

struct RobotState {
  int velocidadL = 0, velocidadR = 0;
  int anguloBase = 90, anguloHombro = 90;
  int anguloCodo = 90, anguloGripper = 90;
  float distancia = 0;
  bool autonomous = false;
} robot;

void setup() {
  Serial.begin(115200);
  
  // Inicializar motores
  setupMotors();
  
  // Inicializar servos
  setupServos();
  
  // Inicializar sensores
  setupSensors();
  
  // Inicializar conectividad
  setupWiFi();
  setupBluetooth();
  setupWebServer();
  
  Serial.println("🤖 Robot inicializado correctamente");
}

void setupMotors() {
  pinMode(MOTOR_L_PWM, OUTPUT);
  pinMode(MOTOR_L_IN1, OUTPUT);
  pinMode(MOTOR_L_IN2, OUTPUT);
  pinMode(MOTOR_R_PWM, OUTPUT);
  pinMode(MOTOR_R_IN1, OUTPUT);
  pinMode(MOTOR_R_IN2, OUTPUT);
  
  // Configurar PWM
  ledcSetup(0, 1000, 8);
  ledcSetup(1, 1000, 8);
  ledcAttachPin(MOTOR_L_PWM, 0);
  ledcAttachPin(MOTOR_R_PWM, 1);
}

void setupServos() {
  servoBase.attach(SERVO_BASE);
  servoHombro.attach(SERVO_HOMBRO);
  servoCodo.attach(SERVO_CODO);
  servoGripper.attach(SERVO_GRIPPER);
  
  // Posición inicial
  servoBase.write(robot.anguloBase);
  servoHombro.write(robot.anguloHombro);
  servoCodo.write(robot.anguloCodo);
  servoGripper.write(robot.anguloGripper);
}

void setupSensors() {
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
}

void setupWiFi() {
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Conectando a WiFi...");
  }
  Serial.printf("WiFi conectado. IP: %s\n", WiFi.localIP().toString().c_str());
}

void setupBluetooth() {
  SerialBT.begin("ESP32_Robot");
  Serial.println("Bluetooth iniciado como 'ESP32_Robot'");
}

void setupWebServer() {
  server.on("/", handleRoot);
  server.on("/move", handleMove);
  server.on("/arm", handleArm);
  server.on("/status", handleStatus);
  server.begin();
}

void controlMotor(int motor, int velocidad, bool direccion) {
  int pwmPin = (motor == 0) ? 0 : 1;  // Canal PWM
  int in1 = (motor == 0) ? MOTOR_L_IN1 : MOTOR_R_IN1;
  int in2 = (motor == 0) ? MOTOR_L_IN2 : MOTOR_R_IN2;
  
  digitalWrite(in1, direccion ? HIGH : LOW);
  digitalWrite(in2, direccion ? LOW : HIGH);
  ledcWrite(pwmPin, abs(velocidad));
}

void moverRobot(int velIzq, int velDer) {
  robot.velocidadL = velIzq;
  robot.velocidadR = velDer;
  
  controlMotor(0, velIzq, velIzq >= 0);
  controlMotor(1, velDer, velDer >= 0);
}

float leerDistancia() {
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);
  
  long duracion = pulseIn(ECHO_PIN, HIGH);
  float distancia = (duracion * 0.034) / 2;
  
  return distancia;
}

void moverBrazo(int base, int hombro, int codo, int gripper) {
  if (base >= 0 && base <= 180) {
    robot.anguloBase = base;
    servoBase.write(base);
  }
  if (hombro >= 0 && hombro <= 180) {
    robot.anguloHombro = hombro;
    servoHombro.write(hombro);
  }
  if (codo >= 0 && codo <= 180) {
    robot.anguloCodo = codo;
    servoCodo.write(codo);
  }
  if (gripper >= 0 && gripper <= 180) {
    robot.anguloGripper = gripper;
    servoGripper.write(gripper);
  }
}

void loop() {
  server.handleClient();
  
  // Leer sensores
  robot.distancia = leerDistancia();
  
  // Procesar comandos Bluetooth
  if (SerialBT.available()) {
    String comando = SerialBT.readString();
    procesarComando(comando);
  }
  
  // Modo autónomo simple
  if (robot.autonomous) {
    navegacionAutonoma();
  }
  
  delay(50);
}

void procesarComando(String cmd) {
  cmd.trim();
  
  if (cmd.startsWith("MOVE")) {
    // MOVE:velocidadIzq,velocidadDer
    int idx = cmd.indexOf(':');
    if (idx > 0) {
      String params = cmd.substring(idx + 1);
      int comma = params.indexOf(',');
      if (comma > 0) {
        int velL = params.substring(0, comma).toInt();
        int velR = params.substring(comma + 1).toInt();
        moverRobot(velL, velR);
      }
    }
  }
  else if (cmd.startsWith("ARM")) {
    // ARM:base,hombro,codo,gripper
    int idx = cmd.indexOf(':');
    if (idx > 0) {
      String params = cmd.substring(idx + 1);
      // Parsear 4 ángulos separados por comas
      // (implementación completa requiere parsing más robusto)
    }
  }
  else if (cmd == "AUTO") {
    robot.autonomous = !robot.autonomous;
    SerialBT.printf("Modo autónomo: %s\n", robot.autonomous ? "ON" : "OFF");
  }
}

void navegacionAutonoma() {
  if (robot.distancia < 20) {
    // Obstáculo detectado - girar
    moverRobot(-150, 150);  // Girar a la derecha
    delay(500);
  } else {
    // Avanzar
    moverRobot(150, 150);
  }
}

// Handlers del servidor web
void handleRoot() {
  String html = R"(
  
  
  
  
    
🤖 Control de Robot
    

      Adelante
      Atrás
      Izquierda
      Derecha
      Parar
    
    
    
  
  
  )";
  server.send(200, "text/html", html);
}

void handleMove() {
  int velL = server.arg("l").toInt();
  int velR = server.arg("r").toInt();
  moverRobot(velL, velR);
  server.send(200, "text/plain", "OK");
}

void handleArm() {
  // Implementar control del brazo vía web
  server.send(200, "text/plain", "Arm control");
}

void handleStatus() {
  String json = "{\"distance\":" + String(robot.distancia) + 
                ",\"motorL\":" + String(robot.velocidadL) +
                ",\"motorR\":" + String(robot.velocidadR) + "}";
  server.send(200, "application/json", json);
}