#include <Servo.h>
#include <LiquidCrystal_I2C.h>

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

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);
  pinMode(sensorIR, INPUT);  // Configura el pin del sensor IR como entrada
  Serial.begin(9600);        // Inicia la comunicación serial

    // 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);

}

//Definimos los parametros basicos del LCD
  lcd.init();                      // Inicializa el LCD
  lcd.backlight();                 // Enciende la retroiluminación
}

void loop() {
  //Detectar objetos.
  int estado = detectarObjeto();  // Llama a la función y almacena el resultado
  Serial.println(estado);         // Imprime el estado en el monitor serial
  delay(500);                     // Pequeña pausa

  //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);

  delay(1000); // Esperar un segundo antes de la próxima lectura
}

// Función para detectar objeto
int detectarObjeto() {
  int estadoSensor = digitalRead(sensorIR); // Lee el estado del sensor IR
  if (estadoSensor == HIGH) {
    // Si el sensor detecta un objeto, retorna 1
    return 1;
  } else {
    // Si el sensor no detecta un objeto, retorna 0
    return 0;
  }
}
// Función para leer el color RGB
ColorRGB readRGB() {
  ColorRGB color;

  color.red = readColor(LOW, LOW);   // Lectura del rojo
  color.green = readColor(HIGH, HIGH); // Lectura del verde
  color.blue = readColor(LOW, HIGH);  // Lectura del azul

  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
}