#include "color.h"
#include <windows.h>
#include <sstream>
#include <string>

Color_sensor::Color_sensor(Sensor_port port, Connection *connection, unsigned int wait_time, unsigned char i2c_address):I2c(port,connection, LOWSPEED, i2c_address ,wait_time){}

Color_sensor::~Color_sensor(){}


int Color_sensor::read(Result &result){
  if(! (result.get_type() == RGB_RESULT)){
    return 0;
  }
  if(!this->has_init){
    init();
  }
  Result *temp;
  temp = (Result *) &result;
  Rgb_color *color = (Rgb_color*)(temp);
  unsigned char rx_buffer[I2C_BUFFER_SIZE];
  unsigned char command[2];
  command[0]= COLOR_ADDRESS;
  command[1]= RED_RAW_LOW;
  i2c_write(&command[0], 2, 6);
  wait_for_bytes(6);
  i2c_read(&rx_buffer[0], 6);
  color->red = ((0xff & rx_buffer[2]) | ((0xff & rx_buffer[1]) << 8));
  color->green = ((0xff & rx_buffer[4]) | ((0xff & rx_buffer[3]) << 8));
  color->blue = ((0xff & rx_buffer[6]) | ((0xff & rx_buffer[5]) << 8));
  return 1;
}

void Color_sensor::init(bool reply){
  Rgb_color color;
  I2c::init(reply);
  read(color);
  read(color);
  this->has_init=true;
}

int Color_sensor::read(){
  if(!this->has_init){
    init();
  }
  unsigned char rx_buffer[I2C_BUFFER_SIZE];
  unsigned char command[2];
  command[0]= COLOR_ADDRESS;
  command[1]= COLOR_NUMBER;

  //test this
  /*i2c_write(&command[0], 2, 14);
  wait_for_bytes(14);
  i2c_read(&rx_buffer[0], 14);*/

  i2c_write(&command[0], 2, 1);
  wait_for_bytes(1);
  i2c_read(&rx_buffer[0], 1);
  /*printf("Color Number: %d\n", rx_buffer[1]);
  printf("Red reading: %d\n", rx_buffer[2]);
  printf("Green reading: %d\n", rx_buffer[3]);
  printf("Blue reading: %d\n", rx_buffer[4]);
  printf("Raw red - High: %d \n", rx_buffer[5]);
  printf("Raw red - Low: %d\n", rx_buffer[6]);
  printf("Raw Green - High: %d\n", rx_buffer[7]);
  printf("Raw Green - Low: %d\n", rx_buffer[8]);
  printf("Raw Blue - High: %d\n", rx_buffer[9]);
  printf("Raw Blue - Low: %d\n", rx_buffer[10]);
  printf("Color index: %d\n", rx_buffer[11]);
  printf("Normalized red: %d\n", rx_buffer[12]);
  printf("Normalized green: %d\n", rx_buffer[13]);
  printf("Normalized blue: %d\n", rx_buffer[14]);*/
  //printf(rx_buffer)
  return 0xff & rx_buffer[1]; //returns unsigned value
}

Sensor_type Color_sensor::get_type(){
  return COLOR;
}

std::string Color_sensor::print(){
   Rgb_color color;
   read(color);
   std::stringstream out;
   out << "red=" << color.red << " green=" << color.green <<" blue=" << color.blue;
   return out.str();
}