#include "chassis.h"

#include "can.h"
#include "device/at9s_pro.h"
#include "component/pid.h"
#include "device/motor_rm.h"
#include <math.h>
#include "bsp/time.h"

KPID_t pid_wheel[4];
KPID_Params_t pid_wheel_params={1, 0.8, 0.0, 0.0, 0.0, 0.8,-1.0,-1.0};

KPID_t pid_lockagl[4];
KPID_Params_t pid_lockagl_params={1, 0.8, 0.05, 0.0, 0.1, 0.8,-1.0,M_2PI};

KPID_t pid_lockomg[4];
KPID_Params_t pid_lockomg_params={2, 1, 0.0, 0.01, 0.0, 0.8,-1.0,-1.0};


KPID_t pid_follow;
KPID_Params_t pid_follow_params={0.5, 1, 0, 0.0, 0, 0.0, -1.0, -1};

MOTOR_RM_Param_t motor1to4_param[4]={
    {.can=BSP_CAN_1,.id=0x201,.module=MOTOR_M3508,.reverse=false,.gear=true,},
    {.can=BSP_CAN_1,.id=0x202,.module=MOTOR_M3508,.reverse=false,.gear=true,},
    {.can=BSP_CAN_1,.id=0x203,.module=MOTOR_M3508,.reverse=false,.gear=true,},
    {.can=BSP_CAN_1,.id=0x204,.module=MOTOR_M3508,.reverse=false,.gear=true,}
};

MOTOR_Feedback_t motor1to4_measure[4];
float lockagl_out[4];
float chassis_out[4];
float Wheel_status[4];
void Inverse_resolve(float vx,float vy,float w)
{
    Wheel_status[0]=vx-vy+w;
    Wheel_status[1]=vx+vy+w;
    Wheel_status[2]=-vx+vy+w;
    Wheel_status[3]=-vx-vy+w;
}

void Motor_Init(float freq)
{	for(int i=0;i<4;i++)
	{
	PID_Init(&pid_wheel[i], KPID_MODE_CALC_D, freq,&pid_wheel_params);
		PID_Init(&pid_lockagl[i], KPID_MODE_CALC_D, freq,&pid_lockagl_params);
	PID_Init(&pid_lockomg[i], KPID_MODE_CALC_D, freq,&pid_lockomg_params);

	}
	
		
	PID_Init(&pid_follow, KPID_MODE_NO_D, freq,&pid_follow_params);
    BSP_CAN_Init();
    for(int i=0;i<4;i++){
	MOTOR_RM_Register(&motor1to4_param[i]);
    }
}
int64_t now,last_wakeup;
float dt;
float vx,vy,w;float chassis_yaw;
float speed[4];
float lockagl[4]={0,0,0,0};
void chassis_control(chassis_ctrl_eulr_t eulr, COMP_AT9S_CMD_t cmd_rc)//k:ת���޷�
{
	//float vx,vy,w;
	now 		=BSP_TIME_Get_us() / 1000000.0f;
    dt 			=(BSP_TIME_Get_us() - last_wakeup) / 1000000.0f;
    last_wakeup	=BSP_TIME_Get_us();
	float delta_angle;
	float cos_delta_angle;
	float sin_delta_angle;
    for(int i=0;i<4;i++){
	MOTOR_RM_Update(&motor1to4_param[i]);
    MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&motor1to4_param[i]);
		if(motor_fed!=NULL)
		{
			motor1to4_measure[i]=motor_fed->motor.feedback;
		}
    }
	switch(cmd_rc.mode){
		case 0:
	vx=0;vy=0;w=0;
		break;
		case 1:
	delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
      cos_delta_angle = cosf(delta_angle);
      sin_delta_angle = sinf(delta_angle);
      vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
      vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
		w=PID_Calc(&pid_follow, eulr.chassis_mech_zero_yaw,
                  eulr.chassis_encoder_yaw, 0.0f, dt);
		break;
		case 2:
	PID_ResetIntegral(&pid_follow);
		
	  delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
      cos_delta_angle = cosf(delta_angle);
      sin_delta_angle = sinf(delta_angle);
      vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
      vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
		
		w=PID_Calc(&pid_follow, eulr.chassis_mech_zero_yaw,
                  eulr.chassis_encoder_yaw, 0.0f, dt);
		break;
		case 3:
	  delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
      cos_delta_angle = cosf(delta_angle);
      sin_delta_angle = sinf(delta_angle);
      vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
      vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
		
	  w=0.6f; 
		break;
		default :
			break;
	}
	
	
    Inverse_resolve(vx, vy, w);
	float abs_max = 0.0f;
	for (int8_t i = 0; i < 4; i++) {
		const float abs_val = fabsf(Wheel_status[i]);
		abs_max = (abs_val > abs_max) ? abs_val : abs_max;
	}
	if (abs_max > 1.0f) {
		for (int8_t i = 0; i < 4; i++) {
			Wheel_status[i] /= abs_max;
		}
	}
	
//	if(vx==0&&vy==0)PID_ResetIntegral(&pid_wheel);
	switch(cmd_rc.mode){
		case 0:
			PID_ResetIntegral(&pid_wheel[0]);
			PID_ResetIntegral(&pid_wheel[1]);
			PID_ResetIntegral(&pid_wheel[2]);
			PID_ResetIntegral(&pid_wheel[3]);
	chassis_out[0] = 0;
    chassis_out[1] = 0;
    chassis_out[2] = 0;
    chassis_out[3] = 0;
		break;
		case 1:
			PID_ResetIntegral(&pid_wheel[0]);
			PID_ResetIntegral(&pid_wheel[1]);
			PID_ResetIntegral(&pid_wheel[2]);
			PID_ResetIntegral(&pid_wheel[3]);
//	chassis_out[0] = 0;
//    chassis_out[1] = 0;
//    chassis_out[2] = 0;
//    chassis_out[3] = 0;
		for(int i=0;i<4;i++){
				speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
			
				if(speed[i]>1)speed[i]=1;
		else if(speed[i]<-1)speed[i]=-1;
				lockagl_out[i] = PID_Calc(&pid_lockagl[i],lockagl[i],motor1to4_measure[i].rotor_abs_angle, 0,dt);
				chassis_out[i] = PID_Calc(&pid_lockomg[i],lockagl_out[i],speed[i], 0,dt);
				if(chassis_out[i]>0.8f)chassis_out[i]=0.8f;
			}
		break;
		case 2:
			for(int i=0;i<4;i++){
				
				lockagl[i]=motor1to4_measure[i].rotor_abs_angle;
				
				speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
				if(speed[i]>1)speed[i]=1;
		else if(speed[i]<-1)speed[i]=-1;
				chassis_out[i] = PID_Calc(&pid_wheel[i],Wheel_status[i],speed[i], 0,dt);
				
			}
			
		break;                                                                            
		case 3:                                                                           
		for(int i=0;i<4;i++){
			
				lockagl[i]=motor1to4_measure[i].rotor_abs_angle;
			
				speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
				if(speed[i]>1)speed[i]=1;
		else if(speed[i]<-1)speed[i]=-1;
				chassis_out[i] = PID_Calc(&pid_wheel[i],Wheel_status[i],speed[i], 0,dt);
			
			}
		break;
		default :
			break;
	}

//	for(int i;i<4;i++){
//		chassis_out[i]*=0.6;
//	}
    MOTOR_RM_SetOutput(&motor1to4_param[0], chassis_out[0]);
    MOTOR_RM_SetOutput(&motor1to4_param[1], chassis_out[1]);
    MOTOR_RM_SetOutput(&motor1to4_param[2], chassis_out[2]);
    MOTOR_RM_SetOutput(&motor1to4_param[3], chassis_out[3]);
//MOTOR_RM_SetOutput(&motor1to4_param[1], 0);
//    MOTOR_RM_SetOutput(&motor1to4_param[2], 0);
    MOTOR_RM_Ctrl(&motor1to4_param[0]);
}