/*
 * robot_state.c
 *
 *  Created on: Aug 2, 2024
 *      Author: akeguo
 */
#include "robot_state.h"

//Manual_State_Entry, Manual_State_Do, Manual_State_Exit

void Manual_State_Exit(void)
{
	LOG("Manual_State_Exit");
}

void Manual_State_Entry(void)
{
	LOG("Manual_Operation_Entry");
}
void Manual_State_Do(void)
{
	//LOG("Manual_State_Do");

	double Wheel_Speed_Max=3000;
	double Turning_speed=1000;

	double Rocker_Range=1000;

	double Speed_Scale=1;
	double Rocker_factor=1;

	double X_Value_1=0;
	double Y_Value_1=0;
	double X_Value_2=0;
	double Y_Value_2=0;

	double R_Speed_Con=0;
	double L_Speed_Con=0;

	int32_t Sum_T=GV.MK32_Key.CH11_RD1+1000;
	Speed_Scale=((double)Sum_T)/2000;

	X_Value_1=GV.MK32_Key.CH0_RY_H+0.00001;
	Y_Value_1=GV.MK32_Key.CH1_RY_V+0.00001;
	//Normalization processing
	X_Value_2=X_Value_1/Rocker_Range;
	Y_Value_2=Y_Value_1/Rocker_Range;

	double Rocker_angle=atan(Y_Value_2/X_Value_2);
	double Vector_modulus=sqrt(X_Value_2*X_Value_2+Y_Value_2*Y_Value_2);


	L_Speed_Con=Y_Value_2*Wheel_Speed_Max-X_Value_2*Turning_speed;
	R_Speed_Con=Y_Value_2*Wheel_Speed_Max+X_Value_2*Turning_speed;


	//Adjusting speed based on modulus
	if (Vector_modulus>1)
	{
		Vector_modulus=1;
	}
	else if (Vector_modulus<0.1)
	{
		Vector_modulus=0;
	}

	L_Speed_Con=L_Speed_Con*Vector_modulus;
	R_Speed_Con=R_Speed_Con*Vector_modulus;



	if (fabs(Rocker_angle>1.4835))
	{
		if (R_Speed_Con>0)
		{ 	R_Speed_Con=Wheel_Speed_Max*Vector_modulus;
			L_Speed_Con=Wheel_Speed_Max*Vector_modulus;
		}
		else
		 {
			R_Speed_Con=-Wheel_Speed_Max*Vector_modulus;
		    L_Speed_Con=-Wheel_Speed_Max*Vector_modulus;
		  }
	 }

	if (fabs(R_Speed_Con)>Wheel_Speed_Max)
	{
		Rocker_factor=fabs(Wheel_Speed_Max/R_Speed_Con);
	}
	else if (fabs(L_Speed_Con)>Wheel_Speed_Max)
	{
		Rocker_factor=fabs(Wheel_Speed_Max/L_Speed_Con);
	}
	else
	{
		Rocker_factor=1;
	}

	GV.Motor_P1.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P2.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P3.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
	GV.Motor_P4.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;

	GV.Motor_P1.Target_Velcity= Speed_Scale*Rocker_factor*R_Speed_Con;
	GV.Motor_P2.Target_Velcity=-Speed_Scale*Rocker_factor*L_Speed_Con;
	GV.Motor_P3.Target_Velcity= Speed_Scale*Rocker_factor*R_Speed_Con;
	GV.Motor_P4.Target_Velcity=-Speed_Scale*Rocker_factor*L_Speed_Con;

}


void Halt_State_Entry(void)
{
	LOG("Halt_State_Entry");
}
void Halt_State_Do(void)
{


	GV.Motor_P1.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
			GV.Motor_P1.Target_Velcity=0;
			GV.Motor_P2.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
			GV.Motor_P2.Target_Velcity=0;
			GV.Motor_P3.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
			GV.Motor_P3.Target_Velcity=0;
			GV.Motor_P4.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
			GV.Motor_P4.Target_Velcity=0;
}
void Halt_State_Exit(void)
{
	LOG("Halt_State_Exit");
}

void Abnormal_State_Entry(void)
{
	LOG("Halt_State_Entry");
}
void Abnormal_State_Do(void)
{
	LOG("Abnormal_State_Do");

}
void Abnormal_State_Exit(void)
{
	LOG("Abnormal_State_Exit");
}