/*			control.c

*/

#include <rtl_debug.h>
#include <time.h>
#include <pthread.h>
#include <rtl_sched.h>
#include <rtl_mutex.h>
//#include <rtl_printf.h>
//#include <asm/io.h>
#include "walk_params.h"
#include "fsm_robot.h"
#include "gen_tray.h"
#include "glob_var.h"
//#include "vsprintf.h"

extern struct w_params gt_param_inbox;
extern char next_robot_st;
extern struct generacion_tray gt_io_box;
extern pthread_mutex_t gt_fmtx, gt_rri_fmtx;

int chan_params (struct w_params *w_p);
void init_robot_params (struct robot_state *robot_st,struct w_params *w_p);



void *control_thread(void *arg)
{

struct w_params w_p_1,w_p;
struct robot_state rob_st;

float valor;
char cadena[11];
	
	
	init_robot_params(&rob_st,&w_p);

	while(1)
	{
//	rtl_printf("estoy en la funcion control \n");
	pthread_mutex_lock(&gt_fmtx);


//	if(rob_st.aVR[0]==0)
//		rtl_printf("/////////////////////////////////////////////// \n");
	fsm_robot(&rob_st,&w_p_1,&w_p);
//prueba
/*        valor=rob_st.ROBOT_time;
        ftoa(cadena,valor,0);
        rtl_printf("valor de robot_time = %s \n",cadena);
        valor=w_p.XtR[0][0];
        ftoa(cadena,valor,0);
        rtl_printf("valor de XtR 0 = %s \n",cadena);
        valor=w_p.XtR[1][0];
        ftoa(cadena,valor,0);
        rtl_printf("valor de XtR 1 = %s \n",cadena);
        valor=w_p.XtR[2][0];
        ftoa(cadena,valor,0);
        rtl_printf("valor de XtR 2 = %s \n",cadena);
        valor=w_p.XtR[3][0];
        ftoa(cadena,valor,0);
        rtl_printf("valor de XtR 3 = %s \n",cadena);
        valor=w_p.XtR[4][0];
        ftoa(cadena,valor,0);
        rtl_printf("valor de XtR 4 = %s \n",cadena);
*/


	
	if(rob_st.R_STATE != STAND)
		{

		//paso de parametros calculados a variable compartida
		gt_io_box.R_STATE=rob_st.R_STATE;
		gt_io_box.STATE_RL=rob_st.STATE_RL;
		gt_io_box.STATE_LL=rob_st.STATE_LL;
		gt_io_box.ROBOT_time=rob_st.ROBOT_time;
		gt_io_box.time=w_p.time;

		}

	gentray(&w_p,&rob_st);
	
	pthread_mutex_unlock(&gt_rri_fmtx);	//rri thread free

	}//end while

	return 0;

}


//funcion que inicializa el estado del robot
//en el futuro deberá de leer el estado del robot y modificarlo a unos valores iniciales
void init_robot_params(struct robot_state *robot_st,struct w_params *w_p)
{
int i;

	//first write in walking parameters box

        gt_param_inbox.h=22.0;
        gt_param_inbox.Ds=10.0;
        gt_param_inbox.Ls=12.0;
        gt_param_inbox.vel=20.0;
        gt_param_inbox.hmin=20.0;
        gt_param_inbox.Qs=0.1;//0.1745
        gt_param_inbox.Qf=0.1;
        gt_param_inbox.Hgs=0.0;
        gt_param_inbox.Hge=0.0;
        gt_param_inbox.Xsd=2.0;
        gt_param_inbox.Xed=2.0;

        w_p->YtR[4][0]=4.0;
        w_p->YtL[4][0]=4.0;
/*	DEBE INICIALIZARSE
	for(i=0;i<3;i++)
	{
	gt_param_inbox.Xc[i][0]=0;
	gt_param_inbox.Yc[i][0]=23;
	gt_param_inbox.Zc[i][0]=0;
	}
*/
	//update the initial parameters
	robot_st->R_STATE=STAND;
	robot_st->FR_STATE=WALK;
	robot_st->STATE_RL=GROUND;
	robot_st->STATE_RL_1=GROUND;
	robot_st->STATE_LL=GROUND;
	robot_st->STATE_LL_1=GROUND;
	next_robot_st=WALK;

	robot_st->ROBOT_time=0.0;
	
	robot_st->aVR[0]=0;
	robot_st->aVR[1]=4.0;
	robot_st->aVR[2]=4.25;
	robot_st->aVL[0]=0;
	robot_st->aVL[1]=4.0;
	robot_st->aVL[2]=-4.25;
	robot_st->aVC[0]=0;
	robot_st->aVC[1]=gt_param_inbox.hmin;
	robot_st->aVC[2]=0;
}