/*			
			walking_params.c
*/
#include <rtl_debug.h>
//#include <math.h>
#include <arch/memory.h>
//#include <rtl_printf.h>
#include "glob_var.h"
#include "walk_params.h"
#include <math.h>
int walking_params(struct w_params *w_ptr_1,struct w_params *w_ptr,struct robot_state *rst,char body)
{
float	d_vector[5],
	t_vector[5],
	a_vector[5],
	yp1,
	ypn,
	time;
int	i;
char cadena[11],cadena2[11];
float valor;

	if(w_ptr->Ds > 20 )
		return -1;

	if(w_ptr->vel > 30 )
		return -2;

	if(w_ptr->h < 10 || w_ptr->h > 25 )
		return -3;

	time = 2000.0*( w_ptr->Ds / w_ptr->vel);//en mseg
	i=(int)(time/30);
	time=i*0.03;//en seg
	w_ptr->time = time;


if(body=='R')//Right leg
{
	//eje X

        w_ptr->XtR[0][0]=rst->aVR[0];
        w_ptr->XtR[0][1]=rst->ROBOT_time;

        w_ptr->XtR[1][0]=rst->aVR[0]+(Lan*sin(w_ptr->Qs)+Laf*(1-cos(w_ptr->Qs)));
        w_ptr->XtR[1][1]=rst->ROBOT_time+(w_ptr->time)*0.2;

        w_ptr->XtR[3][0]=rst->aVR[0]+(w_ptr_1->Ds)+(w_ptr->Ds)-Lan*sin(w_ptr->Qf)-Lab*(1-cos(w_ptr->Qf));
        w_ptr->XtR[3][1]=rst->ROBOT_time+(w_ptr->time);

        w_ptr->XtR[4][0]=rst->aVR[0]+(w_ptr_1->Ds)+(w_ptr->Ds);
        w_ptr->XtR[4][1]=rst->ROBOT_time+(w_ptr->time)*1.2;

        w_ptr->XtR[2][0]=rst->aVR[0]+(w_ptr->XtR[4][0]-rst->aVR[0])/2;
        w_ptr->XtR[2][1]=rst->ROBOT_time+(w_ptr->time)*0.6;


	for(i=0;i<5;i++)
	{
	
		d_vector[i]=w_ptr->XtR[i][0];
		t_vector[i]=w_ptr->XtR[i][1];
	}

	yp1=0.0;
	ypn=0.0;
	spline(t_vector,d_vector,5,yp1,ypn,a_vector);

        for(i=0;i<5;i++)
                w_ptr->XtR[i][2]=a_vector[i];

	//eje Y
        w_ptr->YtR[0][0]=rst->aVR[1];
        w_ptr->YtR[0][1]=rst->ROBOT_time;
        w_ptr->YtR[1][0]=rst->aVR[1]-Lan+Lan*sin(w_ptr->Qs)+Lan*cos(w_ptr->Qs);
        w_ptr->YtR[1][1]=rst->ROBOT_time+(w_ptr->time)*0.2;
        w_ptr->YtR[2][0]=rst->aVR[1]+((w_ptr->h+(w_ptr_1->h-w_ptr->h)/2.0)/4.0);
        w_ptr->YtR[2][1]=rst->ROBOT_time+(w_ptr->time)*0.6;
        w_ptr->YtR[3][0]=w_ptr->YtL[4][0]-Lan+Lab*sin(w_ptr->Qf)+Lan*cos(w_ptr->Qf);
        w_ptr->YtR[3][1]=rst->ROBOT_time+(w_ptr->time);
        w_ptr->YtR[4][0]=w_ptr->YtL[4][0];
        w_ptr->YtR[4][1]=rst->ROBOT_time+(w_ptr->time)*1.2;


        for(i=0;i<5;i++)
        {

                d_vector[i]=w_ptr->YtR[i][0];
                t_vector[i]=w_ptr->YtR[i][1];
        }

        yp1=0.0;
        ypn=0.0;
        spline(t_vector,d_vector,5,yp1,ypn,a_vector);

	for(i=0;i<5;i++)
		w_ptr->YtR[i][2]=a_vector[i];


//prueba
/*        valor=w_ptr->XtR[0][0];
        ftoa(cadena,valor,0);
        valor=w_ptr->YtR[0][0];
        ftoa(cadena2,valor,0);
        rtl_printf("wp--valor de XtR 0 = %s  %s \n",cadena,cadena2);
        valor=w_ptr->XtR[1][0];
        ftoa(cadena,valor,0);
        valor=w_ptr->YtR[1][0];
        ftoa(cadena2,valor,0);
        rtl_printf("wp--valor de XtR 1 = %s  %s \n",cadena,cadena2);
        valor=w_ptr->XtR[2][0];
        ftoa(cadena,valor,0);
        valor=w_ptr->YtR[2][0];
        ftoa(cadena2,valor,0);
        rtl_printf("wp--valor de XtR 2 = %s  %s \n",cadena,cadena2);
        valor=w_ptr->XtR[3][0];
        ftoa(cadena,valor,0);
        valor=w_ptr->YtR[3][0];
        ftoa(cadena2,valor,0);
        rtl_printf("wp--valor de XtR 3 = %s  %s \n",cadena,cadena2);
        valor=w_ptr->XtR[4][0];
        ftoa(cadena,valor,0);
        valor=w_ptr->YtR[4][0];
        ftoa(cadena2,valor,0);
        rtl_printf("wp--valor de XtR 4 = %s  %s \n",cadena,cadena2);
*/

        //tobillo
        w_ptr->QtR[0][0]=0.0;//ONLY FOR SMOOTH TERRAIN in future AvR
        w_ptr->QtR[0][1]=rst->ROBOT_time;
        w_ptr->QtR[1][0]=-(w_ptr->Qs);
        w_ptr->QtR[1][1]=rst->ROBOT_time+(w_ptr->time)*0.2;
        w_ptr->QtR[2][0]=w_ptr->Qf;
        w_ptr->QtR[2][1]=rst->ROBOT_time+(w_ptr->time);
        w_ptr->QtR[3][0]=0.0;
        w_ptr->QtR[3][1]=rst->ROBOT_time+(w_ptr->time)*1.2;

        for(i=0;i<4;i++)
        {

                d_vector[i]=w_ptr->QtR[i][0];
                t_vector[i]=w_ptr->QtR[i][1];
        }

        yp1=0.0;
        ypn=0.0;
        spline(t_vector,d_vector,4,yp1,ypn,a_vector);

        for(i=0;i<4;i++)
                w_ptr->QtR[i][2]=a_vector[i];

}
	

else if(body=='L')//Left leg
{
        //eje X

        w_ptr->XtL[0][0]=rst->aVL[0];
        w_ptr->XtL[0][1]=rst->ROBOT_time;

        w_ptr->XtL[1][0]=rst->aVL[0]+(Lan*sin(w_ptr->Qs)+Laf*(1-cos(w_ptr->Qs)));
        w_ptr->XtL[1][1]=rst->ROBOT_time+(w_ptr->time)*0.2;

        w_ptr->XtL[3][0]=rst->aVL[0]+(w_ptr_1->Ds)+(w_ptr->Ds)-Lan*sin(w_ptr->Qf)-Lab*(1-cos(w_ptr->Qf));
        w_ptr->XtL[3][1]=rst->ROBOT_time+(w_ptr->time);

        w_ptr->XtL[4][0]=rst->aVL[0]+(w_ptr_1->Ds)+(w_ptr->Ds);
        w_ptr->XtL[4][1]=rst->ROBOT_time+(w_ptr->time)*1.2;

        w_ptr->XtL[2][0]=rst->aVL[0]+(w_ptr->XtL[4][0]-rst->aVL[0])/2.0;
        w_ptr->XtL[2][1]=rst->ROBOT_time+(w_ptr->time)*0.6;


        for(i=0;i<5;i++)
        {

                d_vector[i]=w_ptr->XtL[i][0];
                t_vector[i]=w_ptr->XtL[i][1];
        }

        yp1=0.0;
        ypn=0.0;
        spline(t_vector,d_vector,5,yp1,ypn,a_vector);

        for(i=0;i<5;i++)
                w_ptr->XtL[i][2]=a_vector[i];
        //eje Y
        w_ptr->YtL[0][0]=rst->aVL[1];
        w_ptr->YtL[0][1]=rst->ROBOT_time;
        w_ptr->YtL[1][0]=rst->aVL[1]-Lan+Lan*sin(w_ptr->Qs)+Lan*cos(w_ptr->Qs);
        w_ptr->YtL[1][1]=rst->ROBOT_time+(w_ptr->time)*0.2;
        w_ptr->YtL[2][0]=rst->aVL[1]+((w_ptr->h+(w_ptr_1->h-w_ptr->h)/2.0)/4.0);
        w_ptr->YtL[2][1]=rst->ROBOT_time+(w_ptr->time)*0.6;
        w_ptr->YtL[3][0]=w_ptr->YtR[4][0]-Lan+Lab*sin(w_ptr->Qf)+Lan*cos(w_ptr->Qf);
        w_ptr->YtL[3][1]=rst->ROBOT_time+(w_ptr->time);
        w_ptr->YtL[4][0]=w_ptr->YtR[4][0];
        w_ptr->YtL[4][1]=rst->ROBOT_time+(w_ptr->time)*1.2;


        for(i=0;i<5;i++)
        {

                d_vector[i]=w_ptr->YtL[i][0];
                t_vector[i]=w_ptr->YtL[i][1];
        }

        yp1=0.0;
        ypn=0.0;
        spline(t_vector,d_vector,5,yp1,ypn,a_vector);

        for(i=0;i<5;i++)
                w_ptr->YtL[i][2]=a_vector[i];


        //tobillo
        w_ptr->QtL[0][0]=0.0;//ONLY FOR SMOOTH TERRAIN
        w_ptr->QtL[0][1]=rst->ROBOT_time;
        w_ptr->QtL[1][0]=-(w_ptr->Qs);
        w_ptr->QtL[1][1]=rst->ROBOT_time+(w_ptr->time)*0.2;
        w_ptr->QtL[2][0]=w_ptr->Qf;
        w_ptr->QtL[2][1]=rst->ROBOT_time+(w_ptr->time);
        w_ptr->QtL[3][0]=0.0;
        w_ptr->QtL[3][1]=rst->ROBOT_time+(w_ptr->time)*1.2;

        for(i=0;i<4;i++)
        {

                d_vector[i]=w_ptr->QtL[i][0];
                t_vector[i]=w_ptr->QtL[i][1];
        }

        yp1=0.0;
        ypn=0.0;
        spline(t_vector,d_vector,4,yp1,ypn,a_vector);

        for(i=0;i<4;i++)
                w_ptr->QtL[i][2]=a_vector[i];

}


else if(body=='C')
{
	//w_ptr->Xed=w_ptr->Xsd=2.0;
	//calc. of points for hip trajectory
	if( rst->STATE_RL == DS_INI )
	{
        w_ptr->Xc[0][0]=w_ptr->XtR[0][0]+((w_ptr_1->Ds)-(w_ptr->Xed+w_ptr->Xsd))/2+w_ptr->Xed;
        w_ptr->Xc[0][1]=w_ptr->XtR[0][1]+(w_ptr->time)*0.1;
        w_ptr->Xc[1][0]=w_ptr->XtR[0][0]+w_ptr_1->Ds;
        w_ptr->Xc[1][1]=w_ptr->XtR[0][1]+(w_ptr->time)*0.6;
        w_ptr->Xc[2][0]=w_ptr->XtR[0][0]+(w_ptr_1->Ds)+((w_ptr->Ds)+(w_ptr->Xed-w_ptr->Xsd))/2;
        w_ptr->Xc[2][1]=w_ptr->XtR[0][1]+(w_ptr->time)*1.1;

	w_ptr->Zc[0][0]=0;//w_ptr->Zc[0][0];
        w_ptr->Zc[0][1]=w_ptr->XtR[0][1]+(w_ptr->time)*0.1;
        w_ptr->Zc[1][0]=w_ptr->Zc[0][0]+w_ptr->Ls;
        w_ptr->Zc[1][1]=w_ptr->XtR[0][1]+(w_ptr->time)*0.6;
        w_ptr->Zc[2][0]=w_ptr->Zc[0][0];
        w_ptr->Zc[2][1]=w_ptr->XtR[0][1]+(w_ptr->time)*1.1;
	}
	else if ( rst->STATE_LL == DS_INI )
	{
        w_ptr->Xc[0][0]=w_ptr->XtL[0][0]+((w_ptr_1->Ds)-(w_ptr->Xed+w_ptr->Xsd))/2+w_ptr->Xed;
        w_ptr->Xc[0][1]=w_ptr->XtL[0][1]+(w_ptr->time)*0.1;
        w_ptr->Xc[1][0]=w_ptr->XtL[0][0]+w_ptr_1->Ds;
        w_ptr->Xc[1][1]=w_ptr->XtL[0][1]+(w_ptr->time)*0.6;
        w_ptr->Xc[2][0]=w_ptr->XtL[0][0]+(w_ptr_1->Ds)+((w_ptr->Ds)+(w_ptr->Xed-w_ptr->Xsd))/2;
        w_ptr->Xc[2][1]=w_ptr->XtL[0][1]+(w_ptr->time)*1.1;

        w_ptr->Zc[0][0]=0;//w_ptr->Zc[0][0];
        w_ptr->Zc[0][1]=w_ptr->XtL[0][1]+(w_ptr->time)*0.1;
        w_ptr->Zc[1][0]=w_ptr->Zc[0][0]-w_ptr->Ls;
        w_ptr->Zc[1][1]=w_ptr->XtL[0][1]+(w_ptr->time)*0.6;
        w_ptr->Zc[2][0]=w_ptr->Zc[0][0];
        w_ptr->Zc[2][1]=w_ptr->XtL[0][1]+(w_ptr->time)*1.1;

	}
//prueba
/*        valor=w_ptr->Xed;
        ftoa(cadena,valor,0);
        rtl_printf("wp--valor de Xed = %s  \n",cadena);
        valor=w_ptr->Xsd;
        ftoa(cadena,valor,0);
        rtl_printf("wp--valor de Xsd = %s  \n",cadena);

        valor=w_ptr_1->Ds;
        ftoa(cadena,valor,0);
        rtl_printf("wp--valor de Ds-1 = %s  \n",cadena);
        valor=w_ptr->Ds;
        ftoa(cadena,valor,0);
        rtl_printf("wp--valor de Ds = %s  \n",cadena);

        valor=w_ptr->Xc[0][0];
        ftoa(cadena,valor,0);
        rtl_printf("wp--valor de Xc 0 = %s  \n",cadena);
        valor=w_ptr->XtR[1][0];
        ftoa(cadena,valor,0);
        rtl_printf("wp--valor de Xc 1 = %s  \n",cadena);
        valor=w_ptr->Xc[2][0];
        ftoa(cadena,valor,0);
        rtl_printf("wp--valor de Xc 2 = %s  \n",cadena);
*/

        for(i=0;i<3;i++)
        {

                d_vector[i]=w_ptr->Xc[i][0];
                t_vector[i]=w_ptr->Xc[i][1];
        }

        yp1=0.0;
        ypn=0.0;
        spline(t_vector,d_vector,3,yp1,ypn,a_vector);

	for(i=0;i<3;i++)
		w_ptr->Xc[i][2]=a_vector[i];

        for(i=0;i<3;i++)
        {

                d_vector[i]=w_ptr->Zc[i][0];
                t_vector[i]=w_ptr->Zc[i][1];
        }

        yp1=0.0;
        ypn=0.0;
        spline(t_vector,d_vector,3,yp1,ypn,a_vector);

        for(i=0;i<3;i++)
                w_ptr->Zc[i][2]=a_vector[i];


        //actualizacion de parametros
    //    w_ptr_1->Ds = w_ptr->Ds;


        if( rst->STATE_RL == DS_INI )
        {
        w_ptr->Yc[0][0]=w_ptr->YtL[4][0]-Lan+w_ptr_1->hmin;
        w_ptr->Yc[0][1]=w_ptr->Xc[0][1];
        w_ptr->Yc[1][0]=w_ptr->YtL[4][0]-Lan+w_ptr->h;
        w_ptr->Yc[1][1]=w_ptr->Xc[1][1];
        w_ptr->Yc[2][0]=w_ptr->YtL[4][0]-Lan+w_ptr->hmin;
        w_ptr->Yc[2][1]=w_ptr->Xc[2][1];
	}


	if( rst->STATE_LL == DS_INI )
        {
        w_ptr->Yc[0][0]=w_ptr->YtR[4][0]-Lan+w_ptr_1->hmin;
        w_ptr->Yc[0][1]=w_ptr->Xc[0][1];
        w_ptr->Yc[1][0]=w_ptr->YtR[4][0]-Lan+w_ptr->h;
        w_ptr->Yc[1][1]=w_ptr->Xc[1][1];
        w_ptr->Yc[2][0]=w_ptr->YtR[4][0]-Lan+w_ptr->hmin;
        w_ptr->Yc[2][1]=w_ptr->Xc[2][1];
        }

	//actualizacion de parametros
	w_ptr_1->Ds=w_ptr->Ds;
	w_ptr_1->hmin=w_ptr->hmin;
	w_ptr_1->h=w_ptr->h;

        for(i=0;i<3;i++)
        {

                d_vector[i]=w_ptr->Yc[i][0];
                t_vector[i]=w_ptr->Yc[i][1];
        }

        yp1=0.0;
        ypn=0.0;
        spline(t_vector,d_vector,3,yp1,ypn,a_vector);

        for(i=0;i<3;i++)
                w_ptr->Yc[i][2]=a_vector[i];

}
	return 0;

}


void spline (float x[],float y[],int n,float yp1,float ypn,float y2[])
{
int i,k;
float p,qn,sig,un,*u;


	u=vector(1,n-1);
	if (yp1 > 0.99e30)
		y2[0]=u[0]=0.0;
	else
	{
		y2[0]=-0.5;
		u[0]=(3.0 / (x[1]-x[0]) ) * ( ( (y[1]-y[0]) / (x[1]-x[0])) -yp1);
	}

	for(i=1;i<=n-2;i++)
	{
		sig=(x[i]-x[i-1])/(x[i+1]-x[i-1]);
		p=sig*y2[i-1]+2.0;

		y2[i]=(sig-1.0)/p;

		u[i]=(y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
		u[i]=(6.0*u[i]/(x[i+1]-x[i-1])-sig*u[i-1])/p;

	}

	if (ypn > 0.99e30)
		qn=un=0.0;
	else
	{
                qn=0.5;
                un=(3.0/(x[n-1]-x[n-2])) * (ypn-(y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
	}

	y2[n-1]=(un-qn*u[n-2]) / (qn*y2[n-2]+1.0);
	for (k=n-2;k >= 0;k--)
	{
		y2[k]=y2[k]*y2[k+1]+u[k];

		sig=y2[k];

	}
	free_vector(u,1,n-1);

}


//allocate a int vector with subcript range
float *vector (long nl, long nh)
{
float *v;

	v=(float *)kmalloc((size_t) ((nh-nl+1+NR_END)*sizeof(unsigned char)), GFP_ATOMIC);
	if (!v) return 0;
	return v-nl+NR_END;
}

//free a float vector allocated with vector
void free_vector(float *v,long nl,long nh)
{
//	kfree((FREE_ARG)(v+nl-NR_END));
}