/*
	BALANCE.CC

	Controls the balancing pendulum robot, Ballybot.

	Alistair Sutherland
	suthe-aj@ee.uwa.edu.au
	CIIPS, 2003
*/
// Test Gyroscope output
#include <stdlib.h>
#include <string.h>
#include "eyebot.h"
#include "../a-gyro/gyro.h"
#include "motors.h"

// was 40 3.3 3.4 0.6 before the robot was cut in half
#define K1 39.0
#define K2 3.5
#define K3 3.4
#define K4 0.6

#define MAX_STATES_STORED 25
#define SAMPLE_INTERVAL 3


// Prototypes
void SampleState();
void *memset( void *dest, int c, size_t count );


// Global variables (needs to be shared with timer handler)
struct {
	float fAngle;
	float fAngVel;
	float fX;
	float fXVel;
	int   iTime;
} g_StateHistory[MAX_STATES_STORED] = {0};

int g_iSampleCount;
int g_iFirstSample;
int g_iHasBeeped;

int g_iLastTimeStamp;
float g_fLastDisplacement;

// The gyroscope ...
CGyro * g_pGyro;

// The motors ...
CMotors * g_pMotors;
int g_iMotorsOn;

int main( )
{
	int iKey;

	TimerHandle hGyroTimer;

	// Create the motors
	g_pMotors = new CMotors;

	// Init variables
	iKey = 0;
	g_iSampleCount = 0;
	g_iFirstSample = 1; // True initially
	g_iLastTimeStamp = 0;
	g_fLastDisplacement = 0;
	g_iMotorsOn = 1;
	g_iHasBeeped = 0;

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

	// Recalibrate gyro
	g_pGyro->Recalibrate();

	LCDClear();

	// Initialise distance to zero, and get start time
	g_pMotors->Reset();
	memset(&g_StateHistory, 0, sizeof(g_StateHistory));
	g_StateHistory[0].iTime = OSGetCount();

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

	// Now take samples until whenever...
	while ( g_iMotorsOn == 1 && iKey != KEY4)
	{
		// Check for commands:
		iKey = KEYRead();
		if (iKey == KEY4)
		{
			// Stop recording gyro measurments!
			g_pGyro->StopRecording();

			// Motors off!..
			g_iMotorsOn = 0;
			g_pMotors->SetForce(0, 0);

			// stop timer
			OSDetachTimer( hGyroTimer );

			OSWait(2);

			// Make sure motors are off!..
			g_iMotorsOn = 0;
			g_pMotors->Reset();

			// download data (only does something if the gyro was created in DEBUG_ONLINE_MODE mode)
			g_pGyro->DownloadData();

			// you could add code here to transmit the g_StateHistory buffer
		}
	}

	// destroy the gyroscope & motors
	delete g_pGyro;
	delete g_pMotors;
      return 0;
}



//////////////////////////////////////////////////////////
//
// SAMPLE STATE
//
// Read gyroscope and odometers to get state estimates & 
// make control decision
//
void SampleState()
{
	float fForce;

	// Get time ...
	g_StateHistory[g_iSampleCount].iTime = OSGetCount();

	// Get current state:
	g_StateHistory[g_iSampleCount].fAngle =	 g_pGyro->GetAngle();
	g_StateHistory[g_iSampleCount].fAngVel = g_pGyro->GetAngularVel();
	g_StateHistory[g_iSampleCount].fX =		 g_pMotors->GetDistance();

	if (g_iFirstSample)
	{
		g_StateHistory[g_iSampleCount].fXVel = 0.0;
		g_iFirstSample = 0;
	}
	else
	{
		g_StateHistory[g_iSampleCount].fXVel =	(g_StateHistory[g_iSampleCount].fX - g_fLastDisplacement ) / 
							(g_StateHistory[g_iSampleCount].iTime - g_iLastTimeStamp);
	}

	g_iLastTimeStamp = g_StateHistory[g_iSampleCount].iTime;
	g_fLastDisplacement = g_StateHistory[g_iSampleCount].fX;

	// Calculate desired force...
	fForce = g_StateHistory[g_iSampleCount].fAngle * K1 + g_StateHistory[g_iSampleCount].fAngVel * K2 + g_StateHistory[g_iSampleCount].fX * K3 + g_StateHistory[g_iSampleCount].fXVel * K4;

	// Set force...
	if (g_iMotorsOn == 1)
		g_pMotors->SetForce(fForce, g_StateHistory[g_iSampleCount].fXVel);
	else
		g_pMotors->SetForce(0, 0);

	g_iSampleCount++;
	if (g_iSampleCount >= MAX_STATES_STORED) 
	{
		g_iSampleCount = 0; 
	}
}