/*

	GRAPHTPU.CC

	Prints the output of TPU 13 to a graph.

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

*/

// Test TPU output
#include <stdlib.h>
#include "eyebot.h"
extern "C"
{
#include "ppwa.h"
}
#define MAX_SAMPLES 4000
#define MINY 6000
#define MAXY 9000

#define FREE_EXIT(x)							\
	{											\
		if (rgiTPU13)   free(rgiTPU13);			\
		return (x);								\
	}

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

int main( )
{
	int iKey;
	int iSampleNumber;
	int iStart;
	int iEnd;
	int i;

	// Initialise sample arrays
	int * rgiTPU13;

	// Allocate memory for accn sensor samples
	rgiTPU13 = (int *)malloc(MAX_SAMPLES * sizeof(int));
	if (rgiTPU13 == NULL)
	{
		LCDPrintf("Failed to malloc rgiTPU13\n");
		FREE_EXIT(-1);
	}

	// Start writing 50% to SERVO S11 (this is unreltated to TPU13)
	ServoHandle hServo = SERVOInit(SERVO11);
	SERVOSet(hServo, 127);

	
	LCDPrintf("Graph TPU13:\n");
	LCDPrintf("  KEY1 start\n");
	LCDPrintf("  KEY4 stop\n");

	// Wait for KEY1 before starting
	iKey = KEYGet();
	iKey = 0;

	// Beep three times before starting
	AUBeep();
	OSWait(100);
	AUBeep();
	OSWait(100);
	AUBeep();

	LCDClear();

	tpuinit();
	tpurelease();
	tpuinit();

	// Get TPU reading of the first sample
	iSampleNumber = 0;
	
	rgiTPU13[iSampleNumber] = tpuread13();
	OSWait(100);

	for ( rgiTPU13[iSampleNumber]=-1; rgiTPU13[iSampleNumber]<0; rgiTPU13[iSampleNumber] = tpuread13()) 
	{
		OSWait(10);
	}
	iSampleNumber++;

	// Now take samples until the key is pressed again, or we have the maximum...
	while (iKey!=KEY4 && iSampleNumber<MAX_SAMPLES)
	{
		// Get TPU reading
		for ( rgiTPU13[iSampleNumber]=-1; rgiTPU13[iSampleNumber]<0; rgiTPU13[iSampleNumber] = tpuread13()) ;
			
		iStart = iSampleNumber-128;
		if (iStart<0) iStart=0;
		iEnd = iSampleNumber;

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

		// write actual distance and raw-data
		LCDSetPos(7,0);
		LCDPutString("TPU:");
		LCDPutIntS(rgiTPU13[iSampleNumber],6);

		OSWait(10);

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

		iSampleNumber++;

		iKey = KEYRead();
	}

	// Beep when finished...
	AUBeep();	
	FREE_EXIT (0);

	// Release the servo
	SERVORelease(SERVO11);
}

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