/*		GEN_TRAY.C

*/
#include <rtl_debug.h>
#include <pthread.h>
//#include <tgmath.h>
#include <rtl_fifo.h>
#include <rtl_sched.h>
#include <rtl_printf.h>
#include <rtl_mutex.h>
#include "glob_var.h"
#include "gen_tray.h"
#include <math.h>
//#include "vsprintf.h"


extern struct joint_val gt_joint_val;
extern pthread_mutex_t gt_joint_val_vmtx;
#define PSH(X) (*(st++)=(X))
#define ZEROPAD	1		/* pad with zero */
#define SIGN	2		/* unsigned/signed long */
#define PLUS	4		/* show plus */
#define SPACE	8		/* space if plus */
#define LEFT	16		/* left justified */
#define SPECIAL	32		/* 0x */
#define LARGE	64		/* use 'ABCDEF' instead of 'abcdef' */


char * ftoa(char *st, double f, int flags)
{
  int i;
  int z;
  int exp = 0;
  
  if (f < 0) {
    PSH('-');
    f = -f;
  } else {
    if (flags & PLUS) PSH('+');
    if (flags & SPACE) PSH(' ');
  }

  if (f) {
    while (f < 1) {
      f *=10;
      exp--;
    }

    while (f >= 10) {
      f /=10;
      exp++;
    }
  }

  PSH('0'+ (char) f);
  z = (int) f;
  f -= z;
  f *= 10;
  PSH('.');

  for (i=0;i<4;i++) {
    PSH('0'+(char) f);
    z = (int) f;
    f -= z;
    f *= 10;
  }

  PSH('e');
  if (exp < 0) {
    PSH('-');
    exp = -exp;
  } else {
    PSH('+');
  }

  PSH('0'+exp/10);
  exp -= (exp/10) * 10;
  PSH('0'+exp);

  return st;
}
#


void gentray(struct w_params *w_p,struct robot_state *rst)
{
float aux_x[5],aux_y[5],aux_a[5];
float XtR_value,YtR_value,ZtR_value,XtL_value,YtL_value,ZtL_value,Xc_value,Yc_value,Zc_value,QtR_value,QtL_value,QR[6],QL[6];
int i,valor;
char salida[11];	


	//interpolación datos tobillos 
	if(rst->STATE_RL != GROUND)//tobillo derecho

	{
		for(i=0;i<5;i++)
		{
			aux_y[i]=w_p->XtR[i][0];//value
			aux_x[i]=w_p->XtR[i][1];//time
			aux_a[i]=w_p->XtR[i][2];//aceleration
		}

		splint(aux_x,aux_y,aux_a,5,rst->ROBOT_time,&XtR_value);//interpolacion en el eje X derecho

	        for(i=0;i<5;i++)
	        {
	                aux_y[i]=w_p->YtR[i][0];//value
	                aux_x[i]=w_p->YtR[i][1];//time
	                aux_a[i]=w_p->YtR[i][2];//aceleration
        	}

  	     	 splint(aux_x,aux_y,aux_a,5,rst->ROBOT_time,&YtR_value);//interpolacion en el eje X izq

                for(i=0;i<5;i++)
                {
                        aux_y[i]=w_p->ZtR[i][0];//value
                        aux_x[i]=w_p->ZtR[i][1];//time
                        aux_a[i]=w_p->ZtR[i][2];//aceleration
                }

                splint(aux_x,aux_y,aux_a,5,rst->ROBOT_time,&ZtR_value);//interpolacion en el eje Z derecho

	}
	else
	{
		XtR_value=rst->aVR[0];
		YtR_value=rst->aVR[1];
		ZtR_value=rst->aVR[2];
	}
	//Global variable actualization
	rst->aVR[0]=XtR_value;
	rst->aVR[1]=YtR_value;
	rst->aVR[2]=ZtR_value;


       if(rst->STATE_LL != GROUND)//tobillo izq
        {
                for(i=0;i<5;i++)
                {
                        aux_y[i]=w_p->XtL[i][0];//value
                        aux_x[i]=w_p->XtL[i][1];//time
                        aux_a[i]=w_p->XtL[i][2];//aceleration
                }

                splint(aux_x,aux_y,aux_a,5,rst->ROBOT_time,&XtL_value);//interpolacion en el eje X derecho

                for(i=0;i<5;i++)
                {
                        aux_y[i]=w_p->YtL[i][0];//value
                        aux_x[i]=w_p->YtL[i][1];//time
                        aux_a[i]=w_p->YtL[i][2];//aceleration
                }

                 splint(aux_x,aux_y,aux_a,5,rst->ROBOT_time,&YtL_value);//interpolacion en el eje Y izq

                for(i=0;i<5;i++)
                {
                        aux_y[i]=w_p->ZtL[i][0];//value
                        aux_x[i]=w_p->ZtL[i][1];//time
                        aux_a[i]=w_p->ZtL[i][2];//aceleration
                }

                 splint(aux_x,aux_y,aux_a,5,rst->ROBOT_time,&ZtL_value);//interpolacion en el eje Z izq

        }
        else
        {
                XtL_value=rst->aVL[0];
                YtL_value=rst->aVL[1];
		ZtL_value=rst->aVL[2];
        }

        //Global variable actualization
        rst->aVL[0]=XtL_value;
        rst->aVL[1]=YtL_value;
	rst->aVL[2]=ZtL_value;



	//interpolación datos cadera

        for(i=0;i<3;i++)
        {
                aux_y[i]=w_p->Xc[i][0];//value
                aux_x[i]=w_p->Xc[i][1];//time
                aux_a[i]=w_p->Xc[i][2];//aceleration
        }

	splint(aux_x,aux_y,aux_a,3,rst->ROBOT_time,&Xc_value);

	if ( ( rst->ROBOT_time <= (w_p->XtR[0][1]+0.6*w_p->time)) && (rst->R_STATE == START) )
	{
		Xc_value=w_p->XtR[0][0];
	}

        for(i=0;i<3;i++)
        {
                aux_y[i]=w_p->Yc[i][0];//value
                aux_x[i]=w_p->Yc[i][1];//time
                aux_a[i]=w_p->Yc[i][2];//aceleration
        }


	splint(aux_x,aux_y,aux_a,3,rst->ROBOT_time,&Yc_value);

        if ( ( rst->ROBOT_time <= (w_p->XtR[0][1]+0.6*w_p->time)) && (rst->R_STATE == START) )
	{
                Yc_value=w_p->h;
	}

        for(i=0;i<3;i++)
        {
                aux_y[i]=w_p->Zc[i][0];//value
                aux_x[i]=w_p->Zc[i][1];//time
                aux_a[i]=w_p->Zc[i][2];//aceleration
        }


        splint(aux_x,aux_y,aux_a,3,rst->ROBOT_time,&Zc_value);

	if(rst->R_STATE == STAND)
	{
		Xc_value=rst->aVC[0];
		Yc_value=rst->aVC[1];
		Zc_value=rst->aVC[2];
	}
        //Global variable actualization
        rst->aVC[0]=Xc_value;
        rst->aVC[1]=Yc_value;
	rst->aVC[2]=Zc_value;



        //interpolación datos angulo pie
        if(rst->STATE_RL != GROUND)//pie derecho

        {
                for(i=0;i<4;i++)
                {
                        aux_y[i]=w_p->QtR[i][0];//value
                        aux_x[i]=w_p->QtR[i][1];//time
                        aux_a[i]=w_p->QtR[i][2];//aceleration
                }

                splint(aux_x,aux_y,aux_a,4,rst->ROBOT_time,&QtR_value);//interpolacion en el eje X derecho
	}

        else
                QtR_value=rst->aVR[4];


        //Global variable actualization
        rst->aVR[4]=QtR_value;

        if(rst->STATE_LL != GROUND)//pie izq

        {
                for(i=0;i<4;i++)
                {
                        aux_y[i]=w_p->QtL[i][0];//value
                        aux_x[i]=w_p->QtL[i][1];//time
                        aux_a[i]=w_p->QtL[i][2];//aceleration
                }

                splint(aux_x,aux_y,aux_a,4,rst->ROBOT_time,&QtL_value);//interpolacion en el eje X derecho
        }

        else
                QtL_value=rst->aVL[4];


        //Global variable actualization
        rst->aVL[4]=QtL_value;


	//inverse robot kinematic calcul 
	m_inverse('R',Xc_value,XtR_value,Yc_value,YtR_value,QtR_value,QR);
	m_inverse('L',Xc_value,XtL_value,Yc_value,YtL_value,QtL_value,QL);

	QR[0]=asin((rst->aVC[2])/(Yc_value-YtR_value));
	QR[4]=-QR[0];

        QL[0]=asin((rst->aVC[2])/(Yc_value-YtL_value));
        QL[4]=-QL[0];

        QR[0]=(180.0/pi)*QR[0];
        QR[4]=(180.0/pi)*QR[4];
        QL[0]=(180.0/pi)*QL[0];
        QL[4]=(180.0/pi)*QL[4];

	QR[5]=0;
	QL[5]=0;


//      ftoa(salida,QR[0],0);
//      salida[10]=' ';
//        rtl_printf("aVC value: %s \n",salida);


	//write in RRI box the joints positions value
	pthread_mutex_lock(&gt_joint_val_vmtx);
	for(i=0;i<6;i++)
	{
		gt_joint_val.RLj[i]=QR[i];
		gt_joint_val.LLj[i]=QL[i];
//faltan las Tj
	}
//solo prueba
/*	gt_joint_val.RLj[0]=10*XtR_value;
	gt_joint_val.RLj[1]=10*YtR_value;
	gt_joint_val.RLj[2]=10*XtL_value;
	gt_joint_val.RLj[3]=10*YtL_value;
	gt_joint_val.RLj[4]=10*Xc_value;
	gt_joint_val.RLj[5]=10*Yc_value;
*/
#if 0	
        ftoa(salida,QL[1],0);
        salida[10]=' ';
        rtl_printf("QL1_value: %s ",salida);
        ftoa(salida,QL[2],0);
        salida[10]=' ';
        rtl_printf("QL2_value: %s \n",salida);
                                                                                                             
        ftoa(salida,QL[3],0);
        salida[10]=' ';
        rtl_printf("QL3_value: %s ",salida);
        ftoa(salida,QR[1],0);
        salida[10]=' ';
        rtl_printf("QR1_value: %s \n",salida);

	
	ftoa(salida,QR[2],0);
	rtl_printf("QR2_value: %s ",salida);
	ftoa(salida,QR[3],0);
	rtl_printf("QR3_value %s \n",salida);
#endif


//fin prueba
	pthread_mutex_unlock(&gt_joint_val_vmtx);

}


void splint(float xa[],float ya[],float y2a[],int n,float x,float *y)
{
int klo,khi,k;
float h,b,a;

	klo=0;
	khi=n-1;
	while(khi-klo > 1)
	{
		k=(khi+klo) >> 1;
		if (xa[k] > x) khi=k;
		else klo=k;
	}

	h=xa[khi]-xa[klo];
	
	a=(xa[khi]-x)/h;
	b=(x-xa[klo])/h;
	*y=a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])*(h*h)/6.0;
}

void m_inverse(char leg,float X_c,float X_t,float Y_c,float Y_t,float Qf,float *Q)
{
float cq2,alfa,beta,aux,X,Y;
char salida[11];

	X=X_c-X_t;//X_value_hip - X_value_
	Y=Y_c-Y_t;//

/*        ftoa(salida,X_c,0);
        rtl_printf("diferencia X_c : %s ",salida);
        ftoa(salida,Y_c,0);
        rtl_printf("diferencia Y_c : %s \n",salida);

        ftoa(salida,X_t,0);
        rtl_printf("diferencia X_t : %s ",salida);
        ftoa(salida,Y_t,0);
        rtl_printf("diferencia Y_t : %s \n",salida);

        ftoa(salida,X,0);
        rtl_printf("diferencia X : %s ",salida);
        ftoa(salida,Y,0);
        rtl_printf("diferencia Y : %s \n",salida);
*/
	cq2=(X*X+Y*Y-L1*L1-L2*L2)/(2*L1*L2);

	Q[2]=-acos(cq2);			//knee value

	Q[3]= atan(X/(-Y)) - atan( (L2*sin(Q[2])) / (L1+L2*cq2) );//hip value

	alfa=atan( ( L2*sin(Q[2]) ) / ( L1*L2*cos(Q[2]) ) );
	beta=Q[3]+alfa;
	aux=Q[2]-alfa;
	Q[1]=Qf-(beta+aux);//hay que tomar en cuenta Qf

	if(leg=='R')
	{
		Q[1]=-(180.0/pi)*Q[1];
		Q[2]=-(180.0/pi)*Q[2];
		Q[3]=(180.0/pi)*Q[3];
	}

        if(leg=='L')
        {
                Q[1]=(180.0/pi)*Q[1];
                Q[2]=(180.0/pi)*Q[2];
                Q[3]=-(180.0/pi)*Q[3];
        }

/*
        ftoa(salida,Q[1],0);
        rtl_printf("Valor Q1 : %s ",salida);
        ftoa(salida,Q[2],0);
        rtl_printf("Q2 : %s ",salida);
        ftoa(salida,Q[3],0);
        rtl_printf("Q3 : %s \n",salida);
*/

}