/***********************************************************

* Inclinometer Handling functions

*

* Authors: CM Smith & NE Stamatiou

* 

* Blizzard Project 2002

*

************************************************************/



#include "inclino.h"



#define MIN_LIMIT  0.99985

#define MAX_LIMIT  1.00015

#define MIN_SCALE  0.9963

#define MAX_SCALE  1.0034



/* Zero degree limits  - applied to sampled min and max during recalculation */

#define IMAX_SCALE	1.005	

#define IMIN_SCALE	0.995



/* Resampling limits */

#define IULIMIT		1.004

#define ILLIMIT		0.996



/* Noise error bounds */	

#define NOISE_LOW  	75

#define NOISE_HIGH 	75



/*Data stability monitoring threshold - no longer used */

#define INC_THRESHOLD	6



/*Number of data points required for resampling */

#define RAW_NUM		5



int IncRawData[4];

int Sum,k;

int IRestRaw[RAW_NUM];

int inc_min_raw, inc_max_raw;

int IncAngle,AveIncVal;

int irsamples, i_rest_samples;





/*Inclino noise reduction variables - no longer required*/

int CurrentPoint, LastPoint, NextPoint;



/*Translate inclinometer stored angular value into a real inclination */

double GetIAngle(void){

	return (IncAngle * IANGLE_SCALE);

}





/* Inclinometer initialisation routine 

 * Routine statically sets factor to bounds gained from experimentation

 * due to noise at program startup

 */

void InitInclino(void){

	int IncVal;





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

		// Get new sample

		do{

			//Get Inclinometer data from channel 2

			IncVal = OSGetAD(2);

		}while (( IncVal==-1 ) || (IncVal > 800));

	//	LCDPrintf("Val %k    ", IncVal);

	//	initdat[k] = IncVal;

	//	if (IncVal< inc_min_raw) inc_min_raw = IncVal;

	//	if (IncVal> inc_max_raw) inc_max_raw = IncVal;

	//	Sum += IncVal;

	}



	//AveIncVal = Sum/10;

	//AveIncVal *= 1.0165;

	//inc_min_raw *= MIN_SCALE;

	//inc_max_raw *= MAX_SCALE;



	/*Initialisation of inclinometer level characteristics */

	AveIncVal = 615;

	inc_min_raw = 612;

	inc_max_raw = 618;

	LastPoint = 615;

	CurrentPoint = 615;

	for( k=0; k<4; k++) IncRawData[k] = AveIncVal;

	

	StableVal = AveIncVal;

	StableNum =0;

	irsamples =0;

	isamples = 0;

	i_rest_samples = 0;

	LCDPrintf("Inc Init Fin\n");

}





/*Retrieve Inclinometer data and process*/

void GetInclinoData(void){

	int IncVal;

	int ihrs, imins, isecs, itics;

	int iTimeNow;





	// Read inclinometer data from channel 2

	IncVal = OSGetAD(2);

	OSGetTime( &ihrs, &imins, &isecs, &itics);



	if ( IncVal > - 1){



		Sum =0;



		/*Record initial raw value */

		if (isamples < MAX_SAMPLES)iraw[isamples]= IncVal;



		/*Remove noisy data points - take previous good value*/

		if ((IncVal < (AveIncVal - NOISE_LOW)) || (IncVal > (AveIncVal + NOISE_HIGH))){

			if (irsamples == 0){

				IncVal = IncRawData[3];

				}

			else IncVal = IncRawData[irsamples -1];

			}



		/* Retrieve last two points for comparison*/

		//NextPoint = IncVal;



		//if((((LastPoint - CurrentPoint) >INC_THRESHOLD ) || ((CurrentPoint - LastPoint) >INC_THRESHOLD))

		// &&(((NextPoint - CurrentPoint) >INC_THRESHOLD ) || ((CurrentPoint - NextPoint) >INC_THRESHOLD))){

		// 	CurrentPoint = AveIncVal;

		//	}

		//else {

		//	/*No Action required ?*/

		//	}



		/*Log data */

		//if (isamples < MAX_SAMPLES){

		//	ilast[isamples] = LastPoint;

		//	icurrent[isamples] = CurrentPoint;

		//	inext[isamples] = NextPoint;

		//	}



		/*Used to be after the update */

		/*Save to RAW data points for 4PT average */

		if (irsamples >=4) irsamples = 0;

		IncRawData[irsamples++] = IncVal;

		//IncRawData[irsamples++] = CurrentPoint;



		/*Update current point for next iteration */

		//LastPoint = CurrentPoint;

		//CurrentPoint = NextPoint;





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

		for (k = 0; k<4; k++) Sum += IncRawData[k];

		Sum = Sum/4;

		IncVal = Sum;



		/*Handle Inclinometer Noise */

		if ((IncVal >= StableVal  - 1) && (IncVal <= StableVal + 1)){

			StableNum++;

			}

		else {

			StableVal = IncVal;

			}

		

		/*log Eyebot created moving average */

		if (isamples < MAX_SAMPLES)imaverage[isamples]= IncVal;



		iTimeNow = ((((ihrs * 60) + imins) * 60) + isecs)* 100 + itics;



		//use inc_min_raw and inc_max_raw and then use the AveIncVal * to adj average

		if(( IncVal >= inc_min_raw) && (IncVal <= inc_max_raw)){

			if(( IncVal >= (AveIncVal*ILLIMIT)) && (IncVal <= (AveIncVal * IULIMIT))){

				IRestRaw[i_rest_samples++] = IncVal;





				/*Recalculate the rest characteristics */

				if (i_rest_samples == RAW_NUM){

					Sum = 0;

					inc_min_raw = 999999;

					inc_max_raw =0;



					for (k=0; k<RAW_NUM; k++){

					Sum += IRestRaw[k];

					}



					AveIncVal = Sum/RAW_NUM;

					inc_min_raw = AveIncVal * IMIN_SCALE;

					inc_max_raw = AveIncVal * IMAX_SCALE;

					i_rest_samples =0;

				}

			}

			/*Considered to be level */

			IncVal =0;

		}

		else{

			i_rest_samples =0;

			/*Perform translation of angle to align readings around zero degree axis*/

 			IncVal = IncVal - (AveIncVal*IAVE_SCALE);



		} /*Move bracket down after logging done */



		IncAngle = IncVal;



		/*Log data points for later analysis*/

		if (isamples < MAX_SAMPLES){

			itime[isamples] = iTimeNow;

			imin[isamples] = inc_min_raw;

			imax[isamples] = inc_max_raw;

			iangle[isamples] = IncVal * IANGLE_SCALE;

			iaverage[isamples] = AveIncVal;



			isamples++;

		}

	}

}