#include <Servo.h>

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

Servo servo_red;
Servo servo_green;
Servo servo_blue;

int frequency = 0;
int pos_red = 0;
int pos_green = 0;
int pos_blue = 0;

void setup() {
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);

  servo_red.attach(9);
  servo_green.attach(10);
  servo_blue.attach(11);

  servo_red.write(0);
  servo_green.write(0);
  servo_blue.write(0);
  
  // Setting frequency-scaling to 20%
  digitalWrite(S0,HIGH);
  digitalWrite(S1,LOW);
  
  Serial.begin(9600);
}
void loop() {
  
  // Setting red filtered photodiodes to be read
  digitalWrite(S2,LOW);
  digitalWrite(S3,LOW);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  frequency = map(frequency, 25,72,255,0);
  // Printing the value on the serial monitor
  Serial.print("R= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.print("  ");
  if(frequency <= 100){
    Serial.print("Red frequency < 100:");
    Serial.println(frequency);
    for (pos_red = 0; pos_red <= 180; pos_red += 1) { // goes from 0 degrees to 180 degrees 
      // in steps of 1 degree
      servo_red.write(pos_red);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
    for (pos_red = 180; pos_red >= 0; pos_red -= 1) { // goes from 180 degrees to 0 degrees
      servo_red.write(pos_red);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
  }
  delay(100);
  // Setting Green filtered photodiodes to be read
  digitalWrite(S2,HIGH);
  digitalWrite(S3,HIGH);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  frequency = map(frequency, 30,90,255,0);
  // Printing the value on the serial monitor
  Serial.print("G= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.print("  ");
  if(frequency <= 100){
    Serial.print("Green frequency < 100:");
    Serial.println(frequency);
    for (pos_green = 0; pos_green <= 180; pos_green += 1) { // goes from 0 degrees to 180 degrees 
      // in steps of 1 degree
      servo_green.write(pos_green);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
    for (pos_green = 180; pos_green >= 0; pos_green -= 1) { // goes from 180 degrees to 0 degrees
      servo_green.write(pos_green);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
  }
  delay(100);
  // Setting Blue filtered photodiodes to be read
  digitalWrite(S2,LOW);
  digitalWrite(S3,HIGH);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  //Remaping the value of the frequency to the RGB Model of 0 to 255
  frequency = map(frequency, 25,70,255,0);
  // Printing the value on the serial monitor
  Serial.print("B= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.println("  ");
  if(frequency <= 100){
    Serial.print("Blue frequency < 100:");
    Serial.println(frequency);
    for (pos_blue = 0; pos_blue <= 180; pos_blue += 1) { // goes from 0 degrees to 180 degrees 
      // in steps of 1 degree
      servo_blue.write(pos_blue);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
    for (pos_blue = 180; pos_blue >= 0; pos_blue -= 1) { // goes from 180 degrees to 0 degrees
      servo_blue.write(pos_blue);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15ms for the servo to reach the position
    }
  }
  delay(100);
}