#include <oulib.h>
#include "mpu9150.h"

// Load the ISRs
#include "oulib_serial_buffered.h"

/*************************
 * USB Serial Connection *
 *************************/
#define SERIAL_PORT     0
#define SERIAL_BAUD     38400
#define SERIAL_BUFFER_SIZE   40

///////////////////////////////////////////////////////////////////////////
// File pointer for USB connection
FILE* fp0 = NULL;

///////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////
int main(void)
{
  char c;
  uint8_t i;

  DDRD = 0x0;
  PORTD = 0x0;

  // LED on the Arduino
  DDRB = 0x80;

  // Flash the LED a few times
  PORTB ^= 0x80;
  delay_ms(100);
  PORTB ^= 0x80;
  delay_ms(100);
  PORTB ^= 0x80;
  delay_ms(100);
  PORTB ^= 0x80;
  delay_ms(500);

  // Initialize the serial port connected to the USB cable
  fp0 = serial_init_buffered(SERIAL_PORT, SERIAL_BAUD, SERIAL_BUFFER_SIZE, SERIAL_BUFFER_SIZE);

  // Initialize all interrupts
  sei();

  // Say 'hello'
  fprintf(fp0, "hello world\n\r");
  int8_t ret, ret2;
  
  //imu_set_fp(fp0);

  //ret = imu_init(0, fp0);
  
  ret = imu_init(0);

  fprintf(fp0, "Initialize: %d\n\r", ret);

  imu_check_bus(fp0);

  //fprintf(fp0, "Address: %x\n\r", (int) imu_get_address() >> 2);
  //fprintf(fp0, "Compass address: %x\n\r", (int) imu_get_address_compass() >> 2);
  //check_registers(fp0);

  int16_t gyro[3];
  int16_t mag[3];
  int16_t accel[3];
  int32_t val;

  //imu_set_compass_offsets(-15,93);
  imu_set_compass_params_full(24, -11, -30, -69, 96, 121, 188);


  while(1) {
    if(serial_buffered_input_waiting(fp0)){
      c = fgetc(fp0);
      switch(c)
	{
	case 'a':
	  // Read accels
	  ret = imu_read_accel(accel);
	  fprintf(fp0, "%d   %06d %06d %06d\n\r", ret, accel[0], accel[1], accel[2]);
	  //if((ret2 = imu_buffer_check()) > 0)
	  //fprintf(fp0, "Flushed %d bytes\n\r", ret2);
	  break;
	case 'g':
	  // Read gyros
	  ret = imu_read_gyro(gyro);
	  fprintf(fp0, "%d   %06d %06d %06d\n\r", ret, gyro[0], gyro[1], gyro[2]);
	  //if((ret2 = imu_buffer_check()) > 0)
	  //fprintf(fp0, "Flushed %d bytes\n\r", ret2);
	  break;
	case 'm':
	  // Read gyros
	  //ret = imu_read_magnetometer(mag, fp0);
	  ret = imu_read_magnetometer(mag);
	  
	  fprintf(fp0, "%d   %06d %06d %06d\n\r", ret, mag[0], mag[1], mag[2]);
	  //if((ret2 = imu_buffer_check()) > 0)
	  //fprintf(fp0, "Flushed %d bytes\n\r", ret2);
	  break;
	case 'c':
	  // Read compass
	  val = (int32_t) imu_read_compass();
	  val = (val * 3600) >> 16;
	  fprintf(fp0, "%d\n\r", (int16_t) val);
	  break;
	case 'd':
	  // Read compass
	  fprintf(fp0, "%ld\n\r", (int32_t) imu_read_compass());
	  break;
	  //case's':
	  //imu_check_bus(fp0);
	  //break;
	case 'C':
	  // Calibrate compass
	  imu_calibrate_compass(fp0);
	  break;
	case 'v':
	  // Read compass
	  fprintf(fp0, "%ld\n\r", (10* ((int32_t) imu_read_compass_full()))/182);
	  break;
	case 'V':
	  // Calibrate compass
	  imu_calibrate_compass_full(fp0);
	  break;
	case's':
	  imu_check_bus(fp0);
	  break;
	}
    }
  }
};