#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#include <SoftWire.h>
const int buzzerPin = 8; // Pin al que está conectado el buzzer

int Color_Dulce = 0;
// Definimos las Variables de Pruebas.
int Colortest_Rojo= 0;
int Colortest_Azul= 0;
int Colortest_Verde =0;
int potpin_red = A0;
int potpin_green= A1;
int potpin_blue = A2;

//Definimos las Banderas del proceso
bool ValordeBandera_Inicio = false;
bool Dulce_Detectado;
//Definimos la prueba del PWM

// Define los pines para SDA y SCL
#define SDA_PIN A4
#define SCL_PIN A5

// Crea una instancia de SoftWire
SoftWire sw(SDA_PIN, SCL_PIN);

// Inicializa el objeto LCD (ajusta 0x27 al I2C address de tu LCD)
LiquidCrystal_I2C lcd(0x27, 20, 4);



//Definimos la cantidad de ServoMotores que vamos a utilizar.
const int numeroServos = 3;

Servo servos[numeroServos];

// Definición del pin del sensor IR
const int sensorIR = 2;  // Asegúrate de conectar tu sensor a este pin

//Declaramos la Estructura para el sensor de color
struct ColorRGB {
  int red;
  int green;
  int blue;
};

// Definición de pines del Sensor de color
const int s0 = 8;
const int s1 = 9;
const int s2 = 12;
const int s3 = 11;
const int out = 2;

// INICIALIZACION DEL SISTEMA
void setup() {

  //Definimos los Valores de TCS3200
  pinMode(s0, OUTPUT);
  pinMode(s1, OUTPUT);
  pinMode(s2, OUTPUT);
  pinMode(s3, OUTPUT);
  pinMode(out, INPUT);
  digitalWrite(s0, HIGH); // Configurar escala de frecuencia al 20%
  digitalWrite(s1, LOW);
  
  //Sensor Infrarojo
  pinMode(sensorIR, INPUT);  // Configura el pin del sensor IR como entrada


// Inicio de la Comunicacion Serial
  Serial.begin(9600);        

//SERVOMOTORES INICIO
// Asignamos pines a cada servo motor
  servos[0].attach(9);
  servos[1].attach(10);
  servos[2].attach(11);
  servos[3].attach(12);

  // Ejemplo de inicialización de servos a posición 0
  for (int i = 0; i < numeroServos; i++) {
    MoverMotor(i, 0);

}
// LCD LED 20X4 I2C INICIALIZACION
  lcd.init();                      // Inicializa el LCD
  lcd.backlight();                 // Enciende la retroiluminación

//  lcd.setCursor(0, 0);             // Establece el cursor en la primera línea
//  lcd.print("Hola Mundo!");        // Imprime un mensaje en la primera línea

// lcd.setCursor(0, 1);             // Segunda línea
//  lcd.print("LCD 20x4 con I2C");

//  lcd.setCursor(0, 2);             // Tercera línea
//  lcd.print("Arduino");

//  lcd.setCursor(0, 3);             // Cuarta línea
//  lcd.print("Prueba I2C");


  //Inicializamos el SoftWire
    sw.begin();
    pinMode(buzzerPin, OUTPUT); // Configura el pin del buzzer como salida


}

// CODIGO CORRE AQUI

void loop() {

 InicializarSistema(ValordeBandera_Inicio);
  //Leer los colores RGB
 // ColorRGB color = readRGB();
 // Serial.print("Rojo: ");
 // Serial.print(color.red);
 // Serial.print(" Verde: ");
 // Serial.print(color.green);
 // Serial.print(" Azul: ");
 // Serial.println(color.blue);
 int a= detectarObjeto();
  Serial.println(a);
  delay(1000); // Esperar un segundo antes de la próxima lectura
}




///DEFINICION DE FUNCIONES
// Función para detectar dulce
int detectarObjeto() {
  int estadoSensor = digitalRead(sensorIR); // Lee el estado del sensor IR
  if (estadoSensor == HIGH && ValordeBandera_Inicio == false ) {
    // Si el sensor detecta un Dulce, retorna 1\
    ValordeBandera_Inicio = true;
    Dulce_Detectado = true;
    return 1;
  } else {
    // Si el sensor no detecta un dulce, retorna 0
        ValordeBandera_Inicio = false;
        Dulce_Detectado = false;
    return 0;
  }
}
// Función para leer el color RGB
ColorRGB readRGB() {
  ColorRGB color;
  //HABILITAMOS CUANDO INSTALEMOS EL SISTEMA.
 // color.red = readColor(LOW, LOW);   // Lectura del rojo
  //color.green = readColor(HIGH, HIGH); // Lectura del verde
  //color.blue = readColor(LOW, HIGH);  // Lectura del azul
  color.red = analogRead(potpin_red);
  color.red = map(color.red, 0, 1023, 0, 255);  //Esto no es necesario, ya esta en la funcion ReadColor.
  color.green = analogRead(potpin_green);
  color.green = map(color.green, 0, 1023, 0, 255);  //Esto no es necesario, ya esta en la funcion ReadColor.
  color.blue = analogRead(potpin_blue);
  color.blue = map(color.blue, 0, 1023, 0, 255);  //Esto no es necesario, ya esta en la funcion ReadColor.
  return color;
}

// Función original para leer un color individual
int readColor(bool s2State, bool s3State) {
  digitalWrite(s2, s2State);
  digitalWrite(s3, s3State);

  int frequency = pulseIn(out, LOW);
  int colorValue = map(frequency, 25, 500, 255, 0);
  colorValue = constrain(colorValue, 0, 255);

  return colorValue;
}

// Función para mover un servo motor específico a un ángulo específico
int MoverMotor(int motor, int AnguloServo) {
  if (motor >= 0 && motor < numeroServos) { // Verifica si el número de motor es válido
    servos[motor].write(AnguloServo);
    return 0; // Retorna 0 si la operación es exitosa
  }
  return -1; // Retorna -1 si el número de motor no es válido
}

// Función para controlar el buzzer
void controlBuzzer(bool estado) {
  if (estado) {
    tone(buzzerPin, 500);
    delay(1000);
  } else {
    tone(buzzerPin, 800);
    delay(1000);
  }
}


// Definimos una funcion para Recibir los Colores.
int ColordelDulce(){
  //Leer los colores RGB
   ColorRGB color = readRGB(); // Para la prueba fisica, tenemos que descomentarlo.
  //Hacemos un IF, que nos permite identificar el color. Esto hay que cambiarlo a color.red, color.green y color.blue
  if(color.red == 255 && color.green == 255 && color.blue == 255 ) //&& color.green == 255 && color.blue == 255)
  {
    return 0; ///  Es Blanco.
  }else{
    return -1;
  }
  //Serial.print("Rojo: ");
  //Serial.print(color.red);
  //Serial.print(" Verde: ");
  //Serial.print(color.green);
  //Serial.print(" Azul: ");
  //Serial.println(color.blue);

}

int InicializarSistema(bool ValordeBandera_Inicio){
  int estado = detectarObjeto();  // Llama a la función y almacena el resultado
  //Serial.println(estado);
 // Serial.println(ValordeBandera_Inicio);
  delay(500);                     // Pequeña pausa
  if(estado == 1 && ValordeBandera_Inicio == false && Dulce_Detectado == true){
     MoverMotor(0,90);
    lcd.setCursor(0, 0);             // Establece el cursor en la primera línea
  lcd.print("Dulce Detectado  ");        // Imprime un mensaje en la primera línea
  lcd.setCursor(0, 1);             // Establece el cursor en la primera línea
  lcd.print("Abriendo...");        // Imprime un mensaje en la primera línea
  
  //controlBuzzer(true);
  }else if(estado == 0){
  lcd.clear();
  lcd.setCursor(0, 0);             // Establece el cursor en la primera línea
  lcd.print("Dulce No detectado");
      MoverMotor(0,0);
      temporizador(1000);
  }

}

int ClasificacionDeDulces(){
    Color_Dulce = ColordelDulce();
    //Serial.println(Dulce_Detectado);
   Serial.println(Color_Dulce);
    

}

bool temporizador(unsigned long intervalo) {
  static unsigned long tiempoPrevio = 0;
  unsigned long tiempoActual = millis();

  if (tiempoActual - tiempoPrevio >= intervalo) {
    tiempoPrevio = tiempoActual;
    return true;
  }

  return false;
}