#include "oulib.h"
#include "i2c_isr.h"
#include "agavo.h"

/////////////////////////////////////////////
// Agavo color sensor interface
//
// Provides a base level of functionality for configuring and
//  querying the sensor.
//  
// The sensor is an I2C slave.  We rely on the i2c_tr 
//  (transmit/receive) implementation of the generic I2C 
//  master.
//

// uint8_t agavo_initiate_check_status()
//
// Query the sensor as to its status

uint8_t agavo_initiate_check_status()
{
  buffer_add(i2c_buffer_out, AGAVO_REGISTER_STATUS);
  return(i2c_initiate_transmit_receive(AGAVO_ADDRESS, 1, 1));
  
};

// uint8_t agavo_set_caps(uint8_t cap_red,
//		       uint8_t cap_green,
//		       uint8_t cap_blue,
//		       uint8_t cap_clear)
//
//  Initiate the transaction that sets the filter capacitor states
//
//

uint8_t agavo_set_caps(uint8_t cap_red,
		       uint8_t cap_green,
		       uint8_t cap_blue,
		       uint8_t cap_clear)
{
  buffer_add(i2c_buffer_out, AGAVO_REGISTER_CAP_RED);
  buffer_add(i2c_buffer_out, cap_red);
  buffer_add(i2c_buffer_out, cap_green);
  buffer_add(i2c_buffer_out, cap_blue);
  buffer_add(i2c_buffer_out, cap_clear);
  return(i2c_initiate_transmit_receive(AGAVO_ADDRESS, 5, 0));
};


//  uint8_t agavo_initiate_sensor_grab()
//
//  Initiate transaction to grab all 4 sensor values
//
//

uint8_t agavo_initiate_sensor_grab()
{
  buffer_add(i2c_buffer_out, AGAVO_REGISTER_DATA_RED_LOW);
  return(i2c_initiate_transmit_receive(AGAVO_ADDRESS, 1, 8));
};


// uint8_t agavo_initiate_sensor_integral()
//
// Tell sensor to start integration.
//

uint8_t agavo_initiate_sensor_integral()
{
  buffer_add(i2c_buffer_out, AGAVO_REGISTER_STATUS);
  buffer_add(i2c_buffer_out, AGAVO_REGISTER_STATUS_GSSR);
  return(i2c_initiate_transmit_receive(AGAVO_ADDRESS, 2, 0));
};


// uint8_t agavo_set_ints(uint16_t int_red,
//		          uint16_t int_green,
//		          uint16_t int_blue,
//		          uint16_t int_clear)
//
//  Initiate transmission of integral terms
//

uint8_t agavo_set_ints(uint16_t int_red,
		      uint16_t int_green,
		      uint16_t int_blue,
		      uint16_t int_clear)
{
  buffer_add(i2c_buffer_out, AGAVO_REGISTER_INT_RED_LOW);
  buffer_add(i2c_buffer_out, (0xFF & int_red));
  buffer_add(i2c_buffer_out, (0xFF00 & int_red)>>8);
  buffer_add(i2c_buffer_out, (0xFF & int_green));
  buffer_add(i2c_buffer_out, (0xFF00 & int_green)>>8);
  buffer_add(i2c_buffer_out, (0xFF & int_blue));
  buffer_add(i2c_buffer_out, (0xFF00 & int_blue)>>8);
  buffer_add(i2c_buffer_out, (0xFF & int_clear));
  buffer_add(i2c_buffer_out, (0xFF00 & int_clear)>>8);
  return(i2c_initiate_transmit_receive(AGAVO_ADDRESS, 9, 0));
};

// void agavo_extract_values(uint16_t sensor_vals[4])
//
// Extract the four sensor values from the input buffer.  Assumes
//  that the transaction that brings in these values has completed
//  successfully
//

void agavo_extract_values(uint16_t sensor_vals[4])
{
  int8_t i;

  for(i = 0; i < 4; ++i) {
    sensor_vals[i] = (uint8_t) buffer_remove(i2c_buffer_in);
    sensor_vals[i] |= ((0x3&((uint8_t) buffer_remove(i2c_buffer_in)))<<8);
  };
};