#include <Servo.h>
// ¿Que me falta de esta etapa? Nada. Creo.
const int numeroServos = 3;
int Color_Dulce = -1;
Servo servos[numeroServos];
  // [0]= 25 grados == Rojo, [1]=  50 grados == Verde, [2]=  75 grados == Azul, [3]= 100 grados == Desconocido
int Angulos_Depositos_Colores[4] = {25 ,50, 75, 100};
bool ValordeBandera_Inicio = true;
int estado = 1;
bool Dulce_Detectado = true;


//Colores

int potpin_red = A0;
int potpin_green= A1;
int potpin_blue = A2;
struct ColorRGB {
  int red;
  int green;
  int blue;
};

void setup() {
  // put your setup code here, to run once:
  servos[0].attach(9);
  servos[1].attach(10);
  servos[2].attach(11);
  servos[3].attach(12);

  for (int i = 0; i < numeroServos; i++) {
    MoverMotor(i, 0);

}
  Serial.begin(9600); // Inicia la comunicación serial a 9600 baudios
   Serial.println("Inicializando Test!");


}
// Luego de que se inicialice el sistema y detectemos que haya dulces en la entrada, tenemos que mover el servo principal
//desde 0 a 90 grados.

void loop() {
    Clasificacion_Etapa2(Dulce_Detectado, ValordeBandera_Inicio, estado);
    
}

int Clasificacion_Etapa2(bool Dulce_Detectado, bool ValordeBandera_Inicio, int estado){
  //Hay que poner un IF que valide las variables de entrada..
  //Se asume que ya cayo el dulce en el agujero, por lo tanto
  if(Dulce_Detectado == true && ValordeBandera_Inicio == true && estado == 1){
  delay(1000);
  MoverMotor(1,90);
  delay(1000);
  //Necesitamos verificar el Color, y que nos retorne un valor para luego proseguir en la etapa de deposito.
  //Funcion para Detectar el color.
  Color_Dulce =  ClasificacionDeDulces();
  //Si, la funcion detecta el color (entre los 3 parametros es un OR entre los 3 posibles valores), 
  //No, la idea de arriba esta incompleta,la funcion de los dulces nos retorna un valor desde 0 hasta 3.
  //Por lo que se necesitará  un Switch.
  switch (Color_Dulce) {
  case 1:
    // Código para cuando variable es 1
    MoverMotor(2,Angulos_Depositos_Colores[0]);
    delay(1000);
    MoverMotor(1,180);
    delay(1000);
    MoverMotor(1,0);
    delay(1000);
    break;
  case 2:
    // Código para cuando variable es 2
    MoverMotor(2,Angulos_Depositos_Colores[1]);
    delay(1000);
    MoverMotor(1,180);
    delay(1000);
    MoverMotor(1,0);
    delay(1000);
    break;
  case 3:
    // Código para cuando variable es 3
    MoverMotor(2,Angulos_Depositos_Colores[2]);
    delay(1000);
    MoverMotor(1,180);
    delay(1000);
    MoverMotor(1,0);
    delay(1000);
    break;
  //default:
    case 4:
    // Código para cuando ninguna de las condiciones anteriores se cumple
      MoverMotor(2,Angulos_Depositos_Colores[3]);
      delay(1000);
      MoverMotor(1,180);
    delay(1000);
    MoverMotor(1,0);
    delay(1000);
    break;
}
  //Debe mover el segundo servo, a la posicion correspondiente.
  // 25 grados == Rojo, 50 grados == Verde, 75 grados == Azul, 100 grados == Desconocido
  //Luego de mover el servo a la posicion correspondiente,  hay que mover el primer servo a los 180 grados,
  //Para que caiga el dulce.

}
}


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
}

//Colores NO VA AL CODIGO PRINCIPAL, ES EL MISMO
// 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 1; ///  Es Blanco.
  }
   if(color.red == 0 && color.green == 0 && color.blue == 0 ) //&& color.green == 255 && color.blue == 255)
  {
    return 2; ///  Es Blanco.
    
  }else{
    return 4;
  }
  //Serial.print("Rojo: ");
  //Serial.print(color.red);
  //Serial.print(" Verde: ");
  //Serial.print(color.green);
  //Serial.print(" Azul: ");
  //Serial.println(color.blue);

}

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;
}

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

}