/* This file deals with collecting input from the sensors... */

/* set up the relevant registers so the sensors are ready*/
/* also calibrate mag sensors */
#include "common.h"
#include "input.h"
#include <adc.h>
#include <i2c.h>
#include <math.h>
#include <delays.h>



float G[3] ; /* most recent sensor readings from accelerometer */
float M[3] ; /* most recent sesnsor readings from magnetometer */
float dip;

void initialise_input(void) {
	SSPSTAT = 0x80;
	SSPCON1 = 0x28;
	SSPADD = 0x18;
	SSPCON2 = 0x00;
	/* set up I2C interface */
	OpenI2C(MASTER,SLEW_OFF);
}

/* tell all the ADCs to begin reading */
void start_adc(unsigned char adc) {
	unsigned char callsign = (adc *2) + 0b10010000;
	IdleI2C();
	StartI2C();
	while (SSPCON2bits.SEN);
	WriteI2C(callsign);
	IdleI2C();
	!SSPCON2bits.ACKSTAT;
	WriteI2C(0b10011111);
	IdleI2C();
	StopI2C();
	while (SSPCON2bits.PEN);
	}

void start_mag_read(void) {
	start_adc(1);
	start_adc(2);
	start_adc(5);
}

/* read value on adc and put it in M[adc] */
/* adc from 0-7 only */
float read_mag_adc(unsigned char adc) {
	unsigned char callsign = (adc * 2) + 0b10010001;
	unsigned char stuff[3];
	signed int result;
	stuff[2] = 0x80;
	while (stuff[2] & 0x80) {
		IdleI2C();
		StartI2C();
		while (SSPCON2bits.SEN);
		WriteI2C(callsign); /* prepare to receive bytes*/
		IdleI2C();
		getsI2C(&(stuff[0]),3); /* read in data */
		NotAckI2C();
		while (SSPCON2bits.ACKEN);
		StopI2C();
		while (SSPCON2bits.PEN);
	}
	result = (int)stuff[1] + ((int)stuff[0] << 8);
	return result;
}

void do_mag_read(void) {
	M[0] = read_mag_adc(1);
	M[1] = read_mag_adc(2);
	M[2] = read_mag_adc(5);
}

signed int read_grav_adc(unsigned char channel) {
	signed int result;
	OpenADC(ADC_FOSC_8 & ADC_RIGHT_JUST & ADC_12_TAD,
			channel & ADC_INT_OFF & ADC_VREFPLUS_VDD & ADC_VREFMINUS_VSS,
			8);
	Delay10TCYx(5);
	ConvertADC();
	while (BusyADC());
	result = (ReadADC() - 0x0200);
	CloseADC();
	return result;
	}
/* read each accelerometer */
void do_grav_read(void) {
	char f;
	G[0] = G[1] = G[2] = 0;
	for(f=0;f < 16; ++f) {
		G[2] += read_grav_adc(ADC_CH0); // read x
		G[0] += read_grav_adc(ADC_CH1); // read y
		G[1] += read_grav_adc(ADC_CH2); // read z
	}
}