/*

	TESTGYRO.CC

	Displays the output of the gyroscope class on a graph.

	Note:  
	If the gyro class is created in NORMAL_MODE -
		g_pGyro = new CGyro(NORMAL_MODE);
	then the gyroscope expects to use an inclinometer to recalibrate the gyro.

	In GYRO_ONLY_MODE -
		g_pGyro = new CGyro(GYRO_ONLY_MODE);
	only the gyroscope sensor is required...but problems with gyro 'drift' will
	be observed.

	Alistair Sutherland
	suthe-aj@ee.uwa.edu.au
	CIIPS, 2003

*/

// Test TPU output
#include <stdlib.h>
#include "eyebot.h"
#include "gyro.h"
#define MAX_SAMPLES 4000
#define MINY 5590
#define MAXY 8000
#define SAMPLE_INTERVAL 3


// Prototypes...
int ScaleDataToScreen64(int iMinY,int iMaxY,int iY);
void SampleState();

// Globals
int g_iSampleNumber;

// Array of angles
int * g_rgiAngles;

// The gyroscope ...
CGyro * g_pGyro;

int main( )
{
	int iKey;

	// Timer to handle gyro samples
	TimerHandle hGyroTimer;

	// Initialise sample arrays

	// Allocate memory for accn sensor samples
	g_rgiAngles = (int *)malloc(MAX_SAMPLES * sizeof(int));
	if (g_rgiAngles == NULL)
	{
		LCDPrintf("Failed to malloc g_rgiAngles\n");
		exit(0);
	}

	// Create the gyroscope (use GYRO_ONLY_MODE if there is no inclinometer)
	g_pGyro = new CGyro(GYRO_ONLY_MODE);
//	g_pGyro = new CGyro(NORMAL_MODE);

	// Recalibrate gyro
	g_pGyro->Recalibrate();

	LCDClear();
	
	iKey = 0;

	// Reset sample number
	g_iSampleNumber = 0;

	// Start timer to handle samples
	hGyroTimer = OSAttachTimer(SAMPLE_INTERVAL, (TimerFnc) (SampleState));

	// Now wait until key 4 is pressed, or we have the maximum...
	while (iKey!=KEY4 && g_iSampleNumber<MAX_SAMPLES)
	{
		OSWait(3);
		iKey = KEYRead();
	}

	// Stop recording:
	g_pGyro->StopRecording();

	// Stop the timer
	OSDetachTimer( hGyroTimer );

	// download data
	g_pGyro->DownloadData();

	// Beep when finished...
	AUBeep();
	LCDPutString("All done!\n");

	// destroy the timer, gyroscope & any alloced memory
	free(g_rgiAngles);
	delete g_pGyro;
	return 0;
}

int ScaleDataToScreen64(int iMinY,int iMaxY,int iY)
{
	return (64-((iY - iMinY) * 64) / (iMaxY - iMinY));
}


//////////////////////////////////////////////////////////
//
// SAMPLE STATE
//
// Read gyroscope to get state estimates & 
// graph angle
//
void SampleState()
{
	int i;

	// Do nothing if we have taken too many samples
	if (g_iSampleNumber>=MAX_SAMPLES) return;

	// Get current angle:
	g_rgiAngles[g_iSampleNumber] =	(int)(g_pGyro->GetAngle()*180.0/PI);

	int iStart = g_iSampleNumber-128;
	if (iStart<0) iStart=0;
	int iEnd = g_iSampleNumber;

	// draw graphics 
	for(i=0; i<iEnd-iStart; i++)
    {
		LCDSetPixel(ScaleDataToScreen64(-90, 90, g_rgiAngles[i + iStart]),i, 1);
		LCDSetPixel(1 + ScaleDataToScreen64(-90, 90, g_rgiAngles[i + iStart]),i, 1);
    }

	// write actual angle
	LCDSetPos(7,0);
	LCDPutString("Ang:");
	LCDPutIntS(g_rgiAngles[g_iSampleNumber],6);

	// clear LCD 
	for(i=0; i<iEnd-iStart; i++)
    {
		LCDSetPixel( ScaleDataToScreen64(-90, 90, g_rgiAngles[i + iStart]),i, 0);
		LCDSetPixel(1 + ScaleDataToScreen64(-90, 90, g_rgiAngles[i + iStart]),i, 0);
    }

	g_iSampleNumber++;
}