#include <TimerOne.h>
#include <Wire.h>  
#include <LiquidCrystal_I2C.h> //This is only needed if You connect a I2C LCD to Your microcontroller
LiquidCrystal_I2C lcd(0x27,20,4); 

#define S0     6   
#define S1     5
#define S2     4 
#define S3     3  
#define OUT    2  
#define LED    7  

int   g_count = 0;    // count the frequecy  
int   g_array[3];     // store the RGB value
int   g_flag = 0;     // filter of RGB queue
float g_SF[3];        // save the RGB Scale factor 
// Init TSC230 and setting Frequency.

void TSC_Init()
{
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(OUT, INPUT); 
  pinMode(LED, OUTPUT);
  digitalWrite(S0, LOW);// OUTPUT FREQUENCY SCALING 2%
  digitalWrite(S1, HIGH);
  digitalWrite(LED,HIGH) ; // LOW = Swittch ON the 4 LED's , HIGH = swith off the 4 LED's
}

// Select the filter color//
void TSC_FilterColor(int Level01, int Level02)
{
  if(Level01 != 0)
    Level01 = HIGH;
    
  if(Level02 != 0)
    Level02 = HIGH;
    
  digitalWrite(S2, Level01); 
  digitalWrite(S3, Level02); 
}

void TSC_Count()
{
  g_count ++ ;
}

void TSC_Callback()
{
  switch(g_flag)
  {
    case 0:
      Serial.println("->WB Start");
      TSC_WB(LOW, LOW);                   
      break;
    case 1:
      Serial.print("->Frequency R=");
      //lcd.setCursor(0,0);
      //lcd.print("Start");
      Serial.println(g_count);
      g_array[0] = g_count;
      TSC_WB(HIGH, HIGH);             
      break;    
    case 2:
      Serial.print("->Frequency G=");        
      Serial.println(g_count);
      g_array[1] = g_count;
      TSC_WB(LOW, HIGH);             
      break; 
    case 3:
      Serial.print("->Frequency B=");
      Serial.println(g_count); 
      Serial.println("->WB End");
      g_array[2] = g_count;
      TSC_WB(HIGH, LOW);         
      break;
    default:     
      g_count = 0;         
      break; 
  }
} 

 void TSC_WB(int Level0, int Level1)  //White Balance
 {
   g_count = 0;
   g_flag ++;  
   TSC_FilterColor(Level0, Level1); 
   Timer1.setPeriod(1000000);
 }

 void setup()
 {
   TSC_Init();
   lcd.init();
   delay(100);
   lcd.backlight();
   Wire.begin();  
   delay(100);
   lcd.setCursor(14,0);
   lcd.print("Color");
   lcd.setCursor(0,3);
   lcd.print("S0:2 S1:3 S2:4 S3:5 OUT:6 LED:-");
   Serial.begin(9600);
   Timer1.initialize();             // defaulte is 1s
   Timer1.attachInterrupt(TSC_Callback);  
   attachInterrupt(0, TSC_Count, RISING);   
   delay(4000); 
   for(int i=0; i<3; i++)
   Serial.println(g_array[i]);
   g_SF[0] = 255.0/ g_array[0];     //R Scale factor
   g_SF[1] = 255.0/ g_array[1] ;    //G Scale factor 
   g_SF[2] = 255.0/ g_array[2] ;    //B Scale factor
    
   Serial.println(g_SF[0]);
   Serial.println(g_SF[1]);
   Serial.println(g_SF[2]); 
   
  //for(int i=0; i<3; i++)
   // Serial.println(int(g_array[i] * g_SF[i]));
 }
 void loop()
 {
   g_flag = 0; 
   for(int i=0; i<3; i++)
    
   {
     Serial.println(int(g_array[i] * g_SF[i]));
     //lcd.setCursor(0,1);
     //lcd.print(int(g_array[i] * g_SF[i]));
   }
   lcd.setCursor(0,1);
   lcd.print(int(g_array[0] * g_SF[0]));
   lcd.setCursor(6,1);
   lcd.print(int(g_array[1] * g_SF[1]));
   lcd.setCursor(12,1);
   lcd.print(int(g_array[2] * g_SF[2]));
   delay(4000); 
   Clean2004();
 }
 
 void Clean2004()
{
  lcd.setCursor(0,1);
  lcd.print("                    ");
  lcd.setCursor(0,2);
  lcd.print("                    ");
}

