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


#define PI 3.1415926

#define ROT_ORIGIN -204292264
#define PIT_ORIGIN -37108744


double Ref_Speed;
double Act_Speed;
double Speed_Act = 0;
double Y_Value_1;
double X_Value_1;
double Rocker_angle;
double Pit_time = 0;
double Rot_time = 0;
double Rrocker_angle;
double Rotation_speed=0;
double Pitch_speed=0;
double RP_speed_act = 0;
double Speed_Ctrl = 0;
double RP_Speed_Ctrl = 0;

uint8_t Rot_flag = 0;
uint8_t Pit_lim_flag = 0;
int32_t Pit_Position_fir = 0;
int32_t Rot_Position_fir = 0;
uint8_t Rot_dir_flag = 0;
uint8_t Pitch_dir_flag = 0;


void Manual_State_Do(void)
{
	double Rocker_Range=2000;		//摇杆范围
	double X_Value_1=0;
	double Y_Value_1=0;
	double X_Value_2=0;
	double Y_Value_2=0;


	/*转向测试变量设置BEGIN*/
	//转向标记，1为左转，-1为右转
	char is_turning = 0;
	//右前电机
	double R1_Speed_Con=0;
	//左前电机
	double L1_Speed_Con=0;
	//右后电机
	double R2_Speed_Con=0;
	//左后电机
	double L2_Speed_Con=0;
	//转向速度基准
	int32_t Turining_Velcity_Base=1000;

	//CH0_RY_H [-1000,1000]
	X_Value_1=GV.MK32_Key.CH3_LY_H+0.00001;
	//CH1_RY_V [-1000,1000]
	Y_Value_1=GV.MK32_Key.CH2_LY_V+0.00001;
	//归一化
	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);

	if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 0))
	{
		Speed_Ctrl =  (GV.MK32_Key.CH10_LD1 + 1000) * 360 / 1000;
	}
	else {
		Speed_Ctrl = GV.MK32_Key.CH10_LD1 * (4400 - 360) / 1000 + 360;
	}

	Ref_Speed =   Speed_Ctrl;
	Act_Speed =Speed_Ctrl;
	if(Ref_Speed >= 5000) Ref_Speed = 5000;

	if(fabs(Rocker_angle) > PI*0.35)
	{
		//前后行驶
		if(GV.MK32_Key.CH2_LY_V > 0)
		{
			//前进
			R1_Speed_Con = -1;
			L1_Speed_Con = 1;
			R2_Speed_Con = -1;
			L2_Speed_Con = 1;
		}
		else if(GV.MK32_Key.CH2_LY_V < 0)
		{
			//后退
			R1_Speed_Con = 1;
			L1_Speed_Con = -1;
			R2_Speed_Con = 1;
			L2_Speed_Con = -1;
		}
	}
	else if(fabs(Rocker_angle) < PI*0.15)
	{
		if(GV.MK32_Key.CH3_LY_H > 0)
		{

			//右转前行
			R1_Speed_Con = -0.35;
			L1_Speed_Con = 1;
			R2_Speed_Con = -0.35;
			L2_Speed_Con = 1;
		}
		else if(GV.MK32_Key.CH3_LY_H <= 0)
		{
			//左转前行
			R1_Speed_Con = -1;
			L1_Speed_Con = 0.35;
			R2_Speed_Con = -1;
			L2_Speed_Con = 0.35;
		}
	}
	else
	{
		R1_Speed_Con = 0;
		L1_Speed_Con = 0;
		R2_Speed_Con = 0;
		L2_Speed_Con = 0;
	}
	GV.LeftFrontMotor.Target_Velcity = L1_Speed_Con * Ref_Speed;
	GV.RightFrontMotor.Target_Velcity = R1_Speed_Con * Ref_Speed;
	GV.LeftBackMotor.Target_Velcity = L2_Speed_Con * Ref_Speed;
	GV.RightBackMotor.Target_Velcity = R2_Speed_Con * Ref_Speed;
}

void Auto_Back_State_Do(void)
{
	if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 0))
	{
		Speed_Ctrl =  (GV.MK32_Key.CH10_LD1 + 1000) * 360 / 1000;
	}
	else {
		Speed_Ctrl = GV.MK32_Key.CH10_LD1 * (4400 - 360) / 1000 + 360;
	}
	Ref_Speed =  Speed_Ctrl;
	Act_Speed =  Speed_Ctrl;
	if(Ref_Speed >= 5000) Ref_Speed = 5000;

	GV.LeftFrontMotor.Target_Velcity = - Ref_Speed;
	GV.RightFrontMotor.Target_Velcity =  Ref_Speed;
	GV.LeftBackMotor.Target_Velcity = -Ref_Speed;
	GV.RightBackMotor.Target_Velcity = Ref_Speed;
}


void SwingManual_Do(void)
{
	double RX_Value_1=0;
	double RY_Value_1=0;
	double RX_Value_2=0;
	double RY_Value_2=0;

	//CH0_RY_H [-1000,1000]
	RY_Value_1=GV.MK32_Key.CH1_RY_V + 0.00001;
	//CH1_RY_V [-1000,1000]
	RX_Value_1=GV.MK32_Key.CH0_RY_H + 0.00001;
	//归一化
	RX_Value_2=RX_Value_1/1000;
	RY_Value_2=RY_Value_1/1000;
	double Operation_dead_zone=sqrt((RX_Value_2*RX_Value_2+RY_Value_2*RY_Value_2));
	Rrocker_angle = atan(RY_Value_2 / RX_Value_2);

	RP_Speed_Ctrl  = (GV.MK32_Key.CH11_RD1 + 1000) * 800 / 2000;

	RP_speed_act = RP_Speed_Ctrl;

	if(fabs(Rrocker_angle)<=0.15*PI && Operation_dead_zone>=0.3)  //旋转电机
	{
		if(RX_Value_2<0)
		{
			GV.SwingMotor.Target_Velcity = Rotation_speed + RP_Speed_Ctrl;
		}
		else if(RX_Value_2>0)
		{
			GV.SwingMotor.Target_Velcity =  -(Rotation_speed + RP_Speed_Ctrl);
		}
	}
	else
	{
		GV.SwingMotor.Target_Velcity =  0;
	}

	if(GV.MK32_Key.CH6_SC == 1000)  //旋转电机首次定位
	{
		Rot_Position_fir = GV.SwingMotor.Position;
		Rot_current_time = 0;
		Rot_dir_flag = 0;
	}
}
void TiltManual_Do(void)
{
	double RX_Value_1=0;
	double RY_Value_1=0;
	double RX_Value_2=0;
	double RY_Value_2=0;

	RP_Speed_Ctrl  = (GV.MK32_Key.CH11_RD1 + 1000) * 800 / 2000;

	//CH0_RY_H [-1000,1000]
	RY_Value_1=GV.MK32_Key.CH1_RY_V + 0.00001;
	//CH1_RY_V [-1000,1000]
	RX_Value_1=GV.MK32_Key.CH0_RY_H + 0.00001;
	//归一化
	RX_Value_2=RX_Value_1/1000;
	RY_Value_2=RY_Value_1/1000;
	double Operation_dead_zone=sqrt((RX_Value_2*RX_Value_2+RY_Value_2*RY_Value_2));
	Rrocker_angle = atan(RY_Value_2 / RX_Value_2);

	if(fabs(Rrocker_angle)>=0.35*PI && Operation_dead_zone>=0.3)  //俯仰电机
	{
		if(RY_Value_2 >= 0)
		{
			GV.TiltMotor.Target_Velcity =  RP_Speed_Ctrl;
		}
		else if(RY_Value_2 <= 0)
		{
			GV.TiltMotor.Target_Velcity =  - RP_Speed_Ctrl;
		}
	}
	else
	{
		GV.TiltMotor.Target_Velcity =  0;
	}

	if(GV.MK32_Key.CH6_SC == -1000)  //俯仰电机首次定位
	{
		Pit_Position_fir = GV.TiltMotor.Position;
		Pit_current_time = 0;
		Pitch_dir_flag = 0;
	}

}


void SwingAuto_Do(void)
{
	double rp_speed_ctrl = RP_Speed_Ctrl;

	Rot_time = 0;
	Rot_time = abs(Rot_Position_fir - (ROT_ORIGIN + 20000)) / rp_speed_ctrl;
	Rot_time = Rot_time / 720.275;

		switch(Rot_dir_flag)  //先右转再左转
		{
			case 0:

				GV.SwingMotor.Target_Velcity = -rp_speed_ctrl;

				if(Rot_current_time >= Rot_time)
				{
					GV.SwingMotor.Target_Velcity = 0;
					Rot_dir_flag = 1;
					Rot_current_time = 0;
				}
				break;
			case 1:

				GV.SwingMotor.Target_Velcity = rp_speed_ctrl;
				if(Rot_current_time >= Rot_time)
				{
					GV.SwingMotor.Target_Velcity = 0;
					Rot_dir_flag = 0;
					Rot_current_time = 0;
				}
				break;
		}
}

void TiltAuto_Do(void)
{
	double rp_speed_ctrl = RP_Speed_Ctrl;
	Pit_time = 0;
	Pit_time = abs(Pit_Position_fir - (PIT_ORIGIN + 20000)) / rp_speed_ctrl;
	Pit_time = Pit_time / 720.275;

	switch(Pitch_dir_flag)  //下转再上转
	{
		case 0:
			GV.TiltMotor.Target_Velcity = -rp_speed_ctrl;

			if(Pit_current_time >= Pit_time)
			{
				GV.TiltMotor.Target_Velcity = 0;
				Pitch_dir_flag = 1;
				Pit_current_time = 0;
			}
		break;
		case 1:
			GV.TiltMotor.Target_Velcity = rp_speed_ctrl;
			if(Pit_current_time >= Pit_time)
			{
				GV.TiltMotor.Target_Velcity = 0;
				Pitch_dir_flag = 0;
				Pit_current_time = 0;
			}
		break;
	}
}

void SwingHome_Do(void)
{
	if(GV.SwingMotor.Position <= ROT_ORIGIN - 20000)  //使用速度模式，在ROT_ORIGIN左右20000范围内为原点位置
	{
		GV.SwingMotor.Target_Velcity = 100;
	}
	else if(GV.SwingMotor.Position >= ROT_ORIGIN + 20000)
	{
		GV.SwingMotor.Target_Velcity = -100;
	}
	else {
		GV.SwingMotor.Target_Velcity = 0;
	}

//	GV.SwingMotor.Target_Velcity = -100;
//	if((GV.SwingMotor.Position >= (ROT_ORIGIN - 20000)) &&
//			(GV.SwingMotor.Position <= (ROT_ORIGIN + 20000)))
//	{
//		GV.SwingMotor.Target_Velcity = 0;
//	}
}

void TiltHome_Do(void)
{
	if(GV.TiltMotor.Position <= PIT_ORIGIN - 20000)
	{
		GV.TiltMotor.Target_Velcity = 100;
	}
	else if(GV.TiltMotor.Position >= PIT_ORIGIN + 20000)
	{
		GV.TiltMotor.Target_Velcity = -100;
	}
	else {
		GV.TiltMotor.Target_Velcity = 0;
	}

//	GV.TiltMotor.Target_Velcity = -100;
//	if((GV.TiltMotor.Position >= (PIT_ORIGIN - 20000)) &&
//			(GV.TiltMotor.Position <= (PIT_ORIGIN + 20000)))
//	{
//		GV.TiltMotor.Target_Velcity = 0;
//	}
}

void Auto_State_Do(void)
{

}

void Abnormal_State_Do(void)
{
	LOG("Abnormal_State_Do");

}

void Forwards_State_Do(void)
{
	GV.LeftFrontMotor.Target_Velcity = GV.Move_Speed;
	GV.LeftBackMotor.Target_Velcity = GV.Move_Speed;
	GV.RightBackMotor.Target_Velcity = -GV.Move_Speed;
	GV.RightFrontMotor.Target_Velcity = -GV.Move_Speed;

}

void Backwards_State_Do(void)
{
	GV.LeftFrontMotor.Target_Velcity = -GV.Move_Speed;
	GV.LeftBackMotor.Target_Velcity = -GV.Move_Speed;
	GV.RightBackMotor.Target_Velcity = GV.Move_Speed;
	GV.RightFrontMotor.Target_Velcity = GV.Move_Speed;
}
void TurnLeft_State_Do(void)
{
	GV.LeftFrontMotor.Target_Velcity = -CV.TurnLeftRightSpeed;
	GV.LeftBackMotor.Target_Velcity = -CV.TurnLeftRightSpeed;
	GV.RightBackMotor.Target_Velcity = -CV.TurnLeftRightSpeed;
	GV.RightFrontMotor.Target_Velcity = -CV.TurnLeftRightSpeed;
}
void TurnRight_State_Do(void)
{
	GV.LeftFrontMotor.Target_Velcity =CV.TurnLeftRightSpeed;
	GV.LeftBackMotor.Target_Velcity = CV.TurnLeftRightSpeed;
	GV.RightBackMotor.Target_Velcity = CV.TurnLeftRightSpeed;
	GV.RightFrontMotor.Target_Velcity = CV.TurnLeftRightSpeed;
}
void HALT_State_Do(void)
{
	GV.LeftFrontMotor.Target_Velcity = 0;
	GV.LeftBackMotor.Target_Velcity = 0;
	GV.RightBackMotor.Target_Velcity = 0;
	GV.RightFrontMotor.Target_Velcity = 0;
}
//Swing Motor

void SwingHALT_State_Do(void)
{
	GV.SwingMotor.Target_Velcity = 0;
}
void SwingLeft_State_Do(void)
{
//
//	if (GV.SwingMotor.Position > GV.SwingMotor.EncoderOffset + 45 * 65536 * 101 / 360)
//	{
//		GV.SwingMotor.Target_Velcity = 0;
//	} else
//	{
//		GV.SwingMotor.Target_Velcity = *JontSwingSpeed;
//	}
//	GV.SwingMotor.Target_Velcity = *JontSwingSpeed;

}
void SwingRight_State_Do(void)
{

//	if (GV.SwingMotor.Position < GV.SwingMotor.EncoderOffset - 45 * 65536 * 101 / 360)
//	{
//		GV.SwingMotor.Target_Velcity = 0;
//	} else
//	{
//		GV.SwingMotor.Target_Velcity = -*JontSwingSpeed;
//	}
//	GV.SwingMotor.Target_Velcity = -*JontSwingSpeed;
}



void OtherMode_State_Do(void)
{

}

void TiltUp_Do()
{
//	GV.TiltMotor.Run_Mode=2;
//	//GV.TiltMotor.Target_Position+=500;
//	GV.TiltMotor.Target_Velcity = *JontTiltSpeed;
//	if(GV.TiltMotor.Position>=GV.TiltMotor.EncoderOffset+45 * 65536 * 101 / 360)
//	{
//		GV.TiltMotor.Target_Velcity =0;
//	}else
//	{
//		GV.TiltMotor.Target_Velcity = *JontTiltSpeed;
//	}

//	GV.TiltMotor.Target_Velcity = *JontTiltSpeed;
}

void TiltDown_Do()
{
	GV.TiltMotor.Run_Mode=2;
	//GV.TiltMotor.Target_Position-=500;
//	GV.TiltMotor.Target_Velcity = -*JontTiltSpeed;
}
;
void TiltHalt_Do()
{
	GV.TiltMotor.Target_Velcity = 0;
}



void TiltCurrentModeDown_Do()
{

//	0：停止模式
//	1：电流模式
//	2：速度模式
//	3：位置模式
	GV.TiltMotor.Run_Mode=1;
	//GV.TiltMotor.Target_Current = -600;//set current position as home position
	//do nothing
}





