/* ----------------------------------------------------------------- */
/* 'ZControlClass.cc'									    */
/* Control- Class									         */
/* 													    */
/* 	last modified 07/04/2003								    */
/* 	comprises methods for balancing (interruot-driven)		    */
/* 													    */
/* 													    */
/* 													    */
/* 													    */
/* 	(C) Jochen Zimmermann								    */
/* ----------------------------------------------------------------- */


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

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


//constructor
//initialises members
Control::Control()
{
	double temp[4]={0,0,0,1};
	int i;
	
	measuredcog.Equals(temp,4,1);
	virtualcog.Equals(temp,4,1);
	lcog.Equals(temp,4,1);
	rcog.Equals(temp,4,1);
	
	roll=
	pitch=
	deltay=
	deltaz=
	lastdeltay=
	lastdeltaz=
	onebeforelastdeltay=
	onebeforelastdeltaz=0;
	
	proll =0.1;
	ppitch=proll;
	iroll = 1.5;
	ipitch=iroll;
	droll =0.0;
	dpitch=droll;
		
	for(i=0;i<NUMBEROFSERVOS;i++)
	{
		deltaservo[i]=olddeltaservo[i]=0;
		virtualdelta[i]=0;
	}
	speed=10;
	
	load=0.5;
	
	InterruptHandle=0;
	interrupt_active=false;

}


//instance retriever
Control* Control::GetTheInstance()
{
	return &SingleInstance;
}


//initialisations similar to those made in constructor (necessary!)
void Control::Init()
{
	double temp[4]={28,0,0,1};
	int i;
	
	measuredcog.Equals(temp,4,1);
	virtualcog.Equals(temp,4,1);
	temp[1]=28;
	lcog.Equals(temp,4,1);
	temp[1]=-28;
	rcog.Equals(temp,4,1);
		
	
	roll=
	pitch=
	deltay=
	deltaz=
	lastdeltay=
	lastdeltaz=
	onebeforelastdeltay=
	onebeforelastdeltaz=0;
	
	proll =0.02;
	ppitch=proll;
	iroll = 1.5;
	ipitch=iroll;
	droll =0.0;
	dpitch=droll;
	
	for(i=0;i<NUMBEROFSERVOS;i++)
	{
		deltaservo[i]=olddeltaservo[i]=0;
		virtualdelta[i]=0;
	}
	speed=10;
	
	load=0.5;
	
	InterruptHandle=0;
	interrupt_active=false;

}


//static member function to override the fact, that OSAttachTimer() 
//only accepts adresses of C-funcions. Static member functions are accepted
void Control::InterruptBalance()
{
	SingleInstance.Balance();
}


//method to activate interrupt
void Control::InterruptOn()
{
	if(!interrupt_active)
		InterruptHandle=OSAttachTimer(2,InterruptBalance);
	if(InterruptHandle) interrupt_active=true;
}


//method to activate interrupt with desired values
void Control::InterruptOn(LinearAlgebra hleft,LinearAlgebra hright,float percent_on_right_foot)
{
	SetDesiredValues(hleft,hright,percent_on_right_foot);
	InterruptOn();
}


//method to deactivate interrupt
void Control::InterruptOff()
{
	if(interrupt_active)
		OSDetachTimer(InterruptHandle);
	interrupt_active=false;
}


//method to pass (new) desired values to the instance
void Control::SetDesiredValues(LinearAlgebra hleft,LinearAlgebra hright,float percent_on_right_foot)
{
	bool wason=false;
	
	//deactivate interrupt to access the desired values securely
	if(interrupt_active) 
	{
		wason=true;
		InterruptOff();
	}
	
	this->hold_left =hleft;
	this->hold_right=hright;
	this->load=percent_on_right_foot;
	
	if(wason) InterruptOn();	
	
}


//calculate measured centre of gravity locally for controlling
LinearAlgebra Control::CalcMeasCOG(LinearAlgebra lfoot,LinearAlgebra rfoot)
{
	int liftinfo;
	double temp[4]={28,0,0,1};
	lcog.Equals(temp,4,1);
	rcog=lcog;
	Feet* feet=Feet::GetTheInstance();
		
	//read liftinformation with refresh
	liftinfo=feet->Lifted(true);
	
	//both feet are not attached to the ground
	if(liftinfo!=3)
	{
		//if not lifted, read cog
		if(liftinfo!=1) feet->GetCOG(lcog,1,0);	//retrieving left foot's values after refresh
		if(liftinfo!=2) feet->GetCOG(rcog,2,0);	//retrieving values of right foot, no refresh
		feet->LoadDistribution(&temp[0],&temp[1],&temp[2],false);
	}
	else temp[1]=0.5;
	
	//calculate total measured cog
	lcog+=lfoot;	//probalby should be moved into according if-branch
	rcog+=rfoot;	//probalby should be moved into according if-branch
	measuredcog =lcog + ( (rcog-lcog)*temp[1] );
	
	return measuredcog;
}


//virtual cog is calculated out of distribution and location of masses
//not necessary during control loop, only for additional information
//needed for slope-calculation
LinearAlgebra Control::CalcVirtCOG(bool project_to_footlevel)
{
	//vector{x,y,z,1} are the positions of the cog
	
	DenavitHartenberg link;
	LinearAlgebra vect;
	
	double 	foot=  114.75,		//a foot's  mass in g
			shank=  69.25,		//a shank's mass in g
			thigh= 153.25,		//a thigh's mass in g
			body=  765.50,		//  body's  mass in g
			total=1440.00,		//=>2*foot+2*shank+2*thigh+body
			vector[4]={0,0,0,0};//buffer for internal use
	//weights have been estimated as well as their position relative to link
	/**************************FEET**************************/
	vector[0]=12;vector[1]= 10;vector[2]= 18;vector[3]=1;	//setup vector in link coordinate system
	vect.Equals(vector,4,1);		//transpose vector to LinearAlgebra member
	link.Calc(LFOOT);			//calculate matrix(link->base)
	link*=vect;				//calculate this vector relative to base coordinate system
	link*=(foot /total);		//stretch vector by the link's wheight
	virtualcog=link;			//add vector to total virtual cog
	/**											**/
	vector[0]=12;vector[1]=-10;vector[2]= 18;vector[3]=1;
	vect.Equals(vector,4,1);
	link.Calc(RFOOT);
	link*=vect;
	link*=(foot /total);
	virtualcog+=link;
	/*************************SHANKS*************************/
	vector[0]=-15;vector[1]=0;vector[2]=-17;vector[3]=1;
	vect.Equals(vector,4,1);
	link.Calc(LSHANK);
	link*=vect;
	link*=(shank /total);
	virtualcog+=link;
	/**											**/
	vector[0]=-15;vector[1]=0;vector[2]= 17;vector[3]=1;
	vect.Equals(vector,4,1);
	link.Calc(RSHANK);
	link*=vect;
	link*=(shank /total);
	virtualcog+=link;
	/*************************THIGHS*************************/
	vector[0]=-30;vector[1]=0;vector[2]=-17;vector[3]=1;
	vect.Equals(vector,4,1);
	link.Calc(LTHIGH);
	link*=vect;
	link*=(thigh /total);
	virtualcog+=link;
	/**											**/
	vector[0]=-30;vector[1]=0;vector[2]= 17;vector[3]=1;
	vect.Equals(vector,4,1);
	link.Calc(RTHIGH);
	link*=vect;
	link*=(thigh /total);
	virtualcog+=link;
	/**************************BODY**************************/
	vector[0]=-60;vector[1]=0;vector[2]=-31;vector[3]=1;
	vect.Equals(vector,4,1);
	//link.Calc(BODY);	//identity matrix
	link=vect;		//then I*vect=vect
	link*=(body /total);
	virtualcog+=link;
	/**											**/
	
	if(project_to_footlevel)
	{
		LinearAlgebra temp;
		link.Calc(-LFOOT);
		temp=link*virtualcog;
		link.Calc(-RFOOT);
		temp+=link*virtualcog;
		virtualcog=temp*-0.5;
	}
	return virtualcog;
}


//member retrievig function
LinearAlgebra Control::GetMeasCOG()
{
	return measuredcog;
}


//calculate slope angles of subsurface
void	Control::CalcSlope(int* roll,int* pitch)
{
	LinearAlgebra tmp;
	Display* lcd=Display::GetTheInstance();
	double vect[4],height;
	CalcVirtCOG();
	virtualcog.GetData(vect,4,1);
	height=vect[0]+28;
	tmp=virtualcog-measuredcog;
	//lcd->Print(virtualcog);
	//lcd->Print(measuredcog);
	tmp.GetData(vect,4,1);
	rollslope =OwnArcSin(vect[1]/(-height));	//pos rollsplope=sloped from left foot to right foot
	pitchslope=OwnArcSin(vect[2]/(-height));	//pos pitchsplope=uphill
	*roll=rollslope;
	*pitch=pitchslope;
}


//retrieve member
void	Control::GetSlope(int* roll,int* pitch)
{
	*roll=rollslope;
	*pitch=pitchslope;
}


//local arc sine using 1°-resoluted look-up table
int Control::OwnArcSin(double val)
{
	int i=0,sign=1;
	if(val<0)	sign=-1;
	val*=sign;
	if(val> 1) return sign*90;
	while(ownsin[i]<val) i++;
	return sign*i;
}


//local sine using 1°-resoluted look-up table
float Control::OwnSin(int angle)
{
	if(angle<-89 || angle>89) return -100;
	if(angle<0) return ownsin[-angle];
	return ownsin[angle];
}


//member retrieving	
LinearAlgebra Control::GetVirtCOG()
{
	return virtualcog;
}


//member retrieving
LinearAlgebra Control::GetLeftCOG ()
{
	return lcog;
}


//member retrieving
LinearAlgebra Control::GetRightCOG()
{
	return rcog;
}


//central controller, running in interrupt if kicked so
void Control::Balance()
{
	/*This function represents the whole control loop running only one 
	  cycle:
	  Input represents the desired position of the feet.
	  Output says wether servos have been driven or not.
	  It reads in 'CalcMeasCOG()' the values of the sensors, preprocesses
	  them to one central central cog, applies a 'cut-off'-low-pass 
	  filter to the values, calculates the forward control differences
	  with a digital PID- Algorithm, deals with the different generations
	  of control errors, controls servo-movements, and stores integral 
	  and differential parts of these.
	  To close the control loop and run it repeatedly, this fuction has 
	  to be implemented in a loop itself, respectively an interrupt.
	*/
	LinearAlgebra hold_centre,delta;
	Servo* servo=Servo::GetTheInstance();
	bool   drive=false;
	int    i;
	double temp[4]= {0,0,0,1},
		  dbw, lowpass;
	
	//refresh measured COG
	CalcMeasCOG(hold_left,hold_right);	
	//calculte desired value
	hold_centre=hold_left+((hold_right-hold_left)*load);
	//calculate control error
	delta=hold_centre-measuredcog;
	//extract relevant data
	delta.GetData(temp,4,1);
	deltay=temp[1];
	deltaz=temp[2];

//	//filtering out too small changes
//	//this function is just for steady standing issues to 
//	//minimize servo movements and thereby save energy	
//	//as long as the filter is not in use, comment the code
//	//to save computation time
	dbw=0;	//filter width
	if(deltay<dbw && deltay>-dbw) deltay=0;
	if(deltaz<dbw && deltaz>-dbw) deltaz=0;
	
	//PID- Control Parameters
	//proportional amplification of control error
	proll =0.01;
	ppitch=proll;
	//iroll=T/Tn integral parameter should >1
	iroll = 2.0;
	//ipitch=T/Tn
	ipitch=iroll;	
	//droll=Tv/T differential parameter (differential amplification not used by now)
	droll =0.0;
	//dpitch=Tv/T
	dpitch=droll;	
	
	//calculate set value with PID control algorithm acc. to Lutz&Wendt: Taschenbuch der Regelungstechnik, 4.,korrigierte Auflage 2002, Page 447
	roll = -1*proll *((1+iroll+droll  )*deltay-((1+2*droll )*lastdeltay)+droll *onebeforelastdeltay);
	
	pitch=    ppitch*((1+ipitch+dpitch)*deltaz-((1+2*dpitch)*lastdeltaz)+dpitch*onebeforelastdeltaz);
	
	//software lowpass (has to be replaced by HW LP on strain gauges)
	lowpass=10;
	if((roll>lowpass)||(roll<-lowpass)) roll=lowpass;
	if((pitch>lowpass)||(pitch<-lowpass)) pitch=lowpass;
	
	//save recursive values
	onebeforelastdeltay=lastdeltay;
	lastdeltay=deltay;
	onebeforelastdeltaz=lastdeltaz;
	lastdeltaz=deltaz;

	//for(i=0;i<NUMBEROFSERVOS;i++) deltaservo[i]=0;
	//virtualdelta[i]=0;	
	/*uncommenting the last line will reestablish the problem, 
	that assigning servovalues between -1 and 1 to the integer-
	array will result in the fact, that integral part in the 
	mentioned boundaries will not work, and so not work at all*/
			
	//virtualdelta keeps the differnces even if it is kess than 1 (for integral part)
	//it is regarded as a virtual output
	virtualdelta[LHIPS]=
	virtualdelta[RHIPS]=
	virtualdelta[LANKS]=
	virtualdelta[RANKS]+=roll;
	
	virtualdelta[LHIPB]=
	virtualdelta[RHIPB]+=pitch;
	
	virtualdelta[LKNEE]=         
	virtualdelta[RKNEE]+=pitch/2;	//coupling of knee and hip is 1:2
	
	for(i=0;i<NUMBEROFSERVOS;i++) deltaservo[i]=0; //empty buffer necessary to kill old values

	//real output is written (and typecasted, which produces an error)
	for(i=0;i<NUMBEROFSERVOS;i++) deltaservo[i]=(int)virtualdelta[i];

	//check if there is a change	
	for(i=0;i<NUMBEROFSERVOS;i++) 
		if(deltaservo[i]) 
		{
			drive=true;
			virtualdelta[i]=0;
			//virtualdelta[i]-=deltaservo[i];	//subtract passed driven angle
		}
		
	//drive real servos if there is sth to drive	
	if(drive) servo->Move(deltaservo,speed);
}


//Lookup Table initialisation
//1°-resoluted local sine table (consumes 360Byte memory)
float Control::ownsin[90]={
			0.000,0.017,0.035,0.052,0.070,0.087,0.105,0.122,0.139,0.156,
			0.174,0.191,0.208,0.225,0.242,0.259,0.276,0.292,0.309,0.326,
			0.342,0.358,0.375,0.391,0.407,0.423,0.438,0.454,0.469,0.485,
			0.500,0.515,0.530,0.545,0.559,0.574,0.588,0.602,0.616,0.629,
			0.643,0.656,0.669,0.682,0.695,0.707,0.719,0.731,0.743,0.755,
			0.766,0.777,0.788,0.799,0.809,0.819,0.829,0.839,0.848,0.857,
			0.866,0.875,0.883,0.891,0.899,0.906,0.914,0.921,0.927,0.934,
			0.940,0.946,0.951,0.956,0.961,0.966,0.970,0.974,0.978,0.982,
			0.985,0.988,0.990,0.993,0.995,0.996,0.998,0.999,0.999,1.000};