#include "shoot_control.h"

#include "can.h"
#include "component/pid.h"
#include "component/filter.h"
#include "component/at9s_pro_cmd.h"
#include "device/motor_rm.h"
#include <math.h>
#include "bsp/dwt.h"
#include "device/vofa.h"
#define MAX_RPM_3508 7000.0f
uint32_t shoot_ctrl_cnt_last;
float shoot_ctrl_dt;
bool last_firecmd=false;
//要一个函数实现任意几个电机达到同样目标转速
KPID_t pid_shoot_3508_rpm[2];
KPID_Params_t pid_shoot_3508_rpm_params		={1.0f,  1.8f,  0.0f,  0.0f,  0.0f,  0.9f  ,0.0f,-1.0};

KPID_t pid_shoot_3508_errrpm[2];
KPID_Params_t pid_shoot_3508_errrpm_params	={1.0f,  4.0f,  0.40f,  0.04f,  0.25f,  0.25f  ,40.0f,-1.0};

KPID_t pid_shoot_3508_stop[2];
KPID_Params_t pid_shoot_3508_stop_params	={1,  1.2f,  0.5f,  0.01f, 0.0f,  0.0f  ,0.0f,-1.0};

static LowPassFilter2p_t spd_lpf[2];  
static LowPassFilter2p_t out_lpf[2];  

typedef enum {
    SHOOT_STATE_IDLE = 0,      // 熄火
    SHOOT_STATE_READY,         // 准备射击
    SHOOT_STATE_FIRE           // 射击
} ShootState_t;

ShootState_t shoot_state = SHOOT_STATE_IDLE;

//状态机  熄火-准备射击-射击
MOTOR_RM_Param_t shoot_motor_param[2];
MOTOR_RM_t* shoot_motor_feedback[2];


static inline void ScaleSumTo1(float *a, float *b) {
    float sum = *a + *b;
    if (sum > 1.0f) {
        float scale = 1.0f / sum;
        *a *= scale;
        *b *= scale;
    }
}


void shoot_motor_Init(float freq)
{	
//	VOFA_init(VOFA_PROTOCOL_JUSTFLOAT);
	DWT_Init(168);
    BSP_CAN_Init();
        shoot_motor_param[0].can=BSP_CAN_2;
        shoot_motor_param[0].id=0x201;
        shoot_motor_param[0].module=MOTOR_M3508;
        shoot_motor_param[0].reverse=false;
        shoot_motor_param[0].gear=false;
		MOTOR_RM_Register(&shoot_motor_param[0]);
        shoot_motor_param[1].can=BSP_CAN_2;
        shoot_motor_param[1].id=0x202;
        shoot_motor_param[1].module=MOTOR_M3508;
        shoot_motor_param[1].reverse=true;
        shoot_motor_param[1].gear=false;
		MOTOR_RM_Register(&shoot_motor_param[1]);
    for(int i=0;i<2;i++)
    {
		PID_Init(&pid_shoot_3508_rpm[i], KPID_MODE_CALC_D, freq,&pid_shoot_3508_rpm_params);
		PID_Init(&pid_shoot_3508_errrpm[i], KPID_MODE_CALC_D, freq,&pid_shoot_3508_errrpm_params);
		PID_Init(&pid_shoot_3508_stop[i], KPID_MODE_CALC_D, freq,&pid_shoot_3508_stop_params);
        LowPassFilter2p_Init(&spd_lpf[i], 1000.0f, 30.0f);
        LowPassFilter2p_Init(&out_lpf[i], 1000.0f, 30.0f);
    }
	
}
float target_rpm=1000.0f;
float shoot_out[2]={0,0};
float shoot_errout[2];
float shoot_errlpfout[2];
float filtered_rpm[2];

float errtosee;
void shoot_control(COMP_AT9S_CMD_t *rc)
{
    MOTOR_RM_UpdateAll();
	errtosee = shoot_motor_feedback[0]->feedback.rotor_speed - shoot_motor_feedback[1]->feedback.rotor_speed;
    if(rc->online)
    {
        float rel_rpm[2];
        for(int i=0;i<2;i++)
        {

            filtered_rpm[i] = LowPassFilter2p_Apply(&spd_lpf[i], shoot_motor_feedback[i]->feedback.rotor_speed);
            rel_rpm[i]=filtered_rpm[i]/MAX_RPM_3508;
            if(rel_rpm[i]>1.0f)rel_rpm[i]=1.0f;
            if(rel_rpm[i]<-1.0f)rel_rpm[i]=-1.0f;
        }
        float err_out[2];
		double rpm_avg;
        switch(shoot_state)
        {
            case SHOOT_STATE_IDLE:
				
                //位置闭写成环
                for(int i=0;i<2;i++)
                {	
					PID_ResetIntegral(&pid_shoot_3508_errrpm[i]);
                    MOTOR_RM_Relax(&shoot_motor_param[i]);
                }
				
                if(rc->shoot.ready)
                {
                    shoot_state=SHOOT_STATE_READY;
                }
                break;
            case SHOOT_STATE_READY:
                
                for(int i=0;i<2;i++)
                {	
                    shoot_motor_feedback[i] = MOTOR_RM_GetMotor(&shoot_motor_param[i]);
                    shoot_ctrl_dt=DWT_GetDeltaT(&shoot_ctrl_cnt_last);
                    shoot_out[i]=PID_Calc(&pid_shoot_3508_rpm[i],target_rpm/MAX_RPM_3508,rel_rpm[i],0,shoot_ctrl_dt);
                    for(int i=0;i<2;i++){
                        rpm_avg=(rel_rpm[0]+rel_rpm[1])/2.0f;
                    }
                    shoot_errout[i]=PID_Calc(&pid_shoot_3508_errrpm[i],rpm_avg,rel_rpm[i],0,shoot_ctrl_dt);
					/*若输出和大于1，按比例缩放，写成math里的函数*/
					shoot_errlpfout[i] = LowPassFilter2p_Apply(&out_lpf[i], shoot_errout[i]);
					ScaleSumTo1(&shoot_out[i], &shoot_errlpfout[i]);
					err_out[i] = shoot_out[i] + shoot_errout[i];
//                    err_out[i] = LowPassFilter2p_Apply(&out_lpf[i], err_out[i]);
                    MOTOR_RM_SetOutput(&shoot_motor_param[i], err_out[i]);
                }
                if(!rc->shoot.ready)
                {
                    shoot_state=SHOOT_STATE_IDLE;
                }
                else if(last_firecmd==false&&rc->shoot.firecmd==true)//可以加一个到达目标速度的判断
                {
                    shoot_state=SHOOT_STATE_FIRE;
                }
                break;
            case SHOOT_STATE_FIRE:
                for(int i=0;i<2;i++)
                {	
                    shoot_motor_feedback[i] = MOTOR_RM_GetMotor(&shoot_motor_param[i]);
                    shoot_ctrl_dt=DWT_GetDeltaT(&shoot_ctrl_cnt_last);
                    shoot_out[i]=PID_Calc(&pid_shoot_3508_rpm[i],target_rpm/MAX_RPM_3508,rel_rpm[i],0,shoot_ctrl_dt);
                    for(int i=0;i<2;i++){
                        rpm_avg=(rel_rpm[0]+rel_rpm[1])/2.0f;
                    }
                    shoot_errout[i]=PID_Calc(&pid_shoot_3508_errrpm[i],rpm_avg,rel_rpm[i],0,shoot_ctrl_dt);
					/*若输出和大于1，按比例缩放，写成math里的函数*/
					ScaleSumTo1(&shoot_out[i], &shoot_errout[i]);
					err_out[i] = shoot_out[i];
                    MOTOR_RM_SetOutput(&shoot_motor_param[i], err_out[i]);
                }
                
                 shoot_state=SHOOT_STATE_READY;
                
                break;
            default:
                shoot_state=SHOOT_STATE_IDLE;
                break;
        }
        last_firecmd=rc->shoot.firecmd;
        

		
    }else{
        shoot_state=SHOOT_STATE_IDLE;
		
        for(int i=0;i<2;i++)
        {
            PID_Reset(&pid_shoot_3508_rpm[i]);
            PID_Reset(&pid_shoot_3508_stop[i]);
            MOTOR_RM_Relax(&shoot_motor_param[0]);
        }
    }
	MOTOR_RM_Ctrl(&shoot_motor_param[0]);
}