#include <Moodlamp.h>                //this is a custom library I made you will need to download and install it
Moodlamp lamp;                       //declare the lamp

// Define colour sensor LED pins
int ledArray[] = {2,3,4};

// boolean to know if the balance has been set
boolean balanceSet = false;

//place holders for colour detected
int red = 0;
int green = 0;
int blue = 0;

//floats to hold colour arrays
float colourArray[] = {0,0,0};
float whiteArray[] = {0,0,0};
float blackArray[] = {0,0,0};


//place holder for average
int avgRead;


void setup(){
  
  //setup the outputs for the colour sensor
  pinMode(2,OUTPUT);
  pinMode(3,OUTPUT);
  pinMode(4,OUTPUT);
  
  //begin serial communication
  Serial.begin(9600);
  
}

void loop(){

    checkBalance();
    checkColour();
    printColour();
    setColour();  
    getReading(5);
 
}

void checkBalance(){
  //check if the balance has been set, if not, set it
  if(balanceSet == false){
    setBalance();
  } 
}

void setBalance(){
  //set white balance
  //the pulses are warning lights, to tell you to make sure the sensor is sitting over a white sample
  lamp.pulse(255,0,0,2,2,100);              //pulse red
  lamp.pulse(255,255,255,1,1,10);           //pulse white
  delay(2000);                              //delay for two seconds, lights full on
  //scan the white sample.
  //go through each light, get a reading, set the base reading for each colour red, green, and blue to the white array
  for(int i = 0;i<=2;i++){
     digitalWrite(ledArray[i],HIGH);
     delay(100);
     getReading(5);          //number is the number of scans to take for average, this whole function is redundant, one reading works just as well.
     whiteArray[i] = avgRead;
     digitalWrite(ledArray[i],LOW);
     delay(100);
  }
  //done scanning white, now it will pulse blue to tell you that it is time for the black (or grey) sample.
  //If you are using the sensor for a different application you may want to use a true black, in which case the sensor returns a truer colour
  //With a grey sample, what we are doing is tricking the sensor into thinking that colours are a bit darker than they actually are
  //which is perfect when we are trying to mimic the colour with light
  //this first quick pulse is just to say that white has been scanned, I like visual indicators
  lamp.pulse(255,255,255,2,2,10);
  //set black balance
  //pulse blue
  lamp.pulse(0,0,255,2,2,100);
  //pulse white
  lamp.pulse(255,255,255,1,1,10);
  delay(2000);              //wait for slowpokes
  //go ahead and scan, sets the colour values for red, green, and blue when exposed to black
  for(int i = 0;i<=2;i++){
     digitalWrite(ledArray[i],HIGH);
     delay(100);
     getReading(10);
     blackArray[i] = avgRead;
     //blackArray[i] = analogRead(2);
     digitalWrite(ledArray[i],LOW);
     delay(100);
  }
  //pulse to say done black scan
  lamp.pulse(255,255,255,2,2,10);
  //quick little animation cycle through the colours to show readiness
  lamp.getColour(255,0,0);
  delay(200);
  lamp.getColour(0,255,0);
  delay(200);
  lamp.getColour(0,0,255);
  delay(200);
  lamp.pulse(255,255,255,2,1,10);
  //done blinking and whatnot turn lamp off
  lamp.getColour(0,0,0);
  delay(100);
  //set boolean value so we know that balance is set
  balanceSet = true;
  //print the balance so you can see it
  //printBalance();
}



void checkColour(){
    for(int i = 0;i<=2;i++){
     digitalWrite(ledArray[i],HIGH);  //turn or the LED, red, green or blue depending which iteration
     delay(100);                      //delay to allow CdS to stabalize, they are slow
     getReading(10);                  //take a reading however many times
     colourArray[i] = avgRead;        //set the current colour in the array to the average reading
     float greyDiff = whiteArray[i] - blackArray[i];                    //the highest possible return minus the lowest returns the area for values in between
     colourArray[i] = (colourArray[i] - blackArray[i])/(greyDiff)*255; //the reading returned minus the lowest value divided by the possible range multiplied by 255 will give us a value roughly between 0-255 representing the value for the current reflectivity(for the colour it is exposed to) of what is being scanned
     digitalWrite(ledArray[i],LOW);   //turn off the current LED
     delay(100);
  }
  
  
}

void setColour(){
  //the math isn't perfect, and sometimes will return higher than 255 or lower than zero so we just fix the numbers before lighting the moodlamp
  for(int i =0;i<=2;i++){
    if (colourArray[i] > 255){
      colourArray[i] = 255;
    }
    if (colourArray[i] < 0){
      colourArray[i] = 0;
    }
  }
  //set the red, green, and blue place holders in accordance to the scanned colour array and print them
  red = int(colourArray[0]);
  Serial.println(red);
  green = int(colourArray[1]);
  Serial.println(green);
  blue = int(colourArray[2]);
  Serial.println(blue);
  //set the lamp to the last scanned colour
  lamp.getColour(red,green,blue);
  delay(100);    //short delay for my brain to catch up
  
  
}

void getReading(int times){
  int reading;
  int tally=0;
  //take the reading however many times was requested and add them up
 for(int i = 0;i < times;i++){
   reading = analogRead(2);
   tally = reading + tally;
   delay(10);
 }
 //calculate the average and set it
 avgRead = (tally)/times;
 
   
}
//for debugging
//prints the colour in the colour array, you could also send this to processing and write a little sketch to see how good the scanner works.
void printColour(){
 Serial.print("R = ");
 Serial.println(int(colourArray[0]));
 Serial.print("G = ");
 Serial.println(colourArray[1],DEC);
 Serial.print("B = ");
 Serial.println(colourArray[2],DEC);
 //delay(2000);
}
//for debugging
//prints the black and white balances
void printBalance(){
  Serial.println("white balance");
 Serial.print("R = ");
 Serial.println(int(whiteArray[0]));
 Serial.print("G = ");
 Serial.println(whiteArray[1],DEC);
 Serial.print("B = ");
 Serial.println(whiteArray[2],DEC);
 //delay(1000);
 Serial.println("black balance");
Serial.print("R = ");
 Serial.println(int(blackArray[0]));
 Serial.print("G = ");
 Serial.println(blackArray[1],DEC);
 Serial.print("B = ");
 Serial.println(blackArray[2],DEC);
 delay(2000); 
}