/* ----------------------------------------------------------------- */
/* 'ZDisplayClass.cc'									    */
/* Class for Output		 							         */
/* 													    */
/* 	last modified 07/04/2003								    */
/* 	provides methods for output on display 					    */
/* 													    */
/* 													    */
/* 													    */
/* 													    */
/* 	(C) Jochen Zimmermann								    */
/* ----------------------------------------------------------------- */

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


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

//constructor
//initialises members
Display::Display()
{
	int i;
	for(i=0;i<NUMBEROFPOSITIONS;i++)
		positionold[i]=-129;
	resize=0.5;
}


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


//initialisations
bool Display::Init()
{
	int i;
	for(i=0;i<NUMBEROFPOSITIONS;i++)
		positionold[i]=-129;
	resize=0.5;
	LCDMode(SCROLLING|NOCURSOR);
	LCDClear();
	return true;
}


//override LCDClear
void Display::Clear()
{
	int i;
	for(i=0;i<NUMBEROFPOSITIONS;i++)
		positionold[i]=-129;
	resize=0.5;
	LCDClear();
}


//override LCDPrintf
int Display::Print(const char text[], ...)
{
	int return_value=0;
	int length=0,decimals=0,intvalue;
	double doublevalue;
	int pos=0,k=0;
	char *buffer=0;
	
	va_list listpointer;	//pointer to list of variable number of arguments passed by ellipsis ...
	va_start(listpointer, text); //define start of list
	
	//parse text
	if(text[0]=='\0') return -1;	//error
	while(text[pos]!='\0')	//END OF STRING
	{
		length=0;
		while( (text[pos+length]!='\0') && (text[pos+length]!='%') ) length++;
		//print segmented strings
		if(length)
		{
			buffer=new char[length+1];
			for(k=0;k<length;k++) buffer[k]=text[pos+k];
			buffer[length]='\0';
			LCDPutString(buffer);
			pos+=length;
			length=0;
			delete[] buffer;
		}
		//print number
		if(text[pos]=='%')
		{
			pos++;
			while(text[pos]>='0'&&text[pos]<='9')
			{
				length*=10;
				length+=text[pos]-'0';
				pos++;
			}
			if(text[pos]=='.')
			{
				pos++;
				decimals=0;
				while(text[pos]>='0'&&text[pos]<='9')
				{
					decimals*=10;
					decimals+=text[pos]-'0';
					pos++;
				}
			}
			else decimals=3;
			
			switch(text[pos])
			{
			case '%':	LCDPutChar('%');break;
			case 'i': //int
			case 'd': //int
					intvalue= va_arg(listpointer, int);
					LCDPutIntS(intvalue,length);
					break;
			case 'f': //float
					doublevalue= va_arg(listpointer, double);
					LCDPutFloatS(doublevalue,length,decimals);
					break;
			case 'x': //hex
					intvalue= va_arg(listpointer, int);
					LCDPutHex(intvalue);
					break;
			default :	return_value=-1;	
			
			}
			pos++;
		}
		
	}
	
	va_end(listpointer);
	return return_value;
}


//overridden combination of LCDSetPos and LCDPrintf
int Display::Print(int x,int y,const char text[], ...)
{
	LCDSetPos(x,y);
	
	int return_value=0;
	int length=0,decimals=0,intvalue,pos=0,k=0;
	double doublevalue;

	char *buffer=0;
	
	va_list listpointer;	//pointer to list of variable number of arguments passed by ellipsis ...
	va_start(listpointer, text); //define start of list
	
	//parse text
	if(text[0]=='\0') return -1;	//error
	while(text[pos]!='\0')	//END OF STRING
	{
		length=0;
		while( (text[pos+length]!='\0') && (text[pos+length]!='%') ) length++;
		//print segmented strings
		if(length)
		{
			buffer=new char[length+1];
			for(k=0;k<length;k++) buffer[k]=text[pos+k];
			buffer[length]='\0';
			LCDPutString(buffer);
			pos+=length;
			length=0;
			delete[] buffer;
		}
		//print number
		if(text[pos]=='%')
		{
			pos++;
			while(text[pos]>='0'&&text[pos]<='9')
			{
				length*=10;
				length+=text[pos]-'0';
				pos++;
			}
			if(text[pos]=='.')
			{
				pos++;
				decimals=0;
				while(text[pos]>='0'&&text[pos]<='9')
				{
					decimals*=10;
					decimals+=text[pos]-'0';
					pos++;
				}
			}
			else decimals=3;
			
			switch(text[pos])
			{
			case '%':	LCDPutChar('%');break;
			case 'i': //int
			case 'd': //int
					intvalue= va_arg(listpointer, int);
					LCDPutIntS(intvalue,length);
					break;
			case 'f': //float
					doublevalue= va_arg(listpointer, double);
					LCDPutFloatS(doublevalue,length,decimals);
					break;
			case 'x': //hex
					intvalue= va_arg(listpointer, int);
					LCDPutHex(intvalue);
					break;
			default :	return_value=-1;	
			
			}
			pos++;
		}
		
	}
	
	va_end(listpointer);
	return return_value;
}


//LCDPrintf for special data structure of LinearAlgebra-Instance
int Display::Print(LinearAlgebra toprint)//special function to display a Matrix
{
	int button=0,i,j;
	int rows,columns;
	double matrix[4][4];
	UserInput *key=UserInput::GetTheInstance();
	toprint.GetData((double*)matrix);
	columns=toprint.GetColumns();
	rows=toprint.GetRows();
	if(columns==4)
	{
		//displaying
		Clear();
		Print("Rotation:");
		for(i=0;i<4;i++)
		{
			Print("\n");
			if(i==3) Print("\n");
			for(j=0;j<3;j++)
				Print("%4.1f ",matrix[i][j]);
		} 
		while(!(button=key->Read()));
		//OSWait(10);
		if(button==RC_STANDBY) return button;
		
		Clear();
		Print("Vector:");
		for(i=0;i<4;i++)
		{
			if(i==3) Print("\n");
			Print("\n  %4.3f ",matrix[i][3]);
		}
		while(!(button=key->Read()));
		//OSWait(10);
	}
	else
	{
		//displaying
		//lcd->Clear();
		//lcd->Print("Matrix:");
		for(i=0;i<rows;i++)
		{
			Print("\n");
			for(j=0;j<columns;j++)
				Print(" %4.3f ",matrix[i][j]);
		} 
		while(!(button=key->Read()));
		//OSWait(10);		
	}
	return button;
}


//override LCDMenu
int Display::Menu(const char str1[],const char str2[],const char str3[],const char str4[])
{
	return LCDMenu(str1,str2,str3,str4);
}


//special display of foot-boundaries in optimal zoom 
double Display::PaintExtensions(int leftx,int lefty,int rightx,int righty)
{
	//This function paints the triangles, spanned up by the toes
	float x1=0,y1=0,x2=0,y2=0;
	int vx1,vx2,vy1,vy2,invert=1;
	double size;
	
	//in centre there is the highest zoom at 60Pixel/120mm
	double temp=120;
	if(rightx>=0)	{if(temp<( rightx +60)) temp= rightx +60;}
	else			{if(temp<(-rightx +60)) temp=-rightx +60;}
	if(leftx >=0)	{if(temp<( leftx  +60)) temp= leftx  +60;}
	else			{if(temp<(-leftx  +60)) temp=-leftx  +60;}
	if(lefty >=0)	{if(temp<2*( lefty  +60)) temp=2*( lefty  +60);}
	else			{if(temp<2*(-lefty  +60)) temp=2*(-lefty  +60);}
	if(righty>=0)	{if(temp<2*( righty +60)) temp=2*( righty +60);}
	else			{if(temp<2*(-righty +60)) temp=2*(-righty +60);}
	resize=size=60/temp;	//calculate the resizer by the biggest distance to be displayed
	
	//	if(clearold)	invert =2;
	//	else			invert =1;	
	
	//Feet centres
	x1=-6;	y1=0.0;
	vx1=(int)( size*(rightx+x1));	vy1=(int)(size*righty);
	vx2=(int)( size*(leftx -x1));	vy2=(int)(size*lefty );
	LCDSetPixel(CENTREY+vy1,CENTREX+vx1,invert);
	LCDSetPixel(CENTREY+vy2,CENTREX+vx2,invert);
	
	//BorderLines
	//	 xr		 yr				 xl		 yl
	//T4	 2,05  cm	 5,64  cm		T1	 -2,05 cm	  5,64 cm
	//T5	-6,0   cm	 0,0   cm		T2	 -2,05 cm	 -5,64 cm
	//T6	 2,05  cm	-5,64  cm		T3	  6,0  cm	  0,0  cm
	//
	//right
	x1=21;	y1=56;	x2=-60;	y2=0;
	vx1=(int)(size*(rightx+x1));	vy1=(int)(size*(righty+y1));
	vx2=(int)(size*(rightx+x2));	vy2=(int)(size*righty);
	LCDLine(CENTREX+vx1,CENTREY+vy1,CENTREX+vx2,CENTREY+vy2,invert);
	//left
	vx1=(int)(size*(leftx-x1));	vy1=(int)(size*(lefty-y1));
	vx2=(int)(size*(leftx-x2));	vy2=(int)(size*lefty);
	LCDLine(CENTREX+vx1,CENTREY+vy1,CENTREX+vx2,CENTREY+vy2,invert);
	//right
	x1=-60;	y1=0;	x2=21;	y2=-56;
	vx1=(int)(size*(rightx+x1));	vy1=(int)(size*righty);
	vx2=(int)(size*(rightx+x2));	vy2=(int)(size*(righty+y2));
	LCDLine(CENTREX+vx1,CENTREY+vy1,CENTREX+vx2,CENTREY+vy2,invert);
	//left
	vx1=(int)(size*(leftx-x1));	vy1=(int)(size*lefty);
	vx2=(int)(size*(leftx-x2));	vy2=(int)(size*(lefty-y2));
	LCDLine(CENTREX+vx1,CENTREY+vy1,CENTREX+vx2,CENTREY+vy2,invert);
	
	//right
	x1=x2=21;	y1=-56;
	vx1=vx2=(int)(size*(rightx+x1));	
	vy1    =(int)(size*(righty+y1));	vy2=(int)(size*(righty-y1));
	LCDLine(CENTREX+vx1,CENTREY+vy1,CENTREX+vx2,CENTREY+vy2,invert);
	//left
	vx1=vx2=(int)(size*(leftx-x1));	
	vy1    =(int)(size*(lefty+-y1));	vy2=(int)(size*(lefty+y1));
	LCDLine(CENTREX+vx1,CENTREY+vy1,CENTREX+vx2,CENTREY+vy2,invert);
	
	return size;
}
/**/


//special display of foot-boundaries in optimal zoom acceptimng different structure as passed positions
double Display::PaintExtensions(class LinearAlgebra left,class LinearAlgebra right)
{
	double leftx,lefty,rightx,righty;
	double temp[4],size;
	
	if( (left.GetColumns()!=1) || (right.GetColumns()!=1) ) return 0;
	
	left.GetData(temp,4,1);
	leftx=(int)temp[1];
	lefty=(int)temp[2];
	
	right.GetData(temp,4,1);
	rightx=(int)temp[1];
	righty=(int)temp[2];
	
	resize=PaintExtensions(leftx,lefty,rightx,righty);
	
	return size;
}


//display a cross (+ or x) on desired position with zoom-in/out, storing position and style
void Display::PaintCross(float *positionnew,int identity,double resizefactor)
{
	resize=resizefactor;	
	PaintCross(positionnew,identity);
}


//display a cross (+ or x) on desired position, storing position and style
void Display::PaintCross(float *positionnew,int identity)
{
	//Painting 
	int x =0,y =0,d;
	d=identity%2;
	
	if(identity>=NUMBEROFPOSITIONS || identity<0)
	{
		LCDPrintf("wrong identity");
		identity%=NUMBEROFPOSITIONS;
		if(identity<0) identity*=-1;
	}
	
	//clearold	
	x=(int)(positionold[identity*2  ]*resize);
	y=(int)(positionold[identity*2+1]*resize);	
	//clear old cross by inverting it
	LCDSetPixel(CENTREY+y+1,CENTREX+x+d,2);
	LCDSetPixel(CENTREY+y-1,CENTREX+x-d,2);
	LCDSetPixel(CENTREY+y  ,CENTREX+x  ,2);
	LCDSetPixel(CENTREY+y-d,CENTREX+x+1,2);
	LCDSetPixel(CENTREY+y+d,CENTREX+x-1,2);
	
	//replace old cog by new one
	positionold[identity*2  ]=positionnew[0];
	positionold[identity*2+1]=positionnew[1];
	
	x=(int) (positionnew[0]*resize);
	y=(int) (positionnew[1]*resize);
	//paint new cross by inverting it
	LCDSetPixel(CENTREY+y+1,CENTREX+x+d,2);
	LCDSetPixel(CENTREY+y-1,CENTREX+x-d,2);
	LCDSetPixel(CENTREY+y  ,CENTREX+x  ,2);
	LCDSetPixel(CENTREY+y-d,CENTREX+x+1,2);
	LCDSetPixel(CENTREY+y+d,CENTREX+x-1,2);
}


//display a cross (+ or x) on desired position, storing position and style, different structure for position
void Display::PaintCross(LinearAlgebra vector,int identity)
{
	float temp[2];
	double temp2[4];

	if(vector.GetColumns()!=1) return;
	
	vector.GetData(temp2,4,1);
	temp[0]=temp2[1];
	temp[1]=temp2[2];

	PaintCross(temp,identity);
}


//display a cross (+ or x) on desired position with zoom-in/out, storing position 
//and style, special structure for position
void Display::PaintCross(LinearAlgebra vector,int identity,double resizefactor)
{
	resize=resizefactor;
	PaintCross(vector,identity);
}
	

//function to paint a bar on top of display to express load distribution
void Display::PaintLoadBalance(double left,double right,int total)
{
	double size=64;
	int x1=0,x2=127;
	//Calc
	size=(64*(double)total/300.0    );
	x1= 63- (int)(size*(right) );
	x2= 63+ (int)(size*(left   ) );
	//Left foot
	LCDArea(0,0,x1,1,0);
	LCDArea(x1+1,0,x2-1,1,1);
	LCDArea(x2,0,127,1,0);
}