/*GR-SAKURA Sketch Template Version: V1.08*/
#include <rxduino.h>
#include <Wire.h>
#include <Math.h>
//#include <Tone.h>

byte i2cWriteBuffer[10];
byte i2cReadBuffer[10];

#define SensorAddressWrite 0x29 //
#define SensorAddressRead 0x29 // 
#define EnableAddress 0xa0 // register address + command bits
#define ATimeAddress 0xa1 // register address + command bits
#define WTimeAddress 0xa3 // register address + command bits
#define ConfigAddress 0xad // register address + command bits
#define ControlAddress 0xaf // register address + command bits
#define IDAddress 0xb2 // register address + command bits
#define ColorAddress 0xb4 // register address + command bits
#define ColorThreshold 400	// colorThreshold

//#define pinR 1
//#define pinG 2
//#define pinB 3
void Writei2cRegisters(byte numberbytes, byte command)
{
byte i = 0;

Wire.beginTransmission(SensorAddressWrite); // Send address with Write bit set
Wire.write(command); // Send command, normally the register address 
for (i=0;i<numberbytes;i++) // Send data 
Wire.write(i2cWriteBuffer[i]);
Wire.endTransmission();

delayMicroseconds(100); // allow some time for bus to settle 
}

byte Readi2cRegisters(int numberbytes, byte command)
{
byte i = 0;

Wire.beginTransmission(SensorAddressWrite); // Write address of read to sensor
Wire.write(command);
Wire.endTransmission();

delayMicroseconds(100); // allow some time for bus to settle 

Wire.requestFrom(SensorAddressRead,numberbytes); // read data
for(i=0;i<numberbytes;i++)
i2cReadBuffer[i] = Wire.read();
Wire.endTransmission(); 

delayMicroseconds(100); // allow some time for bus to settle 
} 

void init_TCS34725(void)
{
i2cWriteBuffer[0] = 0x10;
Writei2cRegisters(1,ATimeAddress); // RGBC timing is 256 - contents x 2.4mS = 
i2cWriteBuffer[0] = 0x00;
Writei2cRegisters(1,ConfigAddress); // Can be used to change the wait time
i2cWriteBuffer[0] = 0x00;
Writei2cRegisters(1,ControlAddress); // RGBC gain control
i2cWriteBuffer[0] = 0x03;
Writei2cRegisters(1,EnableAddress); // enable ADs and oscillator for sensor 
}

void get_TCS34725ID(void)
{
Readi2cRegisters(1,IDAddress);
if (i2cReadBuffer[0] = 0x44)
Serial.println("TCS34725 is present"); 
else
Serial.println("TCS34725 not responding"); 
}
int hzData = 0;
void get_Colors(void)
{
unsigned int clear_color = 0;
unsigned int red_color = 0;
unsigned int green_color = 0;
unsigned int blue_color = 0;

Readi2cRegisters(8,ColorAddress);
clear_color = (unsigned int)(i2cReadBuffer[1]<<8) + (unsigned int)i2cReadBuffer[0];
red_color = (unsigned int)(i2cReadBuffer[3]<<8) + (unsigned int)i2cReadBuffer[2];
green_color = (unsigned int)(i2cReadBuffer[5]<<8) + (unsigned int)i2cReadBuffer[4];
blue_color = (unsigned int)(i2cReadBuffer[7]<<8) + (unsigned int)i2cReadBuffer[6];

//	double colorAvg = (red_color+green_color+blue_color)/3;
//	double r = red_color/colorAvg;
//	double g = green_color/colorAvg;
//	double b = blue_color/colorAvg;

double r = red_color;
double g = green_color;
double b = blue_color;
Serial.print(r);
Serial.print(",");
Serial.print(g);
Serial.print(",");
Serial.println(b);

int rx = r *70.0;
int gx = g *70.0;
int bx = b *70.0;

analogWrite(5,rx);
analogWrite(6,gx);
analogWrite(7,bx);
// analogWrite(pinG,g*150);
// analogWrite(pinB,b*150);

// digitalWrite(5,HIGH);
} 

void setup() {
//pinMode(5,OUTPUT);
Wire.begin();
Serial.begin(9600); // start serial for output
init_TCS34725();
get_TCS34725ID();// get the device ID, this is just a test to see if we're connected

}

void loop() {
get_Colors();
delay(1000);
// digitalWrite(PIN_LED0, 1);

}