/* this file pulls everything together */
/* the main loop goes here... */
#include "common.h"
#include "input.h"
#include "output.h"
#include "power.h"
#include "interface.h"
#include <delays.h>
#include <math.h>
#include <string.h>
#include <stdio.h>

#define MAG_RESET LATBbits.LATB5
#define MAG_SET LATBbits.LATB2

rom config conf_backup;
ram config conf;

float compass;
float clino;
float heat;
rom float rMc[12]; // stored calibration matrix
rom float rGc[12]; // stored calibartion matrix
float Mc[12] = {1,0,0,
				0,1,0,
				0,0,1,
				0,0,0}; // calibration matrix for Mag sensors
float Gc[12] = {1,0,0,
				0,1,0,
				0,0,1,
				0,0,0}; // calibration matrix for Mag sensors

void Normalise(float *x) {
	float _t;
	_t = sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]); 
	x[0] /= _t; 
	x[1] /= _t; 
	x[2] /= _t;
}

// a = b * c; b is 3v, c is 34m
void Multiply(float *a, float *b, float *c) {
	a[0] = b[0] * c[0] + b[1] * c[3] + b[2] * c[6] + c[9]; 
	a[1] = b[0] * c[1] + b[1] * c[4] + b[2] * c[7] + c[10];
	a[2] = b[0] * c[2] + b[1] * c[5] + b[2] * c[8] + c[11];
} 

// a = b x c
void CrossProduct(float *a, float *b, float *c) {
	a[0] = b[1] * c[2] - b[2] * c[1]; 
	a[1] = b[2] * c[0] - b[0] * c[2]; 
	a[2] = b[0] * c[1] - b[1] * c[0];
}


void calculate_output(void) {
	// Calculate what our bearing and inclination should be...
	float fM[3];
	float down[3];
	float east[3];
	float north[3];
	Multiply(down,G,Gc);
	Normalise(down);
	down[0] *= -1; // I still don't know why I need to do this!
	down[1] *= -1;
	down[2] *= -1;
	Multiply(fM,M,Mc);
	CrossProduct(east,down,fM);
	Normalise(east);
	CrossProduct(north,east,down);
	Normalise(north);
	compass = atan2(east[0],north[0]) * 180 / PI;
	clino = atan2(down[0],sqrt(north[0] * north[0] + east[0] * east[0])) * 180 / PI;
	if (compass < 0) compass += 360;
	Nop()
}

/* these are our state definitions*/
#define OFF -1
#define START_UP 0
#define USB_SETUP 1
#define USB_QUIET 2
#define USB_ACTIVE 3
#define MAIN_SETUP 4
#define MAIN 5
#define LIMBO 6
#define LATCHED 7

#define USB_ON (buttons & 1)
#define USB_OFF (!(buttons & 1))
#define BUT_ON (buttons & 2)
#define BUT_OFF (!(buttons & 2))

void main() {
  signed char state = START_UP;
  signed buttons;
  char text[5] = "SAP ";
  int time = 4;
  /*generic start_up bits go here*/
  while (1) {
    /* stuff to be done on each traverse of state machine */
    switch (state) {
    case START_UP:
      /*prolly not a right load here */
      if (USB_ON) {
	state = USB_SETUP;
      } else {
	state = MAIN_SETUP;
      }
      break;
    case USB_SETUP:
      /* stuff to do while saying hello to Mr USB device... */
      /* may need to wait to get in to USB_QUIET */
      /* turn laser off*/
      /* turn display to dim*/
      state = USB_QUIET;
      if (USB_OFF) state = OFF;
      break;
    case USB_QUIET:
      /* show battery % */
      /* if received hello from usb, go to USB_ACTIVE */
      if (USB_OFF) state = OFF;
      break;
    case USB_ACTIVE:
      /* turn on laser and display to preset values */
      /* display SAP */
      /* read data */
      /* if in debug mode, show last comm */
      /* if received quit, go to USB_QUIET*/
      if(USB_OFF) state = OFF;
      break;
    case MAIN_SETUP:
      /* Start timer */
      /* Turn on laser and display */
      /* Show temp if configured */
      /* Show battery if configured */
      /* if timer expired and BUT_ON, set display and laser to full*/
      /* if timer expired state = MAIN*/
      break;
    case MAIN:
      /* read data */
      /* display it */
      if (BUT_ON) state = LIMBO;
      if (USB_ON) state = USB_SETUP;
      break;
    case LIMBO:
      /* start timer*/
      /* if timer && BUT_ON state = LATCHED */
      /*    and read and latch data       */
      /* if timer && BUT_OFF state = OFF */
      break;
    case LATCHED:
      /* display latched data and leg number */
      if (BUT_OFF) {
	/*copy data to flash*/
	/*leg++ - in flash*/
	state = MAIN;
      }
      break;
    case OFF:
      /* turn off laser and display */
      /* set I2C to off setting */
      while (true) {};
      break;
    default:
      /* print out some error message */
    }
    /* update state of USB and BUT */
  }
  /* Here goes our state machine */
  
	/* restore configuration*/
	/* set IO bits */
	TRISA = 0xff;
	TRISB = 0b11010001;
	TRISC = 0b00111000;
	INTCON2bits.RBPU = 0;
	PORTBbits.RB3 = 1;
	conf=conf_backup;

	Delay10KTCYx(20);

	/* initialise all of our modules... */
	initialise_input();
	initialise_power();
	initialise_output();
	initialise_interface();
	conf.display=0x3f;
	TRISC = 0b00111000;

	/* OK We're approximateley up and running... */
	if (!PORTBbits.RB4) {
		/* we have started up with USB turned on... */
		/* look quiet if started up by USB */
		conf.laser = 0;
		conf.display = 0;
		power_state = 6;
		/* change these when USB says HIYA! */
	}
	write_display(text,1);
	set_laser_brightness();
	if (conf.battery) {
		/* read in batt power */
		/* display battery bpower */
		sprintf(text,"%3d%%",get_charge());
		write_display(text,0);
		Delay10KTCYx(200);
		Delay10KTCYx(200);
		time -= 2;
	}
	if (conf.temperature) {
		/* read in temperature, if directed */
		/* display temperature*/
		sprintf(text,"%3d#",get_temperature());
		write_display(text,0);
		Delay10KTCYx(200);
		Delay10KTCYx(200);
		time -=2;
	}
	while (time > 0) {
		--time;
		Delay10KTCYx(200);
	}
	//we've been on about 1 second
	if (PORTBbits.RB4) {
		/* USB not connected */
		if (power_pressed()) {
			/* move into daylight mode ie v. bright */
			conf.laser = 0xff;
			conf.display = 0x3f;	
		}
	}
	set_laser_brightness();
	write_display(text,0);
	/* setup lasor */
	/* calibrate sensors */
	MAG_RESET = 1;
	MAG_SET = 0;
	Delay10TCYx(2);
	while (1) {
	/* service USB stuff*/
	/* housekeeping...*/
	/* read sensor data...*/
		ServiceUSB();
		set_laser_brightness();
		start_mag_read();
		do_mag_read();
		do_grav_read();
		Nop();
		MAG_RESET = 0;
		Delay10TCYx(2);
		MAG_SET = 1;
		start_mag_read();
		do_mag_read();
		do_grav_read();
		Nop();
		MAG_SET = 0;
		Delay10TCYx(2);
		MAG_RESET = 1;
		Delay10KTCYx(10);
		write_display(text,0);
		}
	shutdown();
	}