#include <Servo.h>

// Definirea pinilor pentru senzorul de culoare
#define S0 2
#define S1 3
#define S2 4
#define S3 5
#define sensorOut 6

// Crearea obiectelor pentru servomotoare
Servo topServo;  // Servomotor pentru deschiderea și închiderea trapa
Servo bottomServo;  // Servomotor pentru poziționarea culorilor

// Variabile pentru stocarea valorilor citite de la senzorul de culoare
int red = 0;
int green = 0;
int blue = 0;

// Contorizarea numărului de culori detectate
int redcolor = 0;
int greencolor = 0;
int orangecolor = 0;
int yellowcolor = 0;

// Unghiurile de deschidere și închidere ale servomotorului pentru trapa
int CLOSE_ANGLE = 150;  // Unghiul pentru închiderea trapa
int OPEN_ANGLE = -20;   // Unghiul pentru deschiderea trapa

void setup() {
  // Setarea pinilor ca ieșire sau intrare
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT); 

  // Configurarea senzorului de culoare
  digitalWrite(S0, HIGH);  // 20% scaling
  digitalWrite(S1, LOW);

  // Atașarea servomotoarelor la pinii de control
  topServo.attach(7);
  bottomServo.attach(8);

  // Setarea poziției inițiale a servomotoarelor
  topServo.write(CLOSE_ANGLE);
  bottomServo.write(OPEN_ANGLE);

  // Inițializarea comunicării seriale pentru debugging
  Serial.begin(9600);
}

void loop() { 
  
  // Citirea valorilor de la senzorul de culoare
  
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);
  red = pulseIn(sensorOut, digitalRead(sensorOut) == HIGH ? LOW : HIGH);

  digitalWrite(S3, HIGH);
  blue = pulseIn(sensorOut, digitalRead(sensorOut) == HIGH ? LOW : HIGH);

  digitalWrite(S2, HIGH);
  green = pulseIn(sensorOut, digitalRead(sensorOut) == HIGH ? LOW : HIGH);

  // Afișarea valorilor culorilor citite
  Serial.print("R: ");
  Serial.print(red, DEC);
  Serial.print(" G: ");
  Serial.print(green, DEC);
  Serial.print(" B: ");
  Serial.print(blue, DEC);
  Serial.println();

  // Detectarea și manipularea diferitelor culori
if (red < 30 && red > 20 && blue < 35 && blue > 29) {
    bottomServo.write(73);  // Poziționarea pentru roz
    delay(700);
    redcolor++;
    Serial.println("Pink");
    openTrap();
    delay(1000);
    closeTrap();
  }

  if (red < 120 && red > 90 && green < 55 && green > 40 ) {
    bottomServo.write(140);  // Poziționarea pentru albastru
    delay(700);
    orangecolor++;
    Serial.println("Blue");
    openTrap();
    delay(1000);
    closeTrap();
  }

  if (red < 40 && red > 25 && green < 40 && green > 20) {
    bottomServo.write(173);  // Poziționarea pentru verde
    delay(700);
    greencolor++;
    Serial.println("Green");
    openTrap();
    delay(1000);
    closeTrap();
  }

  if (red < 30 && red > 16 && green < 50 && green > 34 && blue < 60 && blue > 45) {
    bottomServo.write(-270);  // Poziționarea pentru portocaliu
    delay(700);
    yellowcolor++;
    Serial.println("Orange");
    openTrap();
    delay(1000);
    closeTrap();
  }
  delay(1000);  // Așteaptă 1000 milisecunde (1 secundă) înainte de a citi din nou
}

// Funcția pentru deschiderea trapei
void openTrap() {
  topServo.write(OPEN_ANGLE);  // Trimite comanda pentru a deschide trapa
}

// Funcția pentru închiderea trapei
void closeTrap() {
  topServo.write(CLOSE_ANGLE);  // Trimite comanda pentru a închide trapa
}