/*
 * robot_state.c
 *
 *  Created on: Aug 2, 2024
 *      Author: akeguo
 */
#include "robot_state.h"
#include "fsm.h"
#include "BHBF_ROBOT.h"
#include "BSP/bsp_GPIO.h"
#include "MSP/msp_PID.h"
#include "msp_DMKE_485_Motor.h"

int32_t *DHRoughening_Speed[10]; //目标速度

int32_t *DHRoughening_LaneChangeDistance[10]; //目标速度

int32_t *Roughenging_LeftCompensation;
int32_t *Roughenging_RightCompensation;
int32_t *Roughenging_LeftCompensation_Range;
int32_t *Roughenging_RightCompensation_Range;

//Manual_State_Entry, Manual_State_Do, Manual_State_Exit
void Calbrate_Robot_Positon(int position);
int32_t Horizontal_Target_Angle = 0;

void Move_Forwards_Do(int32_t Target_Angle);
void Move_Backwards_Do(int32_t Target_Angle);

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

}

void Forwards_State_Do(void)
{
	if (IV.RunMode != IV_Run_Mode_Automation)
	{
		IV.RunMode = IV_Run_Mode_Forward;
	}
	GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed;
	GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed;
}

void Backwards_State_Do(void)
{
//	if (IV.RunMode != IV_Run_Mode_Automation)
//	{
//		IV.RunMode = IV_Run_Mode_BackWard;
//	}
	IV.RunMode = IV_Run_Mode_BackWard;
	GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed;
	GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed;
}
void TurnLeft_State_Do(void)
{

//	if (IV.RunMode != IV_Run_Mode_Automation)
//	{
//		IV.RunMode = IV_Run_Mode_TurnLeft;
//	}
	IV.RunMode = IV_Run_Mode_TurnLeft;
	GV.LeftMotor.Target_Velcity = (int32_t) (-CV.LeftTurnSpeed / 0.880 * 101 * 10);
	GV.RightMotor.Target_Velcity = (int32_t) (CV.LeftTurnSpeed / 0.880 * 101 * 10);
}
void TurnRight_State_Do(void)
{
//	if (IV.RunMode != IV_Run_Mode_Automation)
//	{
//		IV.RunMode = IV_Run_Mode_TurnRight;
//	}
	IV.RunMode = IV_Run_Mode_TurnRight;
	GV.LeftMotor.Target_Velcity = (int32_t) (CV.RightTurnSpeed / 0.880 * 101 * 10);
	GV.RightMotor.Target_Velcity = (int32_t) (-CV.RightTurnSpeed / 0.880 * 101 * 10);
}
void HALT_State_Do(void)
{

//	if (IV.RunMode != IV_Run_Mode_EmergencyStop && IV.RunMode != IV_Run_Mode_Automation)
//	{
//		IV.RunMode = IV_Run_Mode_HALT;
//	}
	IV.RunMode = IV_Run_Mode_HALT;
	GV.LeftMotor.Target_Velcity = 0;
	GV.RightMotor.Target_Velcity = 0;
}

//竖直方向前进 保持竖直运行
static int dletAngle = 0;

void Move_Vertical_Task_Forwards_Do()
{
	Move_Forwards_Do(CV.RobotUpAngleValue + PV_App.Vertical_Calibration * 100);
}
//竖直方向后退前进 保持竖直运行
void Move_Vertical_Task_Backwards_Do()
{
	Move_Backwards_Do(CV.RobotUpAngleValue + PV_App.Vertical_Calibration * 100);
}

char is_within_the_target_angle_flag = 0;
//水平方向前进，保持水平方向
void Move_Forwards_Do(int32_t Target_Angle)
{
	if (IV.RunMode != IV_Run_Mode_Automation)
	{
		IV.RunMode = IV_Run_Mode_Forward;
	}


	if (abs(GV.Robot_Angle - Target_Angle) <= 1000) //100倍关系，大于10度
	{
		if (abs(GV.Robot_Angle - Target_Angle) < 100)
		{
			dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 10, 0, 8, 1000);
			GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed - dletAngle / 2;
			GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed + dletAngle / 2;

		}
		else
		{
			dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 15, 0, 8, 2 / 0.880 * 101 * 10 * 2);
			GV.LeftMotor.Target_Velcity = -dletAngle / 2;
			GV.RightMotor.Target_Velcity = dletAngle / 2;
//			GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed - dletAngle / 2;
//						GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed + dletAngle / 2;

		}
	}
	else
	{
		dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 25, 0, 10, 4 / 0.880 * 101 * 10 * 2);
		GV.LeftMotor.Target_Velcity = -dletAngle / 2;
		GV.RightMotor.Target_Velcity = dletAngle / 2;
	}

}
//水平方向后退，保持水平方向

void Move_Backwards_Do(int32_t Target_Angle)
{
	if (IV.RunMode != IV_Run_Mode_Automation)
	{
		IV.RunMode = IV_Run_Mode_BackWard;
	}

	if (abs(GV.Robot_Angle - Target_Angle) <= 1000) //100倍关系，大于10度
	{
		if (abs(GV.Robot_Angle - Target_Angle) < 100)
		{
			dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 10, 0, 8, 1000);
			GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed - dletAngle / 2;
			GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed + dletAngle / 2;

		}
		else
		{
			dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 15, 0, 8, 2 / 0.880 * 101 * 10 * 2);
//			GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed - dletAngle / 2;
//								GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed + dletAngle / 2;

			GV.LeftMotor.Target_Velcity = -dletAngle / 2;
			GV.RightMotor.Target_Velcity = dletAngle / 2;
		}
	}
	else
	{
		dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 25, 0, 10, 4 / 0.880 * 101 * 10 * 2);
		GV.LeftMotor.Target_Velcity = -dletAngle / 2;
		GV.RightMotor.Target_Velcity = dletAngle / 2;
	}

}

void Move_Horizontal_Task_Forwards_Right_Do()
{
	Move_Forwards_Do(CV.RobotRightAngleValue - GV.Left_Compensation);

	//Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation);
}
void Move_Horizontal_Task_Backwards_Right_Do()
{
	Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation);

	//Move_Backwards_Do(CV.RobotRightAngleValue + GV.Right_Compensation);
}
void Move_Horizontal_Task_Forwards_Left_Do()
{
	Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Right_Compensation);

	//Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation);

}
void Move_Horizontal_Task_Backwards_Left_Do()
{
	Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation);

	//Move_Forwards_Do(CV.RobotLeftAngleValue - GV.Left_Compensation);
}

void Move_Head_To_Left_Do(void)
{
	Calbrate_Robot_Positon(CV.RobotLeftAngleValue);
	//Calbrate_Robot_Positon(CV.RobotLeftAngleValue - GV.Right_Compensation);
}
void Move_Head_To_Right_Do(void)
{
	Calbrate_Robot_Positon(CV.RobotRightAngleValue );
	//Calbrate_Robot_Positon(CV.RobotRightAngleValue + GV.Left_Compensation);
}
void Move_Head_To_UP_Do(void)
{
	Calbrate_Robot_Positon(CV.RobotUpAngleValue);

}
void Move_Head_To_Down_Do(void)
{
	Calbrate_Robot_Positon(CV.RobotDownAngleValue);
}


void Calbrate_Robot_Positon(int Target_Angle)
{

	if (abs(GV.Robot_Angle - Target_Angle) <= 20) //100倍关系，大于10度
	{
		GV.LeftMotor.Target_Velcity = 0;
		GV.RightMotor.Target_Velcity = 0;
	}
	else if (abs(GV.Robot_Angle - Target_Angle) < 9 * 100)
	{
		dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 15, 0, 8, 1 / 0.880 * 101 * 10 );
		GV.LeftMotor.Target_Velcity = -dletAngle / 2;
		GV.RightMotor.Target_Velcity = dletAngle / 2;
	}
	else
	{
		dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 25, 0, 10, 3 / 0.880 * 101 * 10 * 2);
		GV.LeftMotor.Target_Velcity = -dletAngle / 2;
		GV.RightMotor.Target_Velcity = dletAngle / 2;
	}
}

/* 终端上下移动，在洗舱里是抬头低头，这里是上下，直接沿用Tilt Up 和 TiltDown函数
 * */
void TiltUp_Do()
{
//	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 0);
//	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);

	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 0);
	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
}

void TiltDown_Do()
{

//	if (GV.ForceValue >= CV.PV.EndPushForceValue)
//	{
//		GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
//		GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
//	}
//	else
//	{
//		GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
//		GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 0);

	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 0);
//	}
}

void TiltHalt_Do()
{
	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
}

/* 拉毛函数控制
 * */
void RougheningEndHALT_Do()
{
	//GF_BSP_GPIO_SetIO(Wind_IO_CTL, K_OFF);
	DMKE_Motors[0]->Command = 0;

}

void RougheningEndSwingClockWise_Do()
{
	//GF_BSP_GPIO_SetIO(Wind_IO_CTL, K_ON);
	DMKE_Motors[0]->TargetSpeedRpm = 2800;


     DMKE_Motors[0]->TargetSpeedRpm = CV.DMK0_Speed;


	DMKE_Motors[0]->Command = 1;
}
void RougheningEndSwingAnticlockWise_Do()
{
	//GF_BSP_GPIO_SetIO(Wind_IO_CTL, K_ON);
	DMKE_Motors[0]->TargetSpeedRpm = 2800;

	DMKE_Motors[0]->TargetSpeedRpm = CV.DMK0_Speed;

	DMKE_Motors[0]->Command = -1;
}


void Wind_OFF()
{
	GF_BSP_GPIO_SetIO(Wind_IO_CTL, K_OFF);
}
void Wind_ON()
{
	GF_BSP_GPIO_SetIO(Wind_IO_CTL, K_ON);
}