//-----------------------------------------------------------------------------
// includes
//

#include <p24HJ128GP204.h>
#include <math.h>
#include "DEE Emulation 16-bit.h"
#include "console.h"
#include "i2c.h"
#include "compass.h"


//-----------------------------------------------------------------------------
// defines
//

#define SCALE 2
#define LSM303_MAG  0x3C
#define LSM303_ACC  0x30

#define X 0
#define Y 1
#define Z 2

#define CTRL_REG1_A 0x20
#define CTRL_REG2_A 0x21
#define CTRL_REG3_A 0x22
#define CTRL_REG4_A 0x23
#define CTRL_REG5_A 0x24
#define HP_FILTER_RESET_A 0x25
#define REFERENCE_A 0x26
#define STATUS_REG_A 0x27
#define OUT_X_L_A 0x28
#define OUT_X_H_A 0x29
#define OUT_Y_L_A 0x2A
#define OUT_Y_H_A 0x2B
#define OUT_Z_L_A 0x2C
#define OUT_Z_H_A 0x2D
#define INT1_CFG_A 0x30
#define INT1_SOURCE_A 0x31
#define INT1_THS_A 0x32
#define INT1_DURATION_A 0x33
#define CRA_REG_M 0x00
#define CRB_REG_M 0x01
#define MR_REG_M 0x02
#define OUT_X_H_M 0x03
#define OUT_X_L_M 0x04
#define OUT_Y_H_M 0x07
#define OUT_Y_L_M 0x08
#define OUT_Z_H_M 0x05
#define OUT_Z_L_M 0x06
#define SR_REG_M 0x09
#define IRA_REG_M 0x0A
#define IRB_REG_M 0x0B
#define IRC_REG_M 0x0C

#define eeinit() DataEEInit()
#define eeread(address) DataEERead(address)
#define eewrite(address,data) DataEEWrite(data,address)


//-----------------------------------------------------------------------------
// prototypes
//

void DisplayFloat3p1 (float f);
void DelayTicks (unsigned char ticks);


//-----------------------------------------------------------------------------
// globals
//

// variables for compass bearing eight-point moving average
unsigned char moving = 0;
float ax[8], ay[8], az[8];
float mx[8], my[8], mz[8];

// magnetometer calibration constants
/*
short M_X_MIN = 0xfdc5;
short M_Y_MIN = 0xfd71;
short M_Z_MIN = 0xfd8e;
short M_X_MAX = 0x019f;
short M_Y_MAX = 0x00e7;
short M_Z_MAX = 0x01ab;
*/
short M_X_MIN = 0xfda9;
short M_Y_MIN = 0xfd71;
short M_Z_MIN = 0xfd81;
short M_X_MAX = 0x019f;
short M_Y_MAX = 0x019d;
short M_Z_MAX = 0x01ad;


//-----------------------------------------------------------------------------
// low-level accel, magne, gyro register access functions
//

void WriteGyro (unsigned char addr, unsigned char data)
{
	i2c_start ();
	i2c_send_byte (0xd2);			
	i2c_send_byte (addr);
	i2c_send_byte (data);
	i2c_stop ();
}

unsigned char ReadGyro (unsigned char addr)
{
	unsigned char data;

	i2c_start ();
	i2c_send_byte (0xd2);			
	i2c_send_byte (addr);
	i2c_start ();
	i2c_send_byte (0xd3);			
	data = i2c_read_byte ();
	i2c_stop ();

	return data;
}

void WriteAccel (unsigned char addr, unsigned char data)
{
	i2c_start ();
	i2c_send_byte (0x30);			
	i2c_send_byte (addr);
	i2c_send_byte (data);
	i2c_stop ();
}

unsigned char ReadAccel (unsigned char addr)
{
	unsigned char data;

	i2c_start ();
	i2c_send_byte (0x30);			
	i2c_send_byte (addr);
	i2c_start ();
	i2c_send_byte (0x31);			
	data = i2c_read_byte ();
	i2c_stop ();

	return data;
}

void WriteMagne (unsigned char addr, unsigned char data)
{
	i2c_start ();
	i2c_send_byte (0x3c);			
	i2c_send_byte (addr);
	i2c_send_byte (data);
	i2c_stop ();
}

unsigned char ReadMagne (unsigned char addr)
{
	unsigned char data;

	i2c_start ();
	i2c_send_byte (0x3c);			
	i2c_send_byte (addr);
	i2c_start ();
	i2c_send_byte (0x3d);			
	data = i2c_read_byte ();
	i2c_stop ();

	return data;
}


//-----------------------------------------------------------------------------
// compass_Init
//

void compass_Init (void)
{
	unsigned char i;
	unsigned char a;

	putchar ('$');
	putchar ('I');
	putchar (',');

	// init globals
	moving = 0;
	for (i = 0; i < 8; i++) {
		ax[i] = 0;
		ay[i] = 0;
		az[i] = 0;
		mx[i] = 0;
		my[i] = 0;
		mz[i] = 0;
	}

	// init gyro
	WriteGyro (0x20, 0x0f);
	WriteGyro (0x23, 0x20);

	// init accelerometer
	WriteAccel (0x20, 0x27);
	WriteAccel (0x23, 0x00);

	// init magnetometer
	WriteMagne (0x00, 0x14);
	WriteMagne (0x02, 0x00);

	// display results of writes
	a = ReadGyro (0x20); 
	putstring ("Gyro 0x20: "); puthex8 (a); putstring (" (0f), ");
	a = ReadGyro (0x23); 
	putstring ("Gyro 0x23: "); puthex8 (a); putstring (" (20), ");
	a = ReadAccel (0x20); 
	putstring ("Acce 0x20: "); puthex8 (a); putstring (" (27), ");
	a = ReadAccel (0x23); 
	putstring ("Acce 0x23: "); puthex8 (a); putstring (" (00), ");
	a = ReadMagne (0x00); 
	putstring ("Magn 0x00: "); puthex8 (a); putstring (" (14), ");
	a = ReadMagne (0x02); 
	putstring ("Magn 0x02: "); puthex8 (a); putstring (" (00)");

	// eeinit
	eeinit ();
	
	putchar (0x0d);
	putchar (0x0a);
}


//-----------------------------------------------------------------------------
// compass_Calibrate
//

void compass_Calibrate (void)
{
	short i;
	unsigned char j;
	short min[3];
	short max[3];
	unsigned char m[6];
	short mag[3];

	putchar ('$');
	putchar ('C');
	putchar (',');

	for (i = 0; i < 3; i++) {
		min[i] = 0x7FFF;
		max[i] = 0x8000;
	}

	// one minute
	for (i = 0; i < 300; i++) {	 

		// run at 5 Hz
		DelayTicks (10);

		if ((i % 5) == 0) {
			putchar ('.');
		}

		// wait for magnetometer to be ready
		while (!(ReadMagne(SR_REG_M) & 0x01)) {
		}

		// read magnetometer values
		m[0] = ReadMagne(OUT_X_L_M);
		m[1] = ReadMagne(OUT_X_H_M);
		m[2] = ReadMagne(OUT_Y_L_M);
		m[3] = ReadMagne(OUT_Y_H_M);
		m[4] = ReadMagne(OUT_Z_L_M);
		m[5] = ReadMagne(OUT_Z_H_M);
			
		// convert magnetometer readings to signed short values
		mag[X] = ((m[1] << 8) | m[0]);
		mag[Y] = ((m[3] << 8) | m[2]);
		mag[Z] = ((m[5] << 8) | m[4]);

		// find current maximum / minimum
		for (j = 0; j < 3; j++) {
			if (mag[j] > max[j]) max[j] = mag[j];
			if (mag[j] < min[j]) min[j] = mag[j];
		}
		
	}

	putchar (',');

	// report values
	for (i = 0; i < 3; i++) {
		puthex16(min[i]);
		putchar (',');
	}
	for (i = 0; i < 3; i++) {
		puthex16(max[i]);
		if (i != 2) {
			putchar (',');
		}
	}
	
	// update values
	M_X_MIN = min[0];
	M_Y_MIN = min[1];
	M_Z_MIN = min[2];
	M_X_MAX = max[0];
	M_Y_MAX = max[1];
	M_Z_MAX = max[2];
	
	putchar (0x0d);
	putchar (0x0a);
}


//-----------------------------------------------------------------------------
// compass_TakeMeasurement
//

void compass_TakeMeasurement (void)
{
	unsigned char a[6];
	unsigned char m[6];
	short b[3];
	short mag[3];
	float g[3];
	float c_magnetom_x;
	float c_magnetom_y;
	float c_magnetom_z;

	// read accelerometers
	a[0] = ReadAccel(OUT_X_L_A);
	a[1] = ReadAccel(OUT_X_H_A);
	a[2] = ReadAccel(OUT_Y_L_A);
	a[3] = ReadAccel(OUT_Y_H_A);
	a[4] = ReadAccel(OUT_Z_L_A);
	a[5] = ReadAccel(OUT_Z_H_A);

	// wait for magnetometer to be ready
	while (!(ReadMagne(SR_REG_M) & 0x01)) {
	}

	// read magnetometer values
	m[0] = ReadMagne(OUT_X_L_M);
	m[1] = ReadMagne(OUT_X_H_M);
	m[2] = ReadMagne(OUT_Y_L_M);
	m[3] = ReadMagne(OUT_Y_H_M);
	m[4] = ReadMagne(OUT_Z_L_M);
	m[5] = ReadMagne(OUT_Z_H_M);
		
	// convert accelerometer readings to signed short values
	b[0] = (a[1] << 8) | a[0];
	b[1] = (a[3] << 8) | a[2];
	b[2] = (a[5] << 8) | a[4];

	// then convert them to float values
	g[0] = b[0] / pow(2,15) * 2.0;
	g[1] = b[1] / pow(2,15) * 2.0;
	g[2] = b[2] / pow(2,15) * 2.0;

	// convert magnetometer readings to signed short values
	mag[X] = ((m[1] << 8) | m[0]);
	mag[Y] = ((m[3] << 8) | m[2]);
	mag[Z] = ((m[5] << 8) | m[4]);

	// convert mag readings to float and scale them
	c_magnetom_x = (float)(mag[X] - M_X_MIN) / (M_X_MAX - M_X_MIN) - 0.5;
	c_magnetom_y = (float)(mag[Y] - M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - 0.5;
	c_magnetom_z = (float)(mag[Z] - M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - 0.5;

	// record readings in moving average taps
	ax[moving] = g[0];
	ay[moving] = g[1];
	az[moving] = g[2];
	mx[moving] = c_magnetom_x;
	my[moving] = c_magnetom_y;
	mz[moving] = c_magnetom_z;
	moving++;
	if (moving == 8) moving = 0;
}


//-----------------------------------------------------------------------------
// compass_ReportBearing
//

void compass_ReportBearing (void)
{
	unsigned char i;
	float g[3];
	float c_magnetom_x;
	float c_magnetom_y;
	float c_magnetom_z;
	float pitch, roll, bearing;

	putchar ('$');
	putchar ('B');
	putchar (',');

	// calculate moving average
	g[0] = 0;
	g[1] = 0;
	g[2] = 0;
	c_magnetom_x = 0;
	c_magnetom_y = 0;
	c_magnetom_z = 0;
	for (i = 0; i < 8; i++) {
		g[0] += ax[i];
		g[1] += ay[i];
		g[2] += az[i];
		c_magnetom_x += mx[i];
		c_magnetom_y += my[i];
		c_magnetom_z += mz[i];
	}
	g[0] /= 8;
	g[1] /= 8;
	g[2] /= 8;
	c_magnetom_x /= 8;
	c_magnetom_y /= 8;
	c_magnetom_z /= 8;

	// compute pitch and roll
	pitch = asin (-g[X]);
	roll = asin(g[Y]/cos(pitch));

	// display pitch and roll
	DisplayFloat3p1 (pitch*180/3.1416);
	putchar (',');
	DisplayFloat3p1 (roll*180/3.1416);
	putchar (',');

	// compute bearing
	float xh = c_magnetom_x * cos(pitch) + c_magnetom_z * sin(pitch);
  	float yh = c_magnetom_x * sin(roll) * sin(pitch) + c_magnetom_y * cos(roll) - c_magnetom_z * sin(roll) * cos(pitch);
  	float zh = -c_magnetom_x * cos(roll) * sin(pitch) + c_magnetom_y * sin(roll) + c_magnetom_z * cos(roll) * cos(pitch);

	bearing = 180 * atan2(yh, xh) / 3.1416;
	if (bearing < 0) bearing += 360;

	// compute bearing
	DisplayFloat3p1 (bearing);

	putchar (0x0d);
	putchar (0x0a);
}


//-----------------------------------------------------------------------------
// calibration utilities
//

void compass_SetDefaultCalibration (void)
{
	// set calibration to something reasonable
	putchar ('$');
	putchar ('D');
	putchar ('C');
	M_X_MIN = 0xfda9;
	M_Y_MIN = 0xfd71;
	M_Z_MIN = 0xfd81;
	M_X_MAX = 0x019f;
	M_Y_MAX = 0x019d;
	M_Z_MAX = 0x01ad;
	putchar (0x0d);
	putchar (0x0a);
}


void compass_WriteCalibration (void)
{
	// write calibration to eeprom
	putchar ('$');
	putchar ('W');
	putchar ('C');
	eewrite ( 0, (M_X_MIN >> 8) & 0xff);
	eewrite ( 1,  M_X_MIN       & 0xff);
	eewrite ( 2, (M_Y_MIN >> 8) & 0xff);
	eewrite ( 3,  M_Y_MIN       & 0xff);
	eewrite ( 4, (M_Z_MIN >> 8) & 0xff);
	eewrite ( 5,  M_Z_MIN       & 0xff);
	eewrite ( 6, (M_X_MAX >> 8) & 0xff);
	eewrite ( 7,  M_X_MAX       & 0xff);
	eewrite ( 8, (M_Y_MAX >> 8) & 0xff);
	eewrite ( 9,  M_Y_MAX       & 0xff);
	eewrite (10, (M_Z_MAX >> 8) & 0xff);
	eewrite (11,  M_Z_MAX       & 0xff);
	putchar (0x0d);
	putchar (0x0a);
}


void compass_ReadCalibration (void)
{
	// read calibration from eeprom
	putchar ('$');
	putchar ('R');
	putchar ('C');
	M_X_MIN  = eeread ( 0) << 8;
	M_X_MIN |= eeread ( 1);
	M_Y_MIN  = eeread ( 2) << 8;
	M_Y_MIN |= eeread ( 3);
	M_Z_MIN  = eeread ( 4) << 8;
	M_Z_MIN |= eeread ( 5);
	M_X_MAX  = eeread ( 6) << 8;
	M_X_MAX |= eeread ( 7);
	M_Y_MAX  = eeread ( 8) << 8;
	M_Y_MAX |= eeread ( 9);
	M_Z_MAX  = eeread (10) << 8;
	M_Z_MAX |= eeread (11);
	putchar (0x0d);
	putchar (0x0a);
}


void compass_LoadCalibration (
	short xmin, short ymin, short zmin,
	short xmax, short ymax, short zmax)
{
	// load calibration from command line
	putchar ('$');
	putchar ('L');
	putchar ('C');
	M_X_MIN = xmin;
	M_Y_MIN = ymin;
	M_Z_MIN = zmin;
	M_X_MAX = xmax;
	M_Y_MAX = ymax;
	M_Z_MAX = zmax;
	putchar (0x0d);
	putchar (0x0a);
}


void compass_PrintCalibration (void)
{
	// print calibration to screen
	putchar ('$');
	putchar ('P');
	putchar ('C');
	putchar (',');
	puthex16 (M_X_MIN);
	putchar (',');
	puthex16 (M_Y_MIN);
	putchar (',');
	puthex16 (M_Z_MIN);
	putchar (',');
	puthex16 (M_X_MAX);
	putchar (',');
	puthex16 (M_Y_MAX);
	putchar (',');
	puthex16 (M_Z_MAX);
	putchar (0x0d);
	putchar (0x0a);
}


//-----------------------------------------------------------------------------
// misc utilities
//

void DisplayFloat3p1 (float f)
{
	unsigned char i;

	if (f < 0) {
		f = f * -1;
		putchar ('-');
	} else {
		putchar ('+');
	}

	i= f / 100.0;
	puthex4 (i);
	f = f - 100.0 * i;
	i = f / 10.0;
	puthex4 (i);
	f = f - 10.0 * i;
	i = f / 1.0;
	puthex4 (i);
	f = f - 1.0 * i;
	putchar ('.');
	f = f * 10;
	i = f / 1.0;
	puthex4 (i);
	f = f - 1.0 * i;
}