#include <ESP32Servo.h>
//pines del puente H

//Ruedas delanteras
int entrada1 = 27;
int entrada2 = 26;

//Ruedas traseras
int entrada3 = 25;
int entrada4 = 33;

// Pines del sensor de ultrasonido
const int echoPin = 23;
const int trigPin = 22;

// Pines de los servos
const int servoPin1 = 19;
const int servoPin2 = 18;

// Variables para el sensor de ultrasonido
float duracion, cm;
float vel_sonido = 340.0;

// Crear objetos servo
Servo myservo1;
Servo myservo2;

// Distancia mínima para detener los servos (en cm)
const int distanciaDeteccion = 10;

// Variables para el temporizador
unsigned long tiempoUltimaDeteccion = 0;
unsigned long tiempoRueda = 0;
unsigned long tiempoActualRueda = 0;
const unsigned long tiempoEsperaDeteccion = 1000; // 1 segundo en milisegundos
const unsigned long tiempoMovimiento = 10000; // 3 segundos en milisegundos
const unsigned long tiempoRetrasoSegundoServo = 1000; // 1 segundo en milisegundos
const unsigned long tiempoAdelanteActivo = 500;
bool objetoDetectado = false;
bool moviendoA150 = false;
bool moviendoA50 = false;
bool ifServo2 = false;
bool AdelanteRecoger = false;
bool RetrocederRecoger = false;

unsigned long tiempoInicioMovimiento = 0;
unsigned long tiempoInicioSegundoServo = 0;
const unsigned long tiempoCambioPosicion2 = 3000; // Tiempo en milisegundos que tarda el segundo servo en volver a la posición inicial

// Variables para almacenar la posición de los servos
int servoPos1 = 150; // Posición inicial del primer servo
int servoPos2 = 80;  // Posición inicial del segundo servo
int direction = 2;   // Dirección del movimiento: 1 (incrementar), -1 (decrementar)
const int limiteSuperior = 160;
const int limiteInferior = 50;
const int limiteRecogerR = 104;
const int limiteRecogerL = 94;

int servo1Info = 0;

void setup() {
  Serial.begin(9600); // Iniciar comunicación serie
  
  //Pines Puente H
  pinMode(entrada1, OUTPUT); 
  pinMode(entrada2, OUTPUT);
  pinMode(entrada3, OUTPUT); 
  pinMode(entrada4, OUTPUT);


  // Configurar pines del sensor de ultrasonido
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  // Adjuntar los servos a los pines especificados
  myservo1.attach(servoPin1);
  myservo2.attach(servoPin2);
  
  // Posiciones iniciales de los servos
  myservo1.write(servoPos1);
  myservo2.write(servoPos2);
}

void loop() {
  // Enviar pulso al sensor de ultrasonido
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Leer la duración del pulso de retorno
  duracion = pulseIn(echoPin, HIGH);
  
  // Calcular la distancia en cm
  cm = (duracion / 2) * (vel_sonido / 10000);
  
  // Imprimir la distancia en el monitor serie
  // Serial.print(cm, 0);
  // Serial.println(" cm");
  
  // Comprobar si se detecta un objeto


  if (cm <= distanciaDeteccion) {

    servo1Info = myservo1.read();
    Serial.println(servo1Info);
    if (servo1Info < limiteRecogerR && servo1Info > limiteRecogerL && !objetoDetectado) {
      objetoDetectado = true;
      tiempoUltimaDeteccion = millis(); // Marcar el tiempo de detección
      Serial.println("Objeto detectado.");
    
    }
  }

  if (objetoDetectado) {
    unsigned long tiempoActual = millis();
    
    // Esperar 1 segundo después de la detección
    if (tiempoActual - tiempoUltimaDeteccion < tiempoEsperaDeteccion) {
      // No hacer nada durante la espera
      
      Serial.println( objetoDetectado);
      
    } else if (!moviendoA150) {
      // Mover el primer servo a 150 grados y esperar 3 segundos
      moviendoA150 = true;
      tiempoInicioMovimiento = tiempoActual;
      myservo1.write(150); // Mover el primer servo a 150 grados
      Serial.println("Primer servo movido a 150 grados.");

      ifServo2 = true;
      // Activar el segundo servo un segundo después
      
    } else if (tiempoActual - tiempoInicioMovimiento < tiempoMovimiento) {
      // Esperar 3 segundos mientras el primer servo está en 150 grados
    } else {
      // Reiniciar el ciclo de movimiento del primer servo desde 150 grados
      moviendoA150 = false;
      servoPos1 = 150; // Reiniciar la posición inicial del primer servo
      myservo1.write(servoPos1);
      Serial.println("Inicio del ciclo de movimiento del primer servo desde 150 grados.");

      // Mover el segundo servo de vuelta a 90 grados después de 3 segundos
      if (moviendoA50 && tiempoActual - tiempoInicioSegundoServo >= tiempoCambioPosicion2) {
        myservo2.write(90);
        moviendoA50 = false;
        Serial.println("Segundo servo movido de vuelta a 90 grados.");
      }

      objetoDetectado = false; // Permitir nueva detección
    }
    if ( ifServo2 == true && tiempoActual - tiempoInicioMovimiento >= tiempoRetrasoSegundoServo) {
        moviendoA50 = true;
        myservo2.write(5); // Mover el segundo servo a 50 grados
        Serial.println("Segundo servo movido a 50 grados.");
        ifServo2 = false;
        AdelanteRecoger = true;
    }

  } else {
    // Si no hay objeto detectado, mover el primer servo motor
    servoPos1 += direction;
    if (servoPos1 >= limiteSuperior || servoPos1 <= limiteInferior) {
      direction = -direction; // Cambiar dirección al llegar a los límites
    }
    myservo1.write(servoPos1);
    delay(5); // Pausa corta para permitir que el primer servo se mueva rápidamente
    
    // Mover el segundo servo en un ciclo diferente si es necesario
    // Ejemplo: puede agregar lógica aquí para el segundo servo si no se está moviendo en respuesta a la detección
  }
  
  tiempoActualRueda = millis();
    
    if (!AdelanteRecoger){
      tiempoRueda = tiempoActualRueda;
    }else {
      if(tiempoActualRueda > tiempoRueda + tiempoAdelanteActivo){
        if (tiempoActualRueda > tiempoRueda + tiempoAdelanteActivo + tiempoAdelanteActivo){
          
          digitalWrite(entrada3, LOW);
          digitalWrite(entrada4, LOW);
          AdelanteRecoger = false;
        }else{
        digitalWrite(entrada3, LOW); // Activar entrada3
        digitalWrite(entrada4, HIGH);  // Desactivar entrada4
      }
      }
    }

  // Pausa antes de la siguiente medición
  delay(100);
}





//#include <Wire.h>
// #include "Adafruit_TCS34725.h"

// // Inicializar el sensor con tiempo de integración de 600ms y ganancia de 1x
// Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_600MS, TCS34725_GAIN_1X);

// void setup(void) {
//   // Iniciar la comunicación serie a 9600 bps
//   Serial.begin(9600);

//   // Configurar los pines I2C para ESP32
//   Wire.begin(21, 22);

//   // Intentar inicializar el sensor
//   if (tcs.begin()) {
//     Serial.println("Found sensor");
//   } else {
//     Serial.println("No TCS3472 found ... check your connections");
//     while (1); // Bucle infinito si no se encuentra el sensor
//   }
// }

// void loop(void) {
//   uint16_t r, g, b, c;
//   tcs.getRawData(&r, &g, &b, &c);

//   // Imprimir los valores en bruto del sensor
//   Serial.print("R: "); Serial.print(r, DEC); Serial.print(" ");
//   Serial.print("G: "); Serial.print(g, DEC); Serial.print(" ");
//   Serial.print("B: "); Serial.print(b, DEC); Serial.print(" ");
//   Serial.print("C: "); Serial.print(c, DEC); Serial.println(" ");

//   // Determinar el color del objeto
//   String color = identifyColor(r, g, b);

//   // Imprimir el color identificado
//   Serial.print("Color identificado: ");
//   Serial.println(color);

//   delay(1000); // Esperar un segundo antes de la siguiente lectura
// }

// // Función para identificar el color del objeto
// String identifyColor(uint16_t r, uint16_t g, uint16_t b) {
//   if (r > 100 && g > 100 && b < 1400) { // Ajustar los umbrales según sea necesario
//     return "Amarillo";
//   } else if (g > r && g > b) {
//     return "Verde";
//   } else if (b > r && b > g) {
//     return "Azul";
//   } else if (r > g && r > b) {
//     return "Rojo";
//   } else {
//     return "Color desconocido";
//   }
// }


//Codigo color con servo 

// #include <Wire.h>
// #include "Adafruit_TCS34725.h"
// #include <ESP32Servo.h>

// // Inicializar el sensor con tiempo de integración de 600ms y ganancia de 1x
// Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_600MS, TCS34725_GAIN_1X);

// // Crear un objeto Servo
// Servo myservo;

// // Variables para almacenar el color detectado y el tiempo
// String detectedColor = "";
// String lastDetectedColor = "";
// unsigned long colorDetectedTime = 0;
// unsigned long restStartTime = 0; // Tiempo cuando comienza el descanso
// bool colorDetected = false;
// bool resting = false; // Indicador de si el sensor está en descanso

// void setup(void) {
//   // Iniciar la comunicación serie a 9600 bps
//   Serial.begin(9600);

//   // Configurar los pines I2C para ESP32
//   Wire.begin(21, 22);

//   // Inicializar el servo en el pin 16
//   myservo.attach(16);

//   // Intentar inicializar el sensor
//   if (tcs.begin()) {
//     Serial.println("Found sensor");
//   } else {
//     Serial.println("No TCS3472 found ... check your connections");
//     while (1); // Bucle infinito si no se encuentra el sensor
//   }

//   // Posicionar el servo en la posición inicial (0 grados)
//   myservo.write(0);
// }

// void loop(void) {
//   unsigned long currentTime = millis();
  
//   if (resting) {
//     // Si está en descanso, comprobar si ha pasado el tiempo de descanso
//     if (currentTime - restStartTime >= 3000) {
//       resting = false; // Terminar el descanso
//       Serial.println("Sensor is now active.");
//     } else {
//       return; // Salir de la función loop mientras está en descanso
//     }
//   }

//   uint16_t r, g, b, c;
//   tcs.getRawData(&r, &g, &b, &c);

//   // Imprimir los valores en bruto del sensor
//   Serial.print("R: "); Serial.print(r, DEC); Serial.print(" ");
//   Serial.print("G: "); Serial.print(g, DEC); Serial.print(" ");
//   Serial.print("B: "); Serial.print(b, DEC); Serial.print(" ");
//   Serial.print("C: "); Serial.print(c, DEC); Serial.println(" ");

//   // Determinar el color del objeto
//   String color = identifyColor(r, g, b);

//   // Imprimir el color identificado
//   Serial.print("Color identificado: ");
//   Serial.println(color);

//   // Si se detecta un nuevo color
//   if (color != "Color desconocido" && color != detectedColor) {
//     detectedColor = color;
//     colorDetectedTime = currentTime;
//     colorDetected = true;
//     myservo.write(90); // Mover el servo a 50 grados
//     Serial.println("Color detected and servo moved to 50 degrees");
//   }

//   // Si el color se ha detectado durante 3 segundos
//   if (colorDetected && (currentTime - colorDetectedTime >= 3000)) {
//     // Leer nuevamente el color
//     tcs.getRawData(&r, &g, &b, &c);
//     color = identifyColor(r, g, b);

//     // Si se detecta el mismo color nuevamente
//     if (color == detectedColor) {
//       myservo.write(0); // Mover el servo a 0 grados
//       Serial.println("Same color detected again, servo moved back to 0 degrees");
//       lastDetectedColor = detectedColor; // Guardar el color detectado
//       detectedColor = ""; // Reiniciar el color detectado
//       colorDetected = false; // Reiniciar la detección de color
//       resting = true; // Activar el descanso del sensor
//       restStartTime = currentTime; // Marcar el inicio del descanso
//     } else {
//       // Si no es el mismo color, resetear el tiempo y continuar detectando
//       colorDetectedTime = currentTime;
//     }
//   }

//   // El tiempo de descanso ya se ha manejado, así que solo esperar un poco antes de la siguiente lectura
//   delay(100);
// }

// // Función para identificar el color del objeto
// String identifyColor(uint16_t r, uint16_t g, uint16_t b) {
//   if (r >= 140 && r <= 190 && g >= 210 && g <= 250 && b >= 105 && b <= 180) {
//     return "Amarillo";
//   } else if (r >= 40 && r <= 58 && g >= 90 && g <= 120 && b >= 75) {
//     return "Verde";
//   } else if (r >= 50 && r <= 140 && g >= 100 && g <= 290 && b >= 110 && b <= 500) {
//     return "Azul";
//   } else if (r >= 100 && r <= 120 && g >= 85 && g <= 105 && b >= 75 && b <= 95) {
//     return "Rojo";
//   } else {
//     return "Color desconocido";
//   }
// }




//(r + 100) >= g && (r - 100) > 500 && b < 1400)


// https://www.youtube.com/watch?v=OPEYwuAbLBI
// https://www.youtube.com/watch?v=-qf8MpPAcIs tutorial sensor de Color




//Codigo sensor de color tcs3472

// #include <Wire.h>
// #include "Adafruit_TCS34725.h"

// /*
//   Conexiones:
//   - SCL a GPIO 22
//   - SDA a GPIO 21
//   - VDD a 3.3V DC
//   - GND a tierra común
// */


// #include "BluetoothSerial.h"

// String device_name = "ESP32-BT-Slave";
// #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
// #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
// #endif
// #include <ESP32Servo.h>

// #include <bits/c++config.h>
// #if !defined(CONFIG_BT_SPP_ENABLED)
// #error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip.
// #endif

// Servo myservo;  
// int pos = 5;

// BluetoothSerial SerialBT;

// int entrada1 = 27;
// int entrada2 = 26;
// int entrada3 = 25;
// int entrada4 = 33;

// void setup() {
//   myservo.attach(18);
//   Serial.begin(115200);
//   SerialBT.begin(device_name);
//   Serial.printf("The device with name \"%s\" is started.\nNow you can pair it with Bluetooth!\n", device_name.c_str());
//     pinMode(entrada1, OUTPUT); 
//     pinMode(entrada2, OUTPUT);
//     pinMode(entrada3, OUTPUT); 
//     pinMode(entrada4, OUTPUT);
//   myservo.write(pos);
// }

// void loop() {
//   char dato = SerialBT.read();

//     if (dato == 'T'){
//     pos = min(pos + 10, 180);  
//     myservo.write(pos);
//   };
//     if (dato == 'X'){
//     pos = max(pos - 10, 5);
//     myservo.write(pos);
//   };
//   if (dato == 'C'){
//     digitalWrite(entrada1, LOW); 
//     digitalWrite(entrada2, HIGH);

//   };
//     if (dato == 'S'){
//     digitalWrite(entrada1, HIGH); 
//     digitalWrite(entrada2, LOW);

//   };
//     if (dato == 'F'){
//     digitalWrite(entrada3, LOW); 
//     digitalWrite(entrada4, HIGH);

//   };
//     if (dato == 'B'){
//     digitalWrite(entrada3, HIGH); 
//     digitalWrite(entrada4, LOW);
//   };
//     if (dato == '0'){
//     digitalWrite(entrada1, LOW); 
//     digitalWrite(entrada2, LOW);
//     digitalWrite(entrada3, LOW); 
//     digitalWrite(entrada4, LOW);
//   };
//     if (dato == '1'){
//     digitalWrite(entrada3, LOW); 
//     digitalWrite(entrada4, LOW);
//   };
//   // if (Serial.available()) {
//   //   SerialBT.write(Serial.read());
//   // }
//   // if (SerialBT.available()) {
//   //   Serial.write(SerialBT.read());
//   // }

//   delay(20);
// }