#include "shoot_control.h"

#include "can.h"
#include "component/filter.h"
#include <math.h>
#include "bsp/dwt.h"

uint32_t shoot_ctrl_cnt_last;
float shoot_ctrl_dt;
bool last_firecmd=false;

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_Init(shoot_t *c, Shoot_Params_t *param, float target_freq)
{
    if (c == NULL || param == NULL || target_freq <= 0.0f) {
        return -1; // 参数错误
    }
    c->param=param;

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

int8_t Chassis_UpdateFeedback(shoot_t *c)
{
    if (c == NULL) {
        return -1; // 参数错误
    }

    float rpm_sum=0.0f;
    for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
        MOTOR_RM_Update(&c->param->fric_motor_param[i]);
		MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&c->param->fric_motor_param[i]);
		if(motor_fed!=NULL)
		{
			c->feedback.fric[i]=motor_fed->motor.feedback;
		}
        c->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&c->filter.fric.in[i], c->feedback.fric[i].rotor_speed);
        c->feedback.fric_rpm[i] = c->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM;
        if(c->feedback.fric_rpm[i]>1.0f)c->feedback.fric_rpm[i]=1.0f;
        if(c->feedback.fric_rpm[i]<-1.0f)c->feedback.fric_rpm[i]=-1.0f;
        rpm_sum+=c->feedback.fric_rpm[i];
    }
    c->feedback.fric_avgrpm=rpm_sum/SHOOT_FRIC_NUM;
    MOTOR_RM_Update(&c->param->trig_motor_param);
	c->feedback.trig = MOTOR_RM_GetMotor(&c->param->trig_motor_param);
    //
    c->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&c->filter.trig.in, c->feedback.trig->feedback.rotor_speed);
    c->feedback.trig_rpm = c->feedback.trig->feedback.rotor_speed / MAX_TRIG_RPM;
    if(c->feedback.trig_rpm>1.0f)c->feedback.trig_rpm=1.0f;  //如果单环效果好就删
    if(c->feedback.trig_rpm<-1.0f)c->feedback.trig_rpm=-1.0f;
    //
    c->errtosee = c->feedback.fric[0].rotor_speed - c->feedback.fric[1].rotor_speed;
	return 0;
}

int8_t Shoot_Control(shoot_t *c, const Shoot_CMD_t *cmd)
{
    if (c == NULL || cmd == NULL) {
        return -1; // 参数错误
    }
    c->dt = DWT_GetDeltaT(&c->lask_wakeup);
//    c->running_state = cmd->state;
	c->online = cmd->online;
    if(!c->online /*|| c->mode==SHOOT_MODE_SAFE*/){
        for(int i=0;i<SHOOT_FRIC_NUM;i++)
        {	
            MOTOR_RM_Relax(&c->param->fric_motor_param[i]);
        }
        MOTOR_RM_Relax(&c->param->trig_motor_param);
    }
    else{
        switch(c->running_state)
        {
            case SHOOT_STATE_IDLE:/*熄火等待*/
                for(int i=0;i<SHOOT_FRIC_NUM;i++)
                {	
                    PID_ResetIntegral(&c->pid.fric_follow[i]);
                    c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],0.0f,c->feedback.fric_rpm[i],0,c->dt);
                    c->output.out_fric[i]=c->output.out_follow[i];
                    c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]);
                    MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]);
                }
                c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt);
                MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig);
                if(cmd->ready)
                    {
                        c->running_state=SHOOT_STATE_READY;
                    }
                break;
            case SHOOT_STATE_READY:/*准备射击*/
                for(int i=0;i<SHOOT_FRIC_NUM;i++)
                {	/* 计算跟随输出->计算修正输出->加和、滤波、输出 */ 
                    c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],c->target_variable.target_rpm/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
                    c->output.out_err[i]=PID_Calc(&c->pid.fric_err[i],c->feedback.fric_avgrpm,c->feedback.fric_rpm[i],0,c->dt);
                    ScaleSumTo1(&c->output.out_follow[i], &c->output.out_err[i]);
                    c->output.out_fric[i]=c->output.out_follow[i]+c->output.out_err[i];
                    c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]);
                    MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]);
                }
                c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt);
                MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig);
                if(!cmd->ready)
                    {
                        c->running_state=SHOOT_STATE_IDLE;
                    }
                    else if(cmd->last_firecmd==false&&cmd->firecmd==true)//可以加一个到达目标速度的判断
                    {
                        c->running_state=SHOOT_STATE_FIRE;
						c->target_variable.target_angle+=c->param->trig_step_angle;
                    }
                break;
            case SHOOT_STATE_FIRE:
//                c->target_variable.target_angle+=c->param->trig_step_angle;
                for(int i=0;i<SHOOT_FRIC_NUM;i++)
                {	
                    c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],c->target_variable.target_rpm/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
                    c->output.out_err[i]=PID_Calc(&c->pid.fric_err[i],c->feedback.fric_avgrpm,c->feedback.fric_rpm[i],0,c->dt);
                    ScaleSumTo1(&c->output.out_follow[i], &c->output.out_err[i]);
                    c->output.out_fric[i]=c->output.out_follow[i]+c->output.out_err[i];
                    c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]);
                    MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]);
                }
                c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt);
                MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig);
                if(!cmd->firecmd)
                    {
                        c->running_state=SHOOT_STATE_READY;
                    }
                break;
            default:
                c->running_state=SHOOT_STATE_IDLE;
                break;
        }
    }
	MOTOR_RM_Ctrl(&c->param->fric_motor_param[0]);
    
    return 0;
}