// Check for compilation issues with sensors
#define RPRINTF_COMPLEX
#include <sys/Axon.h>
#include <rprintf.h>
#include <Sensors/Acceleration/DynamicEngineering/ACCM3D2.h>
#include <Sensors/Compass/Devantech/CMPS03.h>
#include <Sensors/Compass/Honeywell/HMC6343.h>
#include <Sensors/Current/Phidget/AC20A.h>
#include <Sensors/Current/Phidget/AC50A.h>
#include <Sensors/Current/Phidget/DC20A.h>
#include <Sensors/Current/Phidget/DC50A.h>
#include <Sensors/Distance/Devantech/SRF05_Sonar.h>
#include <Sensors/Distance/Maxbotix/EZ1.h>
#include <Sensors/Distance/Ping/PingSonar.h>
#include <Sensors/Distance/Sharp/GP2D12.h>
#include <Sensors/Distance/Sharp/GP2D120.h>
#include <Sensors/Distance/Sharp/GP2D15.h>
#include <Sensors/Distance/Sharp/GP2Y0A02YK.h>
#include <Sensors/Distance/Sharp/GP2Y0A21YK.h>
#include <Sensors/Encoder/Generic/quadrature.h>
#include <Sensors/Humidity/Phidget/Humidity.h>
#include <Sensors/Temperature/Phidget/Temperature.h>
#include <Sensors/Voltage/Phidget/30V.h>


DE_ACCM3D2 		accm3d2 = MAKE_DE_ACCM3D2(F0,F1,F2);
CMPS03 			cmps03  = MAKE_CMPS03(F4);
Phidget_AC20A   pAC20A  = MAKE_Phidget_AC20A(F5);
Phidget_AC50A   pAC50A  = MAKE_Phidget_AC50A(F6);
Phidget_DC20A   pDC20A  = MAKE_Phidget_DC20A(F7);
Phidget_DC50A   pDC50A  = MAKE_Phidget_DC50A(K0);
Devantech_SRF05 srf05   = MAKE_Devantech_SRF05(K1);
Maxbotix_EZ1    maxb	= MAKE_Maxbotix_EZ1(K2);
PingSonar		ping	= MAKE_PingSonar(K3);
Sharp_GP2D12	gp2d12  = MAKE_Sharp_GP2D12(K4);
Sharp_GP2D120	gp2d120 = MAKE_Sharp_GP2D120(K5);
Sharp_GP2D15	gp2d15  = MAKE_Sharp_GP2D15(K6);
Sharp_GP2Y0A02YK gp2a02 = MAKE_Sharp_GP2Y0A02YK(K7);
Sharp_GP2Y0A21YK gp2a21 = MAKE_Sharp_GP2Y0A21YK(F0);
QUADRATURE		 enc1   = MAKE_GENERIC_QUADRATURE(F1,F2,FALSE,32);
Phidget_Humidity pHumid = MAKE_Phidget_Humidity(F3);
Phidget_Temperature pTemp = MAKE_Phidget_Temperature(F4);
Phidget_30V		p30v    = MAKE_Phidget_30V(F5);
HMC6343			hmc6343 = MAKE_HMC6343();

//initialize hardware, ie servos, UART, etc.
void appInitHardware(void){
	//setup UART on USB, use 1Mbps (if doesn't work, 115200 does!)
	uartInit(UART1, 9600);
	rprintfInit(&uart1SendByte);

	accelerometerInit(accm3d2);
	compassInit(cmps03);
	currentInit(pAC20A);
	currentInit(pAC50A);
	currentInit(pDC20A);
	currentInit(pDC50A);
	distanceInit(srf05);
	distanceInit(maxb);
	distanceInit(ping);
	distanceInit(gp2d12);
	distanceInit(gp2d120);
	distanceInit(gp2d15);
	distanceInit(gp2a02);
	distanceInit(gp2a21);
	encoderInit(enc1);
	humidityInit(pHumid);
	temperatureInit(pTemp);
	voltageInit(p30v);
	compassInit(hmc6343);
}

TICK_COUNT appInitSoftware(TICK_COUNT loopStart){

	return 0;
}

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

	accelerometerRead(accm3d2);
	rprintf("Accm3d2 = %ld,%ld,%ld\n",accm3d2.accelerometer.x_axis_mG,accm3d2.accelerometer.y_axis_mG,accm3d2.accelerometer.z_axis_mG);

	compassRead(cmps03);
	rprintf("CMPS03 = %lu\n",cmps03.compass.bearingDegrees);

	currentRead(pAC20A);
	rprintf("pAC20A = %ld\n",pAC20A.current.amps);

	currentRead(pAC50A);
	rprintf("pAC50A = %lu\n",pAC50A.current.amps);

	currentRead(pDC20A);
	rprintf("pDC20A = %lu\n",pDC20A.current.amps);

	currentRead(pDC50A);
	rprintf("pDC50A = %lu\n",pDC50A.current.amps);

	distanceRead(srf05);
	rprintf("srf05 = %lu\n",srf05.distance.cm);

	distanceRead(maxb);
	rprintf("maxb = %lu\n",maxb.distance.cm);

	distanceRead(ping);
	rprintf("PING = %lu\n",ping.distance.cm);

	distanceRead(gp2d12);
	rprintf("GP2D12 = %lu\n",gp2d12.distance.cm);

	distanceRead(gp2d120);
	rprintf("GP2D120 = %lu\n",gp2d120.distance.cm);

	distanceRead(gp2d15);
	rprintf("GP2D15 = %lu\n",gp2d15.distance.cm);

	distanceRead(gp2a02);
	rprintf("GP2A02 = %lu\n",gp2a02.distance.cm);

	distanceRead(gp2a21);
	rprintf("GP2A21 = %lu\n",gp2a21.distance.cm);

	encoderRead(enc1);
	rprintf("Quadrature = %lu\n",enc1.encoder.value);

	humidityRead(pHumid);
	rprintf("Humidity = %u\n",pHumid.humidity.percent);

	temperatureRead(pTemp);
	rprintf("Temperature = %ld\n",pTemp.temperature.celsius);

	voltageRead(p30v);
	rprintf("Voltage = %ld\n",p30v.voltage.volts);

	compassRead(hmc6343);
	rprintf("HMC6343: heading = %ld, roll = %ld, pitch = %ld\n",hmc6343.compass.bearingDegrees, hmc6343.compass.rollDegrees, hmc6343.compass.pitchDegrees);

	return 0;
}