#include "shoot.h"
#include <string.h>
#include "component/filter.h"
#include "component/user_math.h"
#include <math.h>
#include "bsp/time.h"
#include "device/motor_rm.h"
#include "module/config.h"

static bool last_firecmd;

/**
 * @brief 确保两个输出权重的和不超过 1.0f，用于限幅
 */
static inline void ScaleSumTo1(float *a, float *b) {
    float sum = fabsf(*a) + fabsf(*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; 
    return 0;
}

int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
{
    if (s == NULL || s->shoot_Anglecalu.num_to_shoot == 0) {
        return -1; 
    }
    if(s->now - s->shoot_Anglecalu.time_last_shoot >= s->param->shot_delay_time && cmd->firecmd)
    {
        s->shoot_Anglecalu.time_last_shoot = s->now;
        s->target_variable.target_angle += s->param->trig_step_angle;
        // 角度限制在 [-PI, PI]
        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->shoot_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->shoot_Anglecalu, 0, sizeof(s->shoot_Anglecalu));
    memset(&s->output, 0, sizeof(s->output));
    s->target_variable.target_rpm = 6000.0f;

    return 0;
}

int8_t Shoot_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;
        }
        s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed);
        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_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_trig = MOTOR_RM_GetMotor(&s->param->trig_motor_param);
    if(motor_fed_trig != NULL) {
        s->feedback.trig = motor_fed_trig->feedback;
    }

    s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle;
    s->feedback.trig_angle_cicle = fmod(s->feedback.trig_angle_cicle, M_2PI); 
    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_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 = (float)(BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f;
    s->lask_wakeup = BSP_TIME_Get_us();
    s->online = cmd->online;

    if(!s->online){
        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 {
        switch(s->running_state)
        {
            case SHOOT_STATE_IDLE:/* 停止状态：下发 0 电流，避免反电动势制动 */
                for(int i = 0; i < SHOOT_FRIC_NUM; i++)
                {    
                    // 直接将输出清零，不走 PID 计算
                    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;
                    
                    // 重置 PID 积分项，防止下一次启动时瞬间过流
                    PID_ResetIntegral(&s->pid.fric_follow[i]);
                    PID_ResetIntegral(&s->pid.fric_err[i]);

                    MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], 0.0f);
                }
                
                // 拨弹电机保持现有位置控制
                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->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, 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->ready)
                {
                    s->running_state = SHOOT_STATE_IDLE;
                }
                else if(last_firecmd == false && cmd->firecmd == true)
                {
                    s->running_state = SHOOT_STATE_FIRE;
                    s->shoot_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->shoot_Anglecalu.num_to_shoot == 0)
                {
                    s->running_state = SHOOT_STATE_READY;
                }
                break;

            default:
                s->running_state = SHOOT_STATE_IDLE;
                break;
        }
    }

    // 执行底层电机控制
    for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
        MOTOR_RM_Ctrl(&s->param->fric_motor_param[i]);
    }
    MOTOR_RM_Ctrl(&s->param->trig_motor_param);

    last_firecmd = cmd->firecmd;
    return 0;
}