/************************************************************
*Servo test platform file - example of initial processing file
*Authors: CM Smith & NE Stamatiou
*
* Blizzard Project 2002
*************************************************************/

#include <stdlib.h>
#include <stdio.h>
#include "eyebot.h"
#include <string.h>
#include <math.h>
#include "irtv.h"
#include <IRnokia.h>

#define MAX_SAMPLES 7000
#define MINY 5500
#define MAXY 8500


#define INIT_CAM_Y_POS 130        /*Initial camera y position*/
#define INIT_CAM_Z_POS 130        /*Initial camera z position*/
#define INIT_CAM_T_POS 130

#define NEUTRAL	152				  /* Neutral Gear */
#define FIRST	160
#define SECOND  165
#define THIRD   170

#define REVERSE1 144	
#define REVERSE2 140
#define REVERSE3 135

#define STRAIGHT 144
#define HALF_LEFT 72
#define FULL_LEFT 0
#define HALF_RIGHT 216
#define FULL_RIGHT 255

#define DELTA 5		/*Steering increment */

#define SERVO_MIN_POSITION 0
#define SERVO_MAX_POSITION 255

#define UP_SCALE 0.813008
#define DOWN_SCALE 0.869565

#define UPPER_BOUND 58

#define LOWER_BOUND -58



#define MIN_SCALE 0.99987

#define MAX_SCALE 1.00023



#define MIN_LIMIT  0.99985

#define MAX_LIMIT  1.00015



int RawData[4];

int raw[MAX_SAMPLES];

int gtime[MAX_SAMPLES];

int iSum,i;



//int stime[MAX_SAMPLES];

//int servo[MAX_SAMPLES];

int gmin[MAX_SAMPLES];

int gmax[MAX_SAMPLES];

int gaverage[MAX_SAMPLES];

int gangle[MAX_SAMPLES];

int gmaverage[MAX_SAMPLES];

int elapsed[MAX_SAMPLES];

int gnorm[MAX_SAMPLES];

int hardcore[MAX_SAMPLES];

int lower[MAX_SAMPLES];

int upper[MAX_SAMPLES];



int max_raw1, min_raw1, min_raw, max_raw;

ServoHandle gyro_servo, gyro_servo2;

int Angle,AveAngVel1;

int SampleTime;

int gsamples;

int rsamples;

int ssamples;

int samples;

int RestRaw[10];



int lower_bound;

int upper_bound;



char newline = '\n';

char term    =  4;



// Prototypes...

int ScaleDataToScreen64(int iMinY,int iMaxY,int iY);

extern int NewDataX;

extern int NewDataY;



 void accinit(void);

 void accrelease(void);

 int  accreadX(void);

 int  accreadY(void);



void remote_control_demo(void){

	int hrs, mins, secs, tics;

	int TimeNow; //, TimeNow2;

        ServoHandle test_servo;

	int ServoWait;

	int speed;

	int step;

	int j;

	int Elapsed;



        speed = 120;

	step = -1;

	j = 0;

 	Elapsed =0;

	ServoWait = 2;



       	/* Initialize servos for the test bracket*/

        if (!(test_servo = SERVOInit(SERVO8))){

		 LCDPrintf("Serv Init Err!\n");

		 }

        else {

		SERVOSet( test_servo, speed);

        	}



	LCDPrintf("\nServo Init\n");

        OSWait(100);

	LCDClear();



	LCDMenu(" ", " ", " ", "END");



        do{

        	do {

			/*Move Servo */

			SERVOSet(test_servo, speed);

			/* Get time and servo and log data */

			OSGetTime( &hrs, &mins, &secs, &tics);

			TimeNow = ((((hrs * 60) + mins) * 60) + secs)* 100 + tics;

			OSWait(ServoWait);



			/*Log data */

			//if (ssamples < MAX_SAMPLES){

			//	stime[ssamples] = TimeNow;

			//	servo[ssamples++] = speed;

			//}



			speed += step;

			//LCDClear();

			LCDSetPos(1, 0);

			LCDPrintf("Servo %3d\nLoop No %2d\n", speed, j);

        	/* Loop to end of servo movement */

        	} while ( ((speed >0) && (step == -1)) || ((speed <255) && (step == 1)) );

		



		OSWait(1500);

		step *= -1;

		//if ((ServoWait > 0) && (step == 1)) ServoWait = 2

		//if (step == -1) ServoWait = 10;

		if (ServoWait > 1) ServoWait--;

		j++;

	

	}while (j < 15);

	AUBeep();

	SERVORelease(test_servo);

}



void InitGyro(void){

	//int iSum;

	int AngVel;

	//int i;



	/* Initialize servos for the first gyro*/

        if (!(gyro_servo = SERVOInit(SERVO4))){

		 LCDPrintf("Gyro Serv Err!\n");

		 }

        else {

		SERVOSet(gyro_servo, 128);  //was 128 standard

  		LCDPrintf("Gyro Serv OK\n");

		}



        OSWait(100);

	

	accinit();

	accrelease();

	accinit();



	

	// Get mean reading with zero velocity

	iSum = 0;

	min_raw = 999999;

	max_raw =0;



	for (i=0; i<10; i++){

		// Get new sample

		do{

			AngVel = accreadX();

		}while ( AngVel==-1 );



		if (AngVel< min_raw) min_raw = AngVel;

		if (AngVel> max_raw) max_raw = AngVel;

		iSum += AngVel;

	}



	AveAngVel1 = iSum/10;

	min_raw *= MIN_SCALE;

	max_raw *= MAX_SCALE;

	min_raw1 = min_raw;

	max_raw1 = max_raw;



	for( i=0; i<4; i++) RawData[i] = AveAngVel1;

	rsamples =0;

	LCDPrintf("Gyro Init Fin\n");

}





void GetGyroData(void){

	int AngVel;

	int hrs, mins, secs, tics;

	int TimeNow, Elapsed;



	AngVel = accreadX();

	OSGetTime( &hrs, &mins, &secs, &tics);



	if ( AngVel > - 1){

		

		iSum =0;



		/*Record initial raw value */

		if (gsamples < MAX_SAMPLES)raw[gsamples]= AngVel;

		

		if (rsamples >=4) rsamples = 0;

		RawData[rsamples++] = AngVel;

		

		/*Calculate 4pt moving average of raw data */

		for (i = 0; i<4; i++) iSum += RawData[i];

		iSum = iSum/4;

		AngVel = iSum;

 	

		/*log Eyebot created moving average */

		if (gsamples < MAX_SAMPLES)gmaverage[gsamples]= AngVel;



		TimeNow = ((((hrs * 60) + mins) * 60) + secs)* 100 + tics;

		if (SampleTime == 0) SampleTime = TimeNow;

		Elapsed = TimeNow - SampleTime;



		if(( AngVel >= min_raw) && (AngVel <= max_raw)){

			/*Limit the variation in the average as it moves over time*/

			if(( AngVel >= AveAngVel1*MIN_LIMIT) && (AngVel <= AveAngVel1*MAX_LIMIT))RestRaw[samples++] = AngVel;

			AngVel =0;



			/*Recalculate Data */

			if (samples == 10){

				iSum = 0;

				min_raw = 999999;

				max_raw =0;



				for (i=0; i<10; i++){

					if (RestRaw[i]< min_raw) min_raw = RestRaw[i];

					if (RestRaw[i]> max_raw) max_raw = RestRaw[i];

					iSum += RestRaw[i];

				}

			

				AveAngVel1 = iSum/10;

				min_raw *= MIN_SCALE;

				max_raw *= MAX_SCALE;

				samples =0;	

			}

		}

		else{

 			AngVel = AngVel - AveAngVel1;

			/*Upper and Lower bound calculations */

						

			//samples = 0;

			if( AngVel > 0){

				upper_bound = (upper_bound + AngVel)/2;

				}



			else{

				lower_bound = (lower_bound + AngVel)/2;

				}



		} /*Move bracket down after logging done */



		

		/*Record Normalised value */

		if (gsamples < MAX_SAMPLES)gnorm[gsamples]= AngVel;

		

		if (AngVel < lower_bound)AngVel *= DOWN_SCALE;

		if (AngVel > upper_bound)AngVel *= UP_SCALE;

		



		Angle += (AngVel * Elapsed);

		SampleTime = TimeNow;

		

		/*Log data points */

		if (gsamples < MAX_SAMPLES){

			gtime[gsamples] = TimeNow;

			elapsed[gsamples] = Elapsed;

			gmin[gsamples] = min_raw;

			gmax[gsamples] = max_raw;

			hardcore[gsamples] = AngVel;

			gaverage[gsamples] = AveAngVel1;

			gangle[gsamples] = Angle;

			lower[gsamples] = lower_bound;

			upper[gsamples] = upper_bound;

			gsamples++;

			LCDSetPos(0,0);

			LCDPrintf("gsamples %5d\n", gsamples);

		}

	}

}



void sendstr(char s[]){

	int i;

		i = 0;

	while (s[i] != 0){

		OSSendRS232(&s[i], SERIAL1);

		i++;

	}

}



static void SendCharacter(char character)

{

  int result;

  do

  {

    result = OSSendCharRS232(character, SERIAL1);

    if(result == 3)

    {

      LCDClear();

      LCDPrintf("Timeout error.\nRetrying...\n");

      OSWait(50);

      LCDClear();

    }

  } while(result == 3);

}



static void SendInteger(int integer){

	int i;

	double temp;

	int last;



/*Handle negative integers */

if (integer < 0){

	SendCharacter('-');	

	integer *= -1;

	}

	

if (integer == 0){

	SendCharacter('0');

	}

else{

	temp = integer + 0.1;



	i = 0;

	while( temp > 1){

		temp = temp / 10;

		i++;

	}

	temp = temp * 10;



	while (i > 0){

		last = temp;

		temp = (temp - last)* 10;

		SendCharacter( last + '0');

		i--;

	}

  }



  //int shift;

  //for(shift = 24; shift >= 0; shift -= 8)

  //SendCharacter((integer >> shift) & 0xff);



}



int main(void){

	int max_samples;

	int iKey;

	//int i;

	TimerHandle gyro_timer;

	char temp[150];



	/*Initialise globals*/

	SampleTime = 0;

	Angle = 0;

	gsamples = 0;

	ssamples = 0;

	samples = 0;



	/*Initialise Gyroscope for TPU 7*/

	InitGyro();



	LCDPrintf("Reading TPU 6\n");

	LCDMenu("STRT","STRT", "STRT", "STRT");



	// Wait for KEY1 before starting

	iKey = KEYGet();

	iKey = 0;



	LCDClear();



	gyro_timer = OSAttachTimer(10,GetGyroData);

	if (gyro_timer ==0){

		LCDPrintf("Timer ERR\n");

	exit(0);

	}



	LCDPrintf("Timer Set\n");

	LCDPrintf("Logging\n");

	LCDMenu("", "", "", "");

	

	/* Loop a number of times - logging data */

 	remote_control_demo();

	

	OSDetachTimer(gyro_timer);

	LCDPrintf("\nFinished\n");

	OSWait(50);

	LCDMenu(" ", " ", "UL", "END");

	

	iKey = KEYGet();



	if (iKey == KEY3){

		OSInitRS232(SER115200, NONE, SERIAL1);



		//sprintf(temp, "Gyro Average %d, min %d, max %d \n", AveAngVel1, min_raw1, max_raw1); 

		//sendstr(temp);

		

		/*Record sample data */

		//sprintf(temp, "Gyro Samples %d\n", gsamples);

		//sendstr(temp);



		sprintf(temp, "Gtime, Raw, Average, 4pt Average, Min, Max, Normalise, Hardcore, Elapsed, Angle, Upper, Lower \n");

		sendstr(temp);	

		

		/* stime servo - ssamples

  		   angle, raw, gtime, elapsed - gsamples */

		if(ssamples < gsamples){

				max_samples = gsamples;

			}

		else{

				max_samples = ssamples;

			}



		//for (i = 0; i <gsamples; i++){

		for (i = 0; i < max_samples; i++){

			/*Send Servo Data*/

			//if (i < ssamples)SendInteger(stime[i]);

			//SendCharacter(',');

			//if (i < ssamples)SendInteger(servo[i]);

			//SendCharacter(',');



			/*Send Gyro Data*/

			if (i < gsamples)SendInteger(gtime[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(raw[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(gaverage[i]);

			SendCharacter(',');			

			if (i < gsamples)SendInteger(gmaverage[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(gmin[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(gmax[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(gnorm[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(hardcore[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(elapsed[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(gangle[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(upper[i]);

			SendCharacter(',');

			if (i < gsamples)SendInteger(lower[i]);

			SendCharacter('\n');

			

			LCDSetPos(5, 0);

			LCDPrintf("%d sent\n", i);

		}



		LCDPrintf("Upload Done\n");

		AUBeep();

		/* terminate transmission */

		OSSendRS232(&term, SERIAL1);

	}



 	SERVORelease(gyro_servo);



return 0;



}