#define S0 4
#define S1 5
#define S2 6
#define S3 7
#define sensorOut 8

int led = 13;


int R = 0;
int G = 0;
int B = 0;
// QUI I VALORI DIPENDONO ANCHE DALL'ILLUMINAZIONE AMBIENTALE
int red_valore_minimo = 25;
int red_valore_massimo = 72;
int green_valore_minimo = 30;
int green_valore_massimo = 90;
int blue_valore_minimo = 25;
int blue_valore_massimo = 70;


void setup() {
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);
  pinMode(led, OUTPUT);
  // SCALI LA FREQUENZA DEGLI IMPOLSI DI USCITA DAL SENSORE AL 20%
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);
  Serial.begin(9600);
}

void loop() {
  int color;

  delay(1000);
  R = readColor('r');
  delay(50);
  G = readColor('g');
  delay(50);
  B = readColor('b');
  delay(50);

  if(R<45 & R>32 & G<65 & G>55) {
    color = 1; // Red
  }
  else if(G<55 & G>43 & B<47 &B>35) {
    color = 2; // Orange
  }
  else if(R<53 & R>40 & G<53 & G>40) {
    color = 3; // Green
  }
  else if(R<38 & R>24 & G<44 & G>30) {
    color = 4; // Yellow
    }
  else if(R<56 & R>46 & G<65 & G>55) {
    color = 5; // Brown
    }
  else if(G<58 & G>45 & B<40 &B>26) {
    color = 6; // Blue
    }
  else{
    color = -1;
  }
  Serial.println(color);
  if (color > 0){
    accendi_led();
  }
  else{
    spegni_led();
  }
}

void accendi_led(){
  digitalWrite(led,HIGH);
}

void spegni_led(){
  digitalWrite(led,LOW);
}

int readColor(char colore_da_testare) {
  int frequency = 0;

  frequency = pulseIn(sensorOut, LOW);

  if (colore_da_testare == 'r'){
    digitalWrite(S2, LOW);
    digitalWrite(S3, LOW);
    // RIMAPPO I VALORI PER POTERLI POI USARE COME VALORI RGB
    frequency = map(frequency,red_valore_minimo,red_valore_massimo,0,255);
  }
  else if (colore_da_testare == 'g'){
    digitalWrite(S2, HIGH);
    digitalWrite(S3, HIGH);
    // RIMAPPO I VALORI PER POTERLI POI USARE COME VALORI RGB
    frequency = map(frequency,green_valore_minimo,green_valore_massimo,0,255);
  }
  else if (colore_da_testare == 'b'){
    digitalWrite(S2, LOW);
    digitalWrite(S3, HIGH);
    // RIMAPPO I VALORI PER POTERLI POI USARE COME VALORI RGB
    frequency = map(frequency,blue_valore_minimo,blue_valore_massimo,0,255);
  }

  //Serial.print(colore_da_testare);
  //Serial.print(" ");
  //Serial.println(frequency);


  return frequency;
}