#define LED    2 
#define S0     5   
#define S1     4
#define S2     9 
#define S3     14  
#define sensorOut    15

int redFrequency = 0;
int greenFrequency = 0;
int blueFrequency = 0;
int whiteFrequency = 0;

int  redFrequencySum = 0;
int  greenFrequencySum = 0;
int  blueFrequencySum = 0;
int  whiteFrequencySum = 0;

int redFrequencyCenter = 993;
int greenFrequencyCenter = 1270;
int blueFrequencyCenter = 1400;
int whiteFrequencyCenter = 400;

int redFrequencyMapped   = 0;
int greenFrequencyMapped = 0;
int blueFrequencyMapped  = 0;
int whiteFrequencyMapped = 0;


void setup() {

  
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT); 
  pinMode(LED, OUTPUT);

  digitalWrite(S0, HIGH);// OUTPUT FREQUENCY SCALING 
  digitalWrite(S1, LOW);// OUTPUT FREQUENCY SCALING 
  digitalWrite(S2, LOW);// Color Selection
  digitalWrite(S3, LOW);// Color Selection
  digitalWrite(LED,LOW) ;
  SerialUSB.begin(9600);

}

void setColor(char color)
{
  if(color=='r'){
    digitalWrite(S2, LOW);
    digitalWrite(S3, LOW);
    //SerialUSB.println("Red");
    }  
  else if(color=='g'){
    digitalWrite(S2, LOW);
    digitalWrite(S3, HIGH);
    }
  else if(color=='w'){
    digitalWrite(S2, HIGH);
    digitalWrite(S3, LOW);
    }  
  else if(color=='b'){
    digitalWrite(S2, HIGH);
    digitalWrite(S3, HIGH);
    }  
  
  }


void loop() {
// Smoothing by summing a window of 50 measurements
  digitalWrite(LED,LOW) ;
  redFrequencySum = 0;
  greenFrequencySum = 0;
  blueFrequencySum = 0;
  whiteFrequencySum = 0;
  for(int i=0;i<10;i++){
  setColor('r');
  redFrequencySum =(redFrequencySum + pulseIn(sensorOut, LOW));
  setColor('g');
  greenFrequencySum =(greenFrequencySum + pulseIn(sensorOut, LOW));
  setColor('b');

  blueFrequencySum =(blueFrequencySum + pulseIn(sensorOut, LOW));
  setColor('w');
  whiteFrequencySum = (whiteFrequencySum + pulseIn(sensorOut, LOW));
  }
  redFrequency = redFrequencySum -redFrequencyCenter +1000;
  greenFrequency = greenFrequencySum - greenFrequencyCenter+1000;
  blueFrequency = blueFrequencySum - blueFrequencyCenter+1000;
  whiteFrequency = whiteFrequencySum - whiteFrequencyCenter+1000;

digitalWrite(LED,HIGH) ;
delay(100);
  
//  SerialUSB.print("R :");
//  SerialUSB.print(redFrequency*100/whiteFrequency);
//  SerialUSB.print(" G :");
//  SerialUSB.print(greenFrequency*100/whiteFrequency);  
//  SerialUSB.print(" B :");
//  SerialUSB.print(blueFrequency*100/whiteFrequency);
//  SerialUSB.print(" W :");
//  SerialUSB.print(whiteFrequency);
//  SerialUSB.println(" ");
//
//

  SerialUSB.print("  Red: ");
  SerialUSB.println(redFrequency);
  SerialUSB.print("Green: ");
  SerialUSB.println(greenFrequency);
  SerialUSB.print(" Blue: ");
  SerialUSB.println(blueFrequency);
  SerialUSB.print("White: ");
  SerialUSB.println(whiteFrequency);
// 
 redFrequencyMapped   = map(redFrequency,5,100,255,0);
 greenFrequencyMapped = map(greenFrequency,5,100,255,0);
 blueFrequencyMapped  = map(blueFrequency,5,100,255,0);
 whiteFrequencyMapped = map(whiteFrequency,5,100,255,0);
//  SerialUSB.print("  Red: ");
//  SerialUSB.println(redFrequencyMapped);
//  SerialUSB.print("Green: ");
//  SerialUSB.println(greenFrequencyMapped);
//  SerialUSB.print(" Blue: ");
//  SerialUSB.println(blueFrequencyMapped);
//  SerialUSB.print("White: ");
//  SerialUSB.println(whiteFrequencyMapped);

  
  }