#include "shoot_control.h"
#include <string.h>
#include "can.h"
#include "component/filter.h"
#include "component/user_math.h"
#include <math.h>
#include "bsp/time.h"

static bool last_firecmd;

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;
    }
}


int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
{
    if (s == NULL) {
        return -1; // 参数错误
    }
    s->mode=mode;
    return 0;
}

int8_t Shoot_ResetIntegral(Shoot_t *s)
{
    if (s == NULL) {
        return -1; // 参数错误
    }
    for(int i=0;i<SHOOT_FRIC_NUM;i++)
    {	
        PID_ResetIntegral(&s->pid.fric_follow[i]);
        PID_ResetIntegral(&s->pid.fric_err[i]);
    }
    PID_ResetIntegral(&s->pid.trig);
    PID_ResetIntegral(&s->pid.trig_omg);
    return 0;
}

int8_t Shoot_ResetCalu(Shoot_t *s)
{
    if (s == NULL) {
        return -1; // 参数错误
    }
    for(int i=0;i<SHOOT_FRIC_NUM;i++)
    {	
        PID_Reset(&s->pid.fric_follow[i]);
        PID_Reset(&s->pid.fric_err[i]);
        LowPassFilter2p_Reset(&s->filter.fric.in[i], 0.0f);
        LowPassFilter2p_Reset(&s->filter.fric.out[i], 0.0f);
    }
    PID_Reset(&s->pid.trig);
    PID_Reset(&s->pid.trig_omg);
    LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f);
    LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f);
    return 0;
}

int8_t Shoot_ResetOutput(Shoot_t *s)
{
    if (s == NULL) {
        return -1; // 参数错误
    }
    for(int i=0;i<SHOOT_FRIC_NUM;i++)
    {	
        s->output.out_follow[i]=0.0f;
        s->output.out_err[i]=0.0f;
        s->output.out_fric[i]=0.0f;
        s->output.lpfout_fric[i]=0.0f;
    }
	s->output.outagl_trig=0.0f;
	s->output.outomg_trig=0.0f;
    s->output.outlpf_trig=0.0f;
    return 0;
}

int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
{
    if (s == NULL) {
        return -1; // 参数错误
    }
    s->target_variable.target_rpm=4000.0f/MAX_FRIC_RPM;
    return 0;
}
/**
 * \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
 *
 * \param s 包含发射数据的结构体
 * \param num 需要发射的弹丸数量
 */
int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
{
    if (s == NULL  || s->anglecalu.num_to_shoot == 0) {
        return -1; 
    }
    if(s->now - s->anglecalu.time_last_shoot >= s->param->shot_delay_time && cmd->firecmd)
    {
        s->anglecalu.time_last_shoot=s->now;
        s->target_variable.target_angle += s->param->trig_step_angle;
		if(s->target_variable.target_angle>M_PI)s->target_variable.target_angle-=M_2PI;
		else if((s->target_variable.target_angle<-M_PI))s->target_variable.target_angle+=M_2PI;
		s->anglecalu.num_to_shoot--;
    }
    return 0;
}

int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
{
    if (s == NULL || param == NULL || target_freq <= 0.0f) {
        return -1; // 参数错误
    }
    s->param=param;

    BSP_CAN_Init();
    for(int i=0;i<SHOOT_FRIC_NUM;i++){
	MOTOR_RM_Register(&param->fric_motor_param[i]);
    PID_Init(&s->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,&param->fric_follow);
	PID_Init(&s->pid.fric_err[i], KPID_MODE_CALC_D, target_freq,&param->fric_err);
    LowPassFilter2p_Init(&s->filter.fric.in[i], target_freq, s->param->filter.fric.in);
    LowPassFilter2p_Init(&s->filter.fric.out[i], target_freq, s->param->filter.fric.out);
    }
	MOTOR_RM_Register(&param->trig_motor_param);
    PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig);
    PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,&param->trig_omg);

    LowPassFilter2p_Init(&s->filter.trig.in, target_freq, s->param->filter.trig.in);
    LowPassFilter2p_Init(&s->filter.trig.out, target_freq, s->param->filter.trig.out);
	
    memset(&s->anglecalu,0,sizeof(s->anglecalu));
    memset(&s->output,0,sizeof(s->output));

	return 0;
}

int8_t Chassis_UpdateFeedback(Shoot_t *s)
{
    if (s == NULL) {
        return -1; // 参数错误
    }
    float rpm_sum=0.0f;
    for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
        /* 更新摩擦电机反馈 */
        MOTOR_RM_Update(&s->param->fric_motor_param[i]);
		MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->fric_motor_param[i]);
		if(motor_fed!=NULL)
		{
			s->feedback.fric[i]=motor_fed->motor.feedback;
		}
        /* 滤波反馈rpm */
        s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed);
        /* 归一化rpm */
        s->feedback.fric_rpm[i] = s->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM;
        if(s->feedback.fric_rpm[i]>1.0f)s->feedback.fric_rpm[i]=1.0f;
        if(s->feedback.fric_rpm[i]<-1.0f)s->feedback.fric_rpm[i]=-1.0f;
        /* 计算平均rpm */
        rpm_sum+=s->feedback.fric_rpm[i];
    }
    s->feedback.fric_avgrpm=rpm_sum/SHOOT_FRIC_NUM;
	/* 更新拨弹电机反馈 */
    MOTOR_RM_Update(&s->param->trig_motor_param);
	MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->trig_motor_param);
		if(motor_fed!=NULL)
		{
			s->feedback.trig=motor_fed->feedback;
		}
    /* 将多圈角度归化到单圈 (-M_PI, M_PI) */
		s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle;
		s->feedback.trig_angle_cicle = fmod(s->feedback.trig_angle_cicle, M_2PI); // 将角度限制在 [-2π, 2π]
		if (s->feedback.trig_angle_cicle > M_PI) {
		s->feedback.trig_angle_cicle -= M_2PI; // 调整到 [-π, π]
		}else if (s->feedback.trig_angle_cicle < -M_PI) {
		s->feedback.trig_angle_cicle += M_2PI; // 调整到 [-π, π]
		}
    
    s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.rotor_speed);
    s->feedback.trig_rpm = s->feedback.trig.rotor_speed / MAX_TRIG_RPM;
    if(s->feedback.trig_rpm>1.0f)s->feedback.trig_rpm=1.0f;  //如果单环效果好就删
    if(s->feedback.trig_rpm<-1.0f)s->feedback.trig_rpm=-1.0f;
    
    s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed;
	return 0;
}

int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
{
    if (s == NULL || cmd == NULL) {
        return -1; // 参数错误
    }
    if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
        for(int i=0;i<SHOOT_FRIC_NUM;i++)
        {	
            MOTOR_RM_Relax(&s->param->fric_motor_param[i]);
        }
        MOTOR_RM_Relax(&s->param->trig_motor_param);
    }
    else{
		static float pos;
        switch(s->running_state)
        {
			
            case SHOOT_STATE_IDLE:/*熄火等待*/
                for(int i=0;i<SHOOT_FRIC_NUM;i++)
                {	
                    PID_ResetIntegral(&s->pid.fric_follow[i]);
                    s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->feedback.fric_rpm[i],0,s->dt);
                    s->output.out_fric[i]=s->output.out_follow[i];
                    s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
                    MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
                }
                
                s->output.outagl_trig   =PID_Calc(&s->pid.trig,pos,s->feedback.trig_angle_cicle,0,s->dt);
                s->output.outomg_trig  =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
                s->output.outlpf_trig   =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
                MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
                if(cmd->ready)
                    {   
                        Shoot_ResetCalu(s);
                        Shoot_ResetIntegral(s);
                        Shoot_ResetOutput(s);
                        s->running_state=SHOOT_STATE_READY;
                    }
                break;
            case SHOOT_STATE_READY:/*准备射击*/
                
                for(int i=0;i<SHOOT_FRIC_NUM;i++)
                {	/* 计算跟随输出、计算修正输出 */ 
                    s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
                    s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
                    /* 按比例缩放并加和输出 */
                    ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);                    
                    s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
                    /* 滤波 */
                    s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
                    /* 输出 */
                    MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
                }
                /* 拨弹电机输出 */
                s->output.outagl_trig   =PID_Calc(&s->pid.trig,pos,s->feedback.trig_angle_cicle,0,s->dt);
                s->output.outomg_trig  =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
                s->output.outlpf_trig   =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
                MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
                
                /* 检查状态机 */
                if(!cmd->ready)
                    {
                        Shoot_ResetCalu(s);
                        Shoot_ResetOutput(s);
                        s->running_state=SHOOT_STATE_IDLE;
                    }
                    else if(last_firecmd==false&&cmd->firecmd==true)
                    {
                        s->running_state=SHOOT_STATE_FIRE;
                        s->anglecalu.num_to_shoot+=s->param->shot_burst_num;
					
                    }
                break;
            case SHOOT_STATE_FIRE:
				Shoot_CaluTargetAngle(s, cmd);
                for(int i=0;i<SHOOT_FRIC_NUM;i++)
                {	
                    s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
                    s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
                    ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
                    s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
                    s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
                    MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
                }
                s->output.outagl_trig   =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
                s->output.outomg_trig  =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
                s->output.outlpf_trig   =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
                MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
                if(!cmd->firecmd)
                    {
                        s->running_state=SHOOT_STATE_READY;
						pos=s->feedback.trig_angle_cicle;
                    }
                break;
            default:
                s->running_state=SHOOT_STATE_IDLE;
                break;
        }
    }
	MOTOR_RM_Ctrl(&s->param->fric_motor_param[0]);
	MOTOR_RM_Ctrl(&s->param->fric_motor_param[4]);
    last_firecmd = cmd->firecmd;
    return 0;
}


int8_t Shoot_JamDetectionFSM(Shoot_t *s, Shoot_CMD_t *cmd)
{
    if (s == NULL) {
        return -1; // 参数错误
    }
    
    switch (s->jamdetection.jamfsm_state) {
        case SHOOT_JAMFSM_STATE_NORMAL:
            if (s->feedback.trig.torque_current/1000.0f > s->param->jam_threshold) {
                s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_SUSPECTED;
                s->jamdetection.jam_last_time = s->now; // 记录怀疑开始时间
            }
            Shoot_RunningFSM(s, cmd); // 正常运行状态下继续执行射击状态机
            break;

        case SHOOT_JAMFSM_STATE_SUSPECTED:
            if (s->feedback.trig.torque_current/1000.0f < s->param->jam_threshold) {
                s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_NORMAL;
                break;
            } else {
                if ((s->now - s->jamdetection.jam_last_time) >= s->param->jam_suspected_time) {
                    s->jamdetection.jam_detected =true;
					s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_CONFIRMED;
                    break;
                }
            }
            Shoot_RunningFSM(s, cmd);
            break;

        case SHOOT_JAMFSM_STATE_CONFIRMED:
            // 进入处理状态，修改拨弹盘目标角度
            s->target_variable.target_angle = s->feedback.trig_angle_cicle-0.5f*s->param->trig_step_angle;
            s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_DEAL;
            s->jamdetection.jam_last_time = s->now; // 记录处理开始时间
        case SHOOT_JAMFSM_STATE_DEAL:
            Shoot_RunningFSM(s, cmd);
            if ((s->now - s->jamdetection.jam_last_time)>=0.3f&&s->output.outlpf_trig < 0.1f) { /* 给予0.3秒响应时间并检测输出，认为堵塞已解除 */
                s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_NORMAL;
            }
            break;

        default:
            s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_NORMAL;
            break;
    }

    return 0;
}

int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
{
    if (s == NULL || cmd == NULL) {
        return -1; // 参数错误
    }
    s->now = BSP_TIME_Get_us() / 1000000.0f;
    s->dt = (BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f;
    s->lask_wakeup = BSP_TIME_Get_us();
	s->online = cmd->online;//改成检测电机是否在线
    //电机在线检测函数
    Shoot_JamDetectionFSM(s, cmd);
    return 0;
}