/* ----------------------------------------------------------------- */
/* 'ZFeetClass.cc'										    */
/* Centre of Gravity Calculating anf Feet Sensor- Handling	         */
/* 													    */
/* 	last modified 07/04/2003								    */
/* 	used to calculate COG and read feet sensors of andy		    */
/* 													    */
/* 													    */
/* 													    */
/* 													    */
/* 	(C) Jochen Zimmermann								    */
/* ----------------------------------------------------------------- */

#include "ZHeaders.h"
/*
//Headerfile is included by ZHeaders.h
#include "ZFeetClass.h"
*/

//Single Instance is globally created
Feet Feet::SingleInstance;

//constructor
//initialises members
Feet::Feet()
{
	//constructor
	cognew[0]= 6.3;
	cognew[2]=-6.3;
	cognew[1]= cognew[3]= 0.0;
	feetdatalifted[0]=383;
	feetdatalifted[1]=331;
	feetdatalifted[2]=368;
	feetdatalifted[3]=391;
	feetdatalifted[4]=386;
	feetdatalifted[5]=385;
	for(int i=0;i<6;i++) feetdata[i]=oldfeetdata[i]=0;
	deadbandwidth=2;
	liftsensitivity=10;
	readings=800;
}

//instance retirever
Feet* Feet::GetTheInstance()
{
	return &SingleInstance;
}


//initialising feet
bool Feet::Init ()
{
	//analog Inputs are read to static member-buffer
	//for reference- values while lifted
	int mean=0,i;
	cognew[0]= 6.3;
	cognew[2]=-6.3;
	cognew[1]= cognew[3]= 0.0;	
	deadbandwidth=0;
	readings=8;		//generates a readings*(10.66µs+1,74µs) waitstate
	liftsensitivity=10;
	for(i=0;i<6;i++)
	{
		OSGetAD(i+2);
		OSWait(1);
		feetdatalifted[i]=OSGetAD(i+2); 
		feetdata[i]=0;
		oldfeetdata[i]=0;
		mean+=feetdatalifted[i];
	}
	if((mean/6)>390)	//override read values if andy was not lifted
	{
		Display *lcd=Display::GetTheInstance();
		lcd->Print("Bad feet-read!");
		feetdatalifted[0]=383;
		feetdatalifted[1]=331;
		feetdatalifted[2]=368;
		feetdatalifted[3]=391;
		feetdatalifted[4]=386;
		feetdatalifted[5]=385;
		return false;
	}
	else	return true;
}


//read sensors
void Feet::Read()
{	
	//analog Inputs are read to member-buffer
	int i,j;
	for(i=0;i<6;i++)
	{
		feetdata[i]=OSGetAD(i+2); //force multiplexer to switch channel
		for(j=0;j<readings;j++) OSWait(0);	//wait to validate ad-converted value
									//generates a 80*(10.66µs+1,74µs) waitstate
		feetdata[i]=OSGetAD(i+2); //get ad-converted value
		if( ( (feetdata[i]-oldfeetdata[i])<( deadbandwidth)&&(feetdata[i]-oldfeetdata[i])>(-deadbandwidth)  )  )
			feetdata[i]=oldfeetdata[i];
		else
			oldfeetdata[i]=feetdata[i];			
	}
}


//empty buffer
void Feet::ZeroMemory(BYTE* pointer, int count)
{
	int i=0;
	for(i=0;i<count;i++)
	{
		pointer[i]=0;
	}
}


//refresh  sensor values and preprocessed data
void Feet::RefreshCOG()
{
	int i;
	//The Toe-Correctors tries to adjust the sensitiveness of the strain gages. These Values result of measurements made at 20/12/2002
	float toecorr[6]={CORT1,CORT2,CORT3,CORT4,CORT5,CORT6};
	
	Read();
			
	for(i=0;i<6;i++)
	{
		toes[i]=(float)(feetdata[i]-feetdatalifted[i])*TOEMEAN*toecorr[i]; 
	}
	//trigonometric value depend on angles of the strain gates. See file "Schwerpunkt.xls"
	cognew[0]=((toes[0])*COST1+(toes[1])*COST2+(toes[2])*COST3)/3.0; //x1 = left foot
	cognew[1]=((toes[0])*SINT1+(toes[1])*SINT2+(toes[2])*SINT3)/3.0;	//y1   adjusted by setup angle
	cognew[2]=((toes[3])*COST4+(toes[4])*COST5+(toes[5])*COST6)/3.0;	//x2	= right  foot
	cognew[3]=((toes[3])*SINT4+(toes[4])*SINT5+(toes[5])*SINT6)/3.0;	//y2   adjusted by setup angle	
}


//retrieve cog
bool Feet::GetCOG(double* buf)
{
	//function is public access to centre of gravity
	//if ((sizeof(buf)/sizeof(float))<4) return false;
	RefreshCOG();
	buf[0]=cognew[0];//left x
	buf[1]=cognew[1];//left y
	buf[2]=cognew[2];//right x
	buf[3]=cognew[3];//right y
	
	return true;
}


//retrieve cog in vector
LinearAlgebra Feet::GetCOG(LinearAlgebra& buf, int left,bool refresh)
{
	//function is public access to centre of gravity
	//if ((sizeof(buf)/sizeof(float))<4) return false;
	
	
	double temp[4];
	left=(left+1)%2;	//odd is left, even is right
	if(refresh) RefreshCOG();
	
	temp[0]=0;
	temp[1]=cognew[0+2*left];	//x is y
	temp[2]=cognew[1+2*left];	//y is z
	temp[3]=1;
	buf.Equals(temp,4,1);
	return buf;
}


//calculate, refresh, and retrieve load distribution
bool Feet::LoadDistribution(double* left,double* right,double* total,bool refresh)
{
	int i;
	double lleft=0.0,lright=0.0;
	if(refresh) RefreshCOG();
	for(i=0;i<3;i++)
	{
		lleft+=(double)(feetdata[i]-feetdatalifted[i]);
		lright+=(double)(feetdata[i+3]-feetdatalifted[i+3]);
	}
	
	*total=(lleft+lright);
	*left =lleft /(*total);
	*right=lright/(*total);
	return true;
}


//determine if feet are lifted
int  Feet::Lifted(bool refresh)
{
	//function determins the state of each foot of either being 
	//lifted or attached to the ground
	//member 'liftsensitivity' is used to control the sensitivity
	//input 'bool refresh': determins wether sensor should be read 
	//				    or older values should be processed
	//return values are  0: both feet attached to the ground
	//				 1: left  foot lifted
	//				 2: right foot lifted
	//				 3: both  feet lifted
	
	int i,lleft=0,lright=0;
	if (refresh) RefreshCOG();
	for(i=0;i<3;i++)
	{
		lleft+=feetdata[i]-feetdatalifted[i];
		lright+=feetdata[i+3]-feetdatalifted[i+3];
	}
	i=0;
	if(lleft <liftsensitivity) i+=1;
	if(lright<liftsensitivity) i+=2;
	return i;
}