/* Sim.c 

 * Simulate Eyebot running gyroscope processing routines based upon actual gyroscope raw values

 * from specified input file and stored resultant data in specified output file

 * 

 * USAGE: sim input.dat output.dat

 *

 * AUTHOR: CM Smith

 */



#include "sim.h"



int iSum, rsamples, max_raw, min_raw;

int RawData[4];

int RestRaw[10];

int samples;





void ProcessGyroData(int AngVel){

		int i;



		iSum =0;

		if (rsamples >=AVERAGE_SIZE) rsamples = 0;



		/*Take care of spikes in the raw values to remove

		  effect on the moving average */

		//if (AngVel > (max_raw * 1.02) ){

		//	AngVel = (AngVel - max_raw)/1.5 + max_raw;

		//	}

		//else if (AngVel < (min_raw * 0.985)){

		//	AngVel = min_raw - (min_raw - AngVel)/1.5;

		//	}



		RawData[rsamples++] = AngVel;



		/*Calculate AVERAGE_SIZE moving average of raw data */

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

		iSum = iSum/AVERAGE_SIZE;

		AngVel = iSum;



		/*log Eyebot created moving average */

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



	

		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 = (3*upper_bound + AngVel)/4;

			//	}



			//else{

			//	lower_bound = (3*lower_bound + AngVel)/4;

			//	}



		} /*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;

		//if (gsamples < MAX_SAMPLES)oldAngle[gsamples] = Angle;

		Angle += (AngVel * 10);



		/*Log data points */

		if (gsamples < MAX_SAMPLES){

			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++;

		}

}



/*Open file - pass gyro data for handling*/

/*Place results in output file */

int main(int argc, char *argv[]){

	FILE *out;

	FILE *in;

	int i,j;

	int entries;

	char c;



gsamples =0;

Angle = 0;



/* check for arguments */

if (argc != 3){

		printf( "Input and output files not correctly specified\n"); 

		return 1;

	}

else {

	printf("Opening input file %s\n", argv[1]);

	in = fopen(argv[1], "r");

	if(in == NULL){

		printf("Cannot open file!!\n");

		return 1;

	}

}





/*Read in the necessary stats*/

fscanf(in,"%d,%d,%d\n", &AveAngVel1, &min_raw, &max_raw);

printf("Average %d, Min %d, Max %d\n", AveAngVel1, min_raw, max_raw);



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

printf("Average is %d\n", AveAngVel1);

printf("Max is %d\n", min_raw);

printf("Min is %d\n", max_raw);



printf("Initial Angle is %d\n", Angle);



i =0;

printf("Finished initialisation\n ");



c = fgetc(in);

printf("Got the char\n");

while (c!= (char)EOF ){

	ungetc(c, in);

	j = 0;

	while(j < 2){

		c = fgetc(in);

		if(c ==',') j ++;

	}

	fscanf(in, "%d, %d\n", &gtime[i], &graw[i]);

	printf("%d: %d, %d\n",i, gtime[i], graw[i]);

	ProcessGyroData(graw[i]);

	i++;

	c = fgetc(in);

}



entries = i;

printf("Entries scanned %d\n", entries);

printf("Closing input file\n");

fclose(in);



printf("Opening output file: %s \n", argv[2]);

out = fopen(argv[2], "w");

if(out == NULL){

		printf("ERROR: Cannot open output file!!\n");

		return 1;

}



printf("Writing to output file\n");

fprintf(out,"\n\n\n\n Gtime, Raw, 4PT, Min, Max, Average, Norm, Hardcore, Angle\n");

i=0;

while(i < entries){

	fprintf(out, "%d, %d, %d, %d, %d, %d, %d, %d, %d\n", gtime[i], graw[i], gmaverage[i], gmin[i],gmax[i], gaverage[i], gnorm[i], hardcore[i], gangle[i] );

	i++;



}



printf("Finished writing output file\n");

fclose(out);

printf("\nAll done!\n");



return 0;

}