/* ----------------------------------------------------------------- */
/* 'ZBehaviourClass.cc'									    */
/* Behaviour-Class			 						         */
/* 													    */
/* 	last modified 12/04/2003								    */
/* 	provides functions to run high-level behaviour			    */
/* 	this class is used to interface the user and kick lower level   */
/* 	applictaions										    */
/* 													    */
/* 													    */
/* 	(C) Jochen Zimmermann								    */
/* ----------------------------------------------------------------- */


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


//constructor
//initialises members
Behaviour::Behaviour()
{
	
}


//initalising lower-level classes
void Behaviour::Init()
{
	Servo* 	 servo=Servo::    GetTheInstance();
	Display*   lcd=  Display::  GetTheInstance();
	Feet* 	 feet= Feet::     GetTheInstance();
	UserInput* key=  UserInput::GetTheInstance();
	Control*   pid=Control::    GetTheInstance();
	
	//initalising Singleton classes (hardware)
	key-> Init();
    	lcd-> Init();
    	lcd->Clear();
  	lcd->Print(0,0," ______________ ");
	lcd->Print(1,0,"|              |");
	lcd->Print(2,0,"|Cling Andy  to|");
	lcd->Print(3,0,"|its holder and|");
	lcd->Print(4,0,"|press any key!|");
	lcd->Print(5,0,"|______________|");
	lcd->Menu(" "," "," ","OK");
  	key->Wait();
  	feet->Init();
  	pid-> Init();
  	
  	lcd->Clear();
  	lcd->Print(0,0," ______________ ");
	lcd->Print(1,0,"|              |");
	lcd->Print(2,0,"|Straighten the|");
	lcd->Print(3,0,"|robot.  Switch|");
	lcd->Print(4,0,"| on  battery! |");
	lcd->Print(5,0,"|______________|");
	lcd->Menu(" "," "," ","OK");
	key->Wait();
  	servo->Init();
  	
  	return;
}


//releasing lower-level classes
void Behaviour::Release()
{
	Servo* 	 servo=Servo::    GetTheInstance();
	Display*   lcd=  Display::  GetTheInstance();
	UserInput* key=  UserInput::GetTheInstance();
	
	lcd->Clear();
	lcd->Print(0,0," ______________ ");
	lcd->Print(1,0,"|              |");
	lcd->Print(2,0,"|Cling Andy  to|");
	lcd->Print(3,0,"|its holder and|");
	lcd->Print(4,0,"|press any key!|");
	lcd->Print(5,0,"|______________|");
	lcd->Menu(" "," "," ","OK");
	key->Wait();
  	
  	//releasing servos
  	servo->Release();
  	
  	lcd->Clear();
	lcd->Print(0,0," ______________ ");
	lcd->Print(1,0,"|              |");
	lcd->Print(2,0,"|  Switch off  |");
	lcd->Print(3,0,"| battery-pack!|");
	lcd->Print(4,0,"|     Done!    |");
	lcd->Print(5,0,"|______________|");
	lcd->Menu(" "," "," ","OK");
	key->Wait();
  	lcd->Clear();
	lcd->Print("Switch off Batt-pack!\nDone");	
	
	return;
}


//runs the class, behaviour choosing, etc.
void Behaviour::Run()
{
	Display*   lcd=  Display::  GetTheInstance();
	UserInput* key=  UserInput::GetTheInstance();
	
	int pressed=0;
	int pos=0;
	bool printnew=true;
	
	//Display Menu Screen
	do
	{
		//display
		if(printnew)
		{
			lcd->Clear();
			lcd->Print(0,0," ______________ ");
			lcd->Print(1,0,"| |(1)Stand    |");
			lcd->Print(2,0,"| |(2)Squats   |");
			lcd->Print(3,0,"| |(3)Swing    |");
			lcd->Print(4,0,"| |(4)Set      |");
			lcd->Print(5,0,"|_|____________|");
			lcd->Menu("EXIT","  ^ "," v  ","  OK");
			printnew=false;
		}
		lcd->Print(pos+1,1,"*");
		pressed=key->Wait();
		switch(pressed)
		{
		case KEY1:
		case RC_RED:
		case RC_STANDBY:	//exit program
						lcd->Clear();
						lcd->Print(1,0," ______________ ");
						lcd->Print(2,0,"|              |");
						lcd->Print(3,0,"|Do You really |");
						lcd->Print(4,0,"|want to exit? |");
						lcd->Print(5,0,"|______________|");
						lcd->Print(6,0,"                ");
						lcd->Menu("No"," "," ","Yes");
						pressed=key->Wait();
						if(pressed==KEY4 || pressed==RC_OK || pressed==RC_BLUE)
							pressed=7777;
						else pressed=0;
						printnew=true;
						break;
		case KEY2:
		case RC_GREEN:
		case RC_PLAY:		//previous choice
						lcd->Print(pos+1,1," ");
						pos=pos+3;
						pos=pos%4;
						break;	
		case KEY3:
		case RC_YELLOW:
		case RC_STOP:		//next choice
						lcd->Print(pos+1,1," ");
						pos=pos+1;
						pos=pos%4;
						break;	
		case RC_1:
		case RC_2:
		case RC_3:
		case RC_4:		//kick behaviour
						switch(pressed)
						{
						case RC_1:pos=0;break;
						case RC_2:pos=1;break;
						case RC_3:pos=2;break;
						case RC_4:pos=3;break;
						}
		case KEY4:
		case RC_OK:
		case RC_BLUE:		//kick chosen behaviour
						switch(pos)
						{
						case 0://kick behaviour 1
							  Stand();
							  break;	
						case 1://kick behaviour 2
							  Squats();
							  break;	
						case 2://kick behaviour 3
							  Swing();
							  break;	
						case 3://kick behaviour 4
							  Set();
							  break;	
						}
						printnew=true;
						break;
		}
	}while(pressed!=7777);

}


//performing squats with underlying controller
void Behaviour::Squats()
{
	int button=0,i,speed,delta;
	double temp[4]={28,0,0,1};
	bool isrunning=false,display=false;
	int servoposition[NUMBEROFSERVOS];
	bool down=true;
	
	LinearAlgebra vector(temp,4,1);
	DenavitHartenberg hold_left,hold_right;
	Feet*      feet=Feet::	 GetTheInstance();
	Display*   lcd=Display::  GetTheInstance();
	UserInput* key=UserInput::GetTheInstance();
	Control*   pid=Control::  GetTheInstance();
	Servo*     servos=Servo:: GetTheInstance();
		
	//errect robot
	for(i=0;i<NUMBEROFSERVOS;i++) servoposition[i]=128;
	servoposition[LHIPB]=servoposition[RHIPB]=148;
	servoposition[LANKB]=servoposition[RANKB]=138;
	servos->Set(servoposition,2);
	
	//calculate desired values from given position	
	hold_left.Calc (LFOOT);
	hold_left*=vector;
	hold_right.Calc(RFOOT);
	hold_right*=vector;
	
	//set desired speed
	speed=1;
	delta=1;
	
	//Displaying
	lcd->Clear();
	lcd->Print(0,0,"%d",speed);
	if(display)	lcd->PaintExtensions(hold_left,hold_right);
	if(isrunning) 	lcd->Menu("OFF ","DISP"," ","EXIT");
	else			lcd->Menu(" ON ","DISP"," ","EXIT");

	do
	{
		//Displaying
		if(display)
		{
			//paint load distribution bar
			feet->LoadDistribution(&temp[0],&temp[1],&temp[2],false);
			lcd->PaintLoadBalance(temp[0],temp[1],(int)temp[2]);
			
			//paint centres of force
			lcd->PaintCross(pid->GetLeftCOG (),0);
			lcd->PaintCross(pid->GetRightCOG(),2);
			//paint NPCM
			lcd->PaintCross(pid->GetMeasCOG (),1);
		}
		
		//Driving
		for(i=0;i<NUMBEROFSERVOS;i++) servoposition[i]=0;
		if(down)
		{
			//set desired relative position
			servoposition[LANKB]=servoposition[RANKB]=+delta;
			servoposition[LHIPB]=servoposition[RHIPB]=+delta;
			servoposition[LKNEE]=servoposition[RKNEE]=-2*delta;
			//drive
			servos->Move(servoposition,speed);	
			//toggle direction
			servos->Get(servoposition);
			if(	(servoposition[LANKB]>=((servos->GetLimit( LANKB))-1))||
				(servoposition[RANKB]>=((servos->GetLimit( RANKB))-1))||
				(servoposition[LHIPB]>=((servos->GetLimit( LHIPB))-1))||
				(servoposition[RHIPB]>=((servos->GetLimit( RHIPB))-1))||
				(servoposition[LKNEE]<=((servos->GetLimit(-LKNEE))+1))||
				(servoposition[RKNEE]<=((servos->GetLimit(-RKNEE))+1)) 	)
				down=false;	
		}
		else
		{
			//set desired relative position
			servoposition[LANKB]=servoposition[RANKB]=-delta;
			servoposition[LHIPB]=servoposition[RHIPB]=-delta;
			servoposition[LKNEE]=servoposition[RKNEE]=+2*delta;
			//drive
			servos->Move(servoposition,speed);	
			//toggle direction
			servos->Get(servoposition);
			if(	(servoposition[LANKB]<=((servos->GetLimit(-LANKB))+1))||
				(servoposition[RANKB]<=((servos->GetLimit(-RANKB))+1))||
				(servoposition[LHIPB]<=((servos->GetLimit(-LHIPB))+1))||
				(servoposition[RHIPB]<=((servos->GetLimit(-RHIPB))+1))||
				(servoposition[LKNEE]>=((servos->GetLimit( LKNEE))-1))||
				(servoposition[RKNEE]>=((servos->GetLimit( RKNEE))-1))		)
				down=true;		
		}
		
		//User- Feedback processing
		button=key->WaitTime(1);
		switch(button)
		{
			case KEY1:
			case RC_RED:	if(isrunning) 
						{
							pid->InterruptOff();
							lcd->Menu(" ON ","DISP"," ","EXIT");
							isrunning=false;
						}
					 	else 
					 	{
					 		pid->InterruptOn(hold_left,hold_right,0.5);
					 		lcd->Menu("OFF ","DISP"," ","EXIT");
					 		isrunning=true;
					 	}
					 	break;
			case RC_PLAY:	speed++;
						if(speed>10) speed=10;
						lcd->Print(0,0,"%2d",speed);
						break;
			case RC_STOP:	speed--;
						if(speed<1) speed=1;
						lcd->Print(0,0,"%2d",speed);
						break;
			case KEY2:
			case RC_GREEN:	if(display)	
						{
							display=false;
							lcd->Clear();
							if(isrunning) lcd->Menu("OFF ","DISP"," ","EXIT");
							else lcd->Menu(" ON ","DISP"," ","EXIT");
						}
						else 
						{
							display=true;		
							lcd->PaintExtensions(hold_left,hold_right);
							if(isrunning) lcd->Menu("OFF ","DISP"," ","EXIT");
							else lcd->Menu(" ON ","DISP"," ","EXIT");
						}
						break;
		}
		
	}while((button!=KEY4)&&(button!=RC_BLUE)&&(button!=RC_OK)&&(button!=RC_STANDBY));
	
	//stop controller-interrupt
	if(isrunning) pid->InterruptOff();
	
	return ;
}


//standing and compensating surface
void Behaviour::Stand()
{
	int button=0;
	double temp[4]={28,0,0,1};
	bool isrunning=false;
	LinearAlgebra vector(temp,4,1);
	DenavitHartenberg hold_left,hold_right;
	Feet*      feet=Feet::	 GetTheInstance();
	Display*   lcd=Display::  GetTheInstance();
	UserInput* key=UserInput::GetTheInstance();
	Control*   pid=Control::  GetTheInstance();
	
	
	//calculate desired position
	hold_left.Calc (LFOOT);
	hold_left*=vector;
	hold_right.Calc(RFOOT);
	hold_right*=vector;
	
	//paint feet's boundaries
	lcd->Clear();
	lcd->PaintExtensions(hold_left,hold_right);
	
	do
	{
		//paint load distribution bar
		feet->LoadDistribution(&temp[0],&temp[1],&temp[2],false);
		lcd->PaintLoadBalance(temp[0],temp[1],(int)temp[2]);
		
		//paint centres of force
		lcd->PaintCross(pid->GetLeftCOG (),0);
		lcd->PaintCross(pid->GetRightCOG(),2);
		//paint NPCM
		lcd->PaintCross(pid->GetMeasCOG (),1);
		//paint NPCM
		lcd->PaintCross(pid->CalcVirtCOG (),3);
				
		//display controller state toggle menu
		if(isrunning) lcd->Menu("OFF"," "," ","EXIT");
		else lcd->Menu(" ON "," "," ","EXIT");
		
		//read user input
		button=key->Read();
		
		if(button==KEY1 || button==RC_RED)
		{
			//toggle controller running state 
			if(isrunning) 
			{
				pid->InterruptOff();
				isrunning=false;
			}
		 	else 
		 	{
		 		pid->InterruptOn(hold_left,hold_right,0.5);
		 		isrunning=true;
		 	}
		}
		
		
	}while((button!=KEY4)&&(button!=RC_BLUE)&&(button!=RC_OK)&&(button!=RC_STANDBY));
	
	//stop controller-interrupt if running
	if(isrunning) pid->InterruptOff();
	
	return ;
};


//standing and swinging from left to right
void Behaviour::Swing()
{
	int button=0,i;
	double temp[4]={28,0,0,1};
	float percent_on_right_foot=0.5,delta=0.05,overswing=0.0;
	bool isrunning=false,swingright=true;
	int servoposition[NUMBEROFSERVOS];
	LinearAlgebra vector(temp,4,1);
	DenavitHartenberg hold_left,hold_right;
	Feet*      feet=Feet::	 GetTheInstance();
	Display*   lcd=Display::  GetTheInstance();
	UserInput* key=UserInput::GetTheInstance();
	Control*   pid=Control::  GetTheInstance();
	Servo*	 servo=Servo::  GetTheInstance();
	
	//errect robot without controller
	for(i=0;i<NUMBEROFSERVOS;i++) servoposition[i]=128;
	servoposition[LHIPB]=servoposition[RHIPB]=148;
	servoposition[LANKB]=servoposition[RANKB]=138;
	servo->Set(servoposition,1);
	
	//calculating positions to hold from actual positions
	hold_left.Calc (LFOOT);
	hold_left*=vector;
	hold_right.Calc(RFOOT);
	hold_right*=vector;
	
	//painting feet's boundaries
	lcd->Clear();
	lcd->PaintExtensions(hold_left,hold_right);
	
	do
	{
		//paint load distribition bar
		feet->LoadDistribution(&temp[0],&temp[1],&temp[2],false);
		lcd->PaintLoadBalance(temp[0],temp[1],(int)temp[2]);
		
		//paint centres of force
		lcd->PaintCross(pid->GetLeftCOG (),0);
		lcd->PaintCross(pid->GetRightCOG(),2);
		//painting NPCM
		lcd->PaintCross(pid->GetMeasCOG (),1);
		
		//swinging
		if (isrunning)
		{
			pid->SetDesiredValues(hold_left,hold_right,percent_on_right_foot);
			if(swingright)
			{
				if(percent_on_right_foot<(1.0+overswing))
					percent_on_right_foot+=delta;	//move desired value to the right
				else swingright=false;			//toggle direction
			}
			else
			{
				if(percent_on_right_foot>(0.0-overswing)) 
					percent_on_right_foot-=delta;	//move desired value to the left
				else swingright=true;			//toggle direction
			}
		}
		
		//wait and read user input
		button=key->WaitTime(5);
		
		//display menu for controller state toggle
		if(isrunning) lcd->Menu("OFF"," "," ","EXIT");
		else lcd->Menu(" ON "," "," ","EXIT");
		
		if(button==KEY1 || button==RC_RED)
		{
			//toggle controller running state 
			if(isrunning) 
			{
				pid->InterruptOff();
				isrunning=false;
			}
		 	else 
		 	{
		 		pid->InterruptOn(hold_left,hold_right,0.5);
		 		isrunning=true;
		 	}
		}
				
	}while((button!=KEY4)&&(button!=RC_BLUE)&&(button!=RC_OK)&&(button!=RC_STANDBY));
	
	//stop controller-interrupt if running
	if(isrunning) pid->InterruptOff();
	
	return ;
};


//controller offline, just angle correction
void Behaviour::Set()
{
	int servo=0,speed=9,delta=10,button=0;
	bool exit=false;
	int position[NUMBEROFSERVOS];
	Servo*     hservo=Servo::    GetTheInstance();
	Display*   lcd=   Display::  GetTheInstance();
	UserInput* key=   UserInput::GetTheInstance();
	hservo->Get(position);
	
	lcd->Clear();
	lcd->Print("SetServos");
	lcd->Print(2,1,"Servo: %3d",servo);
	lcd->Print(3,1,"Value: %3d",position[servo]);
	lcd->Print(4,1,"Speed: %3d",speed);
	lcd->Print(5,1,"Delta: %3d",delta);
	lcd->Menu("+","-","NEXT"," OK ");
	
	while(!exit)
	{
	button=0;
	IRTVFlush();
	hservo->Get(position);
	
	button=key->Wait();
	switch(button)
	{
		case KEY1:
		case RC_FF: 	position[servo]+=delta;
					hservo->Set(position,speed);
					hservo->Get(position);
					lcd->Print(3,8,"%3d",position[servo]);
					break;
		case KEY2:
		case RC_RW:	position[servo]-=delta;
					hservo->Set(position,speed);
					hservo->Get(position);
					lcd->Print(3,8,"%3d",position[servo]);
					break;
		case KEY3:
		case RC_PLAY:	servo++;
					if(servo>=NUMBEROFSERVOS) servo=0;
					lcd->Print(2,8,"%3d",servo);					
					lcd->Print(3,8,"%3d",position[servo]);
					OSWait(40);
					break;
		case RC_STOP:	servo--;
					if(servo<0) servo=NUMBEROFSERVOS-1;
					lcd->Print(2,8,"%3d",servo);
					lcd->Print(3,8,"%3d",position[servo]);
					OSWait(40);
					break;
		case RC_1:	speed=1;
					lcd->Print(4,8,"%3d",speed);
					break;
		case RC_2:	speed=2;
					lcd->Print(4,8,"%3d",speed);
					break;
		case RC_3:	speed=3;
					lcd->Print(4,8,"%3d",speed);
					break;
		case RC_4:	speed=4;
					lcd->Print(4,8,"%3d",speed);
					break;
		case RC_5:	speed=5;
					lcd->Print(4,8,"%3d",speed);
					break;
		case RC_6:	speed=6;
					lcd->Print(4,8,"%3d",speed);
					break;
		case RC_7:	speed=7;
					lcd->Print(4,8,"%3d",speed);
					break;
		case RC_8:	speed=8;
					lcd->Print(4,8,"%3d",speed);
					break;
		case RC_9:	speed=9;
					lcd->Print(4,8,"%3d",speed);
					break;
		case RC_0:	speed=10;
					lcd->Print(4,8,"%3d",speed);
					break;
		case KEY4:
		case RC_STANDBY:	
		case RC_OK:	exit=true;break;
		case RC_RED:	if(delta==1) delta =10;
					else 
					if(delta==10) delta=50;
					else delta=1;
					lcd->Print(5,8,"%3d",delta);
					OSWait(40);
					break;
		default:		break;
	}
	}
}