#include "hardware.h"

//Written by SoR for use with WebbotLib
//June 23, 2010
//http://www.societyofrobots.com
//see comments below to use code
//see .h file for an example of how to process the data


//#define TEST //comment out this line to use sensor; useful for debugging IMU code only

// Initialise the hardware
void appInitHardware(void) {
	initHardware();
}

// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){

	delay_ms(100);//safe bootup time
	LED_on(&statusLED);//indicate powered and ready
	#ifdef TEST
		rprintf("\nready\n");
	#endif

	return 0;
}

GYRO_TYPE gX;
GYRO_TYPE gY;
GYRO_TYPE gZ;
ACCEL_TYPE aX;
ACCEL_TYPE aY;
ACCEL_TYPE aZ;
COMPASS_TYPE ber;
COMPASS_TYPE pit;
COMPASS_TYPE rol;
//raw magnetometer values
//int16_t raw_comp_X;
//int16_t raw_comp_Y;
//int16_t raw_comp_Z;

int cByte;

// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {

	//read all sensors
	gyroRead(pitchRoll);
	gyroRead(yaw);
	accelerometerRead(acceleration);
	compassRead(compass);

	//store sensor data
	gX = pitchRoll.gyro.x_axis_degrees_per_second;
	gY = pitchRoll.gyro.y_axis_degrees_per_second;
	gZ = yaw.gyro.z_axis_degrees_per_second;
	aX = acceleration.accelerometer.x_axis_mG;
	aY = acceleration.accelerometer.y_axis_mG;
	aZ = acceleration.accelerometer.z_axis_mG;
	ber= compass.compass.bearingDegrees;
	pit= compass.compass.pitchDegrees;
	rol= compass.compass.rollDegrees;

	//raw magnetometer values - not used here,
	//but would make a great magnetic field sensor
	//raw_comp_X = compass.rawX;
	//raw_comp_Y = compass.rawY;
	//raw_comp_Z = compass.rawZ

	//process sensor data (so that its ready when called)
		//Kalman Filter goes here (future update)

	#ifdef TEST
		delay_ms(500);//test output
	#endif

	//transmit data when requested
	cByte=uartGetByte(uart0);//store command data

	#ifdef TEST
		rprintf("%d ",cByte);//test output
	#endif

	if(cByte!=-1)//has new data
		{
		if (cByte == '1')//output all data
			{
			//output all sensors, use ';' to signal end
			#ifdef TEST
				rprintfCRLF();//go to next line
			#endif

			rprintf("%d,%d,%d,%d,%d,%d,%d,%d,%d;",gX,gY,gZ,aX,aY,aZ,ber,pit,rol);
			
			#ifdef TEST
				rprintfCRLF();//go to next line
			#endif
			}

		if (cByte == '2')//output in readable form for debugging
			{
			//output all sensors
			rprintf("pitchRoll: ");
			gyroDump(pitchRoll);
			rprintfCRLF();//go to next line

			rprintf("yaw: ");
			gyroDump(yaw);
			rprintfCRLF();//go to next line

			rprintf("accel: ");
			accelerometerDump(acceleration);
			rprintfCRLF();//go to next line

			rprintf("compass: ");
			compassDump(compass);
			rprintfCRLF();//go to next line
			}
			
		if (cByte == '3')//turn LED off
			LED_off(&statusLED);
		if (cByte == '4')//turn LED on
			LED_on(&statusLED);
		}

	return 0;
}