#include 
#include 
#include 

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

// Definición de pines
#define JOYSTICK_X_PIN 36
#define JOYSTICK_Y_PIN 39
#define JOYSTICK_BTN_PIN 34
#define ULTRASONIC_TRIG_PIN 5
#define ULTRASONIC_ECHO_PIN 18

// Servomotores del brazo robótico
Servo servoBase, servoShoulder, servoElbow;
Servo servoWrist, servoGripper, servoRotation;

// Variables de control
struct RobotPosition {
    int base = 90, shoulder = 90, elbow = 90;
    int wrist = 90, gripper = 0, rotation = 90;
};

RobotPosition currentPos, targetPos;
WebServer server(80);

// Función para leer distancia ultrasónica
float readDistance() {
    digitalWrite(ULTRASONIC_TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(ULTRASONIC_TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(ULTRASONIC_TRIG_PIN, LOW);
    
    long duration = pulseIn(ULTRASONIC_ECHO_PIN, HIGH);
    return (duration * 0.034) / 2;
}

// Control suave de servomotores
void smoothMove() {
    currentPos.base += (targetPos.base - currentPos.base) * 0.1;
    currentPos.shoulder += (targetPos.shoulder - currentPos.shoulder) * 0.1;
    currentPos.elbow += (targetPos.elbow - currentPos.elbow) * 0.1;
    
    servoBase.write(currentPos.base);
    servoShoulder.write(currentPos.shoulder);
    servoElbow.write(currentPos.elbow);
}

// Página web de control
void handleRoot() {
    String html = "";
    html += "
Control del Brazo Robótico
";
    html += "
Posición Base: " + String(currentPos.base) + "°
";
    html += "
Distancia: " + String(readDistance()) + " cm
";
    html += "";
    server.send(200, "text/html", html);
}

void setup() {
    Serial.begin(115200);
    
    // Configurar servos
    servoBase.attach(13);
    servoShoulder.attach(12);
    servoElbow.attach(14);
    servoWrist.attach(27);
    servoGripper.attach(26);
    servoRotation.attach(25);
    
    // Configurar pines de sensor
    pinMode(ULTRASONIC_TRIG_PIN, OUTPUT);
    pinMode(ULTRASONIC_ECHO_PIN, INPUT);
    pinMode(JOYSTICK_BTN_PIN, INPUT_PULLUP);
    
    // Conectar WiFi
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) {
        delay(1000);
        Serial.println("Conectando a WiFi...");
    }
    
    Serial.println("IP: " + WiFi.localIP().toString());
    
    // Configurar servidor web
    server.on("/", handleRoot);
    server.begin();
    
    // Posición inicial
    smoothMove();
}

void loop() {
    server.handleClient();
    
    // Leer joystick
    int joyX = analogRead(JOYSTICK_X_PIN);
    int joyY = analogRead(JOYSTICK_Y_PIN);
    bool joyBtn = !digitalRead(JOYSTICK_BTN_PIN);
    
    // Mapear a ángulos de servo
    targetPos.base = map(joyX, 0, 4095, 0, 180);
    targetPos.shoulder = map(joyY, 0, 4095, 0, 180);
    
    // Control de gripper con botón
    if (joyBtn) {
        targetPos.gripper = 180; // Cerrar
    } else {
        targetPos.gripper = 0;   // Abrir
    }
    
    // Verificar obstáculos
    float distance = readDistance();
    if (distance < 10) { // Si hay obstáculo < 10cm
        Serial.println("¡OBSTÁCULO DETECTADO!");
        // Detener movimiento hacia adelante
    } else {
        smoothMove();
    }
    
    // Debug
    Serial.printf("Joy: X=%d Y=%d Btn=%d | Dist=%.1fcm\n", 
                  joyX, joyY, joyBtn, distance);
    
    delay(50);
}