/*		FSM_ROBOT.C
*/
#include <rtl_debug.h>
#include <rtl_sched.h>
#include <rtl_mutex.h>
//#include <rtl_printf.h>
#include "glob_var.h"
#include "walk_params.h"

extern struct w_params gt_param_inbox;
extern char next_robot_st;
extern pthread_mutex_t gt_param_inbox_vmtx;
int hip_pass;

void fsm_robot(struct robot_state *rst,struct w_params *w_p_1,struct w_params *w_p)
{

	rst->FR_STATE = next_robot_st;
//prueba
	if(rst->FR_STATE != STAND)
//		rtl_printf("esto esta andando no lo puedo creer \n");

					
        if((rst->FR_STATE) != (rst->R_STATE))
        {

                switch(rst->R_STATE)
                {
                        case STAND:
                                if ((rst->FR_STATE == WALK) && (rst->STATE_RL == GROUND)  && (rst->STATE_LL == GROUND) )
                                        rst->R_STATE = START;
                                else
                                {
                                        rst->R_STATE = STAND;
                                        rst->FR_STATE = STAND;
                                }
//prueba
                                w_p_1->Ds=0.0;
                                w_p_1->h=gt_param_inbox.h;
                                w_p_1->Ls=gt_param_inbox.Ls;
                                w_p_1->vel=gt_param_inbox.vel;
                                w_p_1->hmin=gt_param_inbox.hmin;
                                w_p_1->Qs=gt_param_inbox.Qs;
                                w_p_1->Qf=gt_param_inbox.Qf;

                                break;

                        case START:
                                if ( (rst->FR_STATE == WALK) && ((rst->STATE_RL == DS_END) || (rst->STATE_LL == DS_END)) )
					rst->R_STATE = WALK;

                                else if (rst->FR_STATE == STAND && ((rst->STATE_RL == DS_END) || (rst->STATE_LL == DS_END)) )
                                        rst->R_STATE = STOP;

//				rtl_printf("start \n");
                                break;

                        case WALK:
                                if (rst->FR_STATE == STAND )
                                {
                                        if (rst->STATE_RL == GROUND && rst->STATE_LL == DS_END)
                                                rst->R_STATE = STOP;
                                        if (rst->STATE_LL == GROUND && rst->STATE_RL == DS_END)
                                                rst->R_STATE = STOP;
                                }
//				rtl_printf("walk \n");
                                break;

                        case STOP:
                                if(rst->STATE_RL == GROUND && rst->STATE_LL == GROUND)
                                        rst->R_STATE = STAND;

//				rtl_printf("stop \n");
                                break;

                        default:
				break;

                }//end switch
        }//end if



//finite state machine for robot legs

	switch(rst->STATE_RL)//right leg
	{
		case GROUND:
//			rtl_printf("GROUND LEG \n");			
			if ( ( rst->ROBOT_time > (w_p->XtR[3][1]+w_p->time) && rst->R_STATE != STAND ) || (rst->R_STATE == START) )
			{
				//w_p_1= w_p;//walking parameters actualization 
				pthread_mutex_lock(&gt_param_inbox_vmtx);
				//w_p=&gt_param_inbox;//read walking parameters from inbox
				w_p->Ds=gt_param_inbox.Ds;
				w_p->h=gt_param_inbox.h;
				w_p->Ls=gt_param_inbox.Ls;
				w_p->vel=gt_param_inbox.vel;
				w_p->hmin=gt_param_inbox.hmin;
				w_p->Xsd=gt_param_inbox.Xsd;
				w_p->Xed=gt_param_inbox.Xed;
				w_p->Qs=gt_param_inbox.Qs;
				w_p->Qf=gt_param_inbox.Qf;
				pthread_mutex_unlock(&gt_param_inbox_vmtx);
				rst->STATE_RL = DS_INI;
				walking_params(w_p_1,w_p,rst,'R');
				w_p_1= w_p;//walking parameters actualization
				hip_pass=0;
			}

			rst->STATE_RL_1 = GROUND;

			break;

		case DS_INI:
//			rtl_printf("DS_INI LEG \n");

			if ( (rst->ROBOT_time >= (w_p->XtR[0][1]+0.1*w_p->time)) && (hip_pass==0) )//se recalculan los puntos de paso de la cadera
			{
				walking_params(w_p_1,w_p,rst,'C');
				hip_pass=1;
			}

                       if ( rst->ROBOT_time > w_p->XtR[1][1] )
                                rst->STATE_RL = AVANT;

                        rst->STATE_RL_1 = DS_INI;

			break;

		case AVANT:
//			rtl_printf("AVANT \n");
			if ( rst->ROBOT_time > w_p->XtR[3][1] )
				rst->STATE_RL = DS_END;

			rst->STATE_RL_1 = AVANT;

			break;

                case DS_END:
//			rtl_printf("DS_END \n");
                        if ( rst->ROBOT_time > w_p->XtR[4][1]  )
                                rst->STATE_RL = GROUND;

			rst->STATE_RL_1 = DS_END;

			break;

		default: break;

	} //switch end

        switch(rst->STATE_LL)
        {
                case GROUND:

                        if (rst->STATE_RL == DS_END )
			{
                                //w_p_1= w_p;//walking parameters actualization
                                pthread_mutex_lock(&gt_param_inbox_vmtx);
                                //w_p=&gt_param_inbox;//read walking parameters from inbox
                                w_p->Ds=gt_param_inbox.Ds;
                                w_p->h=gt_param_inbox.h;
                                w_p->Ls=gt_param_inbox.Ls;
                                w_p->vel=gt_param_inbox.vel;
                                w_p->hmin=gt_param_inbox.hmin;
				w_p->Qs=gt_param_inbox.Qs;
				w_p->Qf=gt_param_inbox.Qf;
                                pthread_mutex_unlock(&gt_param_inbox_vmtx);
                                rst->STATE_LL = DS_INI;
				hip_pass=0;
				walking_params(w_p_1,w_p,rst,'L');
				w_p_1= w_p;//walking parameters actualization
			}

                        rst->STATE_LL_1 = GROUND;

			break;

	        case DS_INI:

			if ( (rst->ROBOT_time >= (w_p->XtL[0][1]+0.1*w_p->time))  &&  (hip_pass==0) )//se recalculan los puntos de paso de la cadera
			{
				walking_params(w_p_1,w_p,rst,'C');
				hip_pass=1;
			}


                        if (rst->STATE_RL == GROUND )
                                rst->STATE_LL = AVANT;

                        rst->STATE_LL_1 = DS_INI;

			break;

                case AVANT:

                        if (rst->STATE_RL == DS_INI )
                                rst->STATE_LL = DS_END;

                        rst->STATE_LL_1 = AVANT;

			break;

                case DS_END:

                        if (rst->STATE_RL == AVANT )
                                rst->STATE_LL = GROUND;
                                
			rst->STATE_LL_1 = DS_END;

			break;

                default: break;

        } //switch end


        rst->ROBOT_time+=P_CONT*0.001;//global time robot refresh



}