#include "module/chassis.hpp"
#include "device/motor_rm.h"
#include "task/user_task.h"
#include "module/dr16.h"
#include "bsp/time.h"

#define CHASSIS_GAIN 100.0f

#define LIMIT(x, min, max) \
    do { if ((x) < (min)) (x) = (min); else if ((x) > (max)) (x) = (max); } while (0)

Chassis::Chassis() {
    motors_param[0] = {
        .can = BSP_CAN_2,
        .id = 0x201,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
    motors_param[1] = {
        .can = BSP_CAN_2,
        .id = 0x202,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
    motors_param[2] = {
        .can = BSP_CAN_2,
        .id = 0x203,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
    motors_param[3] = {
        .can = BSP_CAN_2,
        .id = 0x204,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
    pid_params[0] = KPID_Params_t{
        .k=0.1,
        .p=0.1,
        .i=0,
        .d=0,
        .i_limit=0.5,
        .out_limit=1,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_params[1] = KPID_Params_t{
        .k=0.1,
        .p=0.1,
        .i=0,
        .d=0,
        .i_limit=0.5,
        .out_limit=1,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_params[2] = KPID_Params_t{
        .k=0.1,
        .p=0.1,
        .i=0,
        .d=0,
        .i_limit=0.5,
        .out_limit=1,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_params[3] = KPID_Params_t{
        .k=0.1,
        .p=0.1,
        .i=0,
        .d=0,
        .i_limit=0.5,
        .out_limit=1,
        .d_cutoff_freq=100,
        .range = -1,
    };
}

void Chassis::init() {
    for(int i = 0; i < 4; i++){
        PID_Init(&pid[i], KPID_MODE_NO_D, CTRL_CHASSIS_FREQ, &pid_params[i]);
        MOTOR_RM_Register(&motors_param[i]);
    }
    last_time = BSP_TIME_Get_us();
}

void Chassis::operator()() {
    last_time = time;
    time = BSP_TIME_Get_us();
    dt = time - last_time;
    //更新数据
    update_motors();
    //解算底盘数据
    calc_chassis();
    //计算pid
    calc_pid();
    if(dr16.data.sw_l == DR16_SW_DOWN){
        MOTOR_RM_Relax(&motors_param[0]);
        MOTOR_RM_Relax(&motors_param[1]);
        MOTOR_RM_Relax(&motors_param[2]);
        MOTOR_RM_Relax(&motors_param[3]);
    }else {
        for(int i = 0; i < 4; i++){
            MOTOR_RM_SetOutput(&motors_param[i], pid_output[i]);
        }
    }
    //发送控制
    send_control();
    // MOTOR_RM_SetOutput(&motors_param[0], 0.1f);
    // MOTOR_RM_Ctrl(&motors_param[0]);


}

void Chassis::update_motors(){
    MOTOR_RM_UpdateAll();

    MOTOR_RM_t *motor = NULL;
    motor = MOTOR_RM_GetMotor(&motors_param[0]);
    chassis_motors[0] = *motor;
    motor = MOTOR_RM_GetMotor(&motors_param[1]);
    chassis_motors[1] = *motor;
    motor = MOTOR_RM_GetMotor(&motors_param[2]);
    chassis_motors[2] = *motor;
    motor = MOTOR_RM_GetMotor(&motors_param[3]);
    chassis_motors[3] = *motor;
}

void Chassis::calc_chassis(){
    if(dr16.data.sw_r == DR16_SW_DOWN){
        target_speed.m1 = (dr16.data.ch_l_x - dr16.data.ch_l_y + dr16.data.ch_r_x) * CHASSIS_GAIN;  // 右前
        target_speed.m2 = (-dr16.data.ch_l_x - dr16.data.ch_l_y + dr16.data.ch_r_x) * CHASSIS_GAIN;  // 右后
        target_speed.m3 = (-dr16.data.ch_l_x + dr16.data.ch_l_y + dr16.data.ch_r_x) * CHASSIS_GAIN;  // 左后
        target_speed.m4 = (dr16.data.ch_l_x + dr16.data.ch_l_y + dr16.data.ch_r_x) * CHASSIS_GAIN;  // 左前

        /* 限幅，防止溢出 */
        LIMIT(target_speed.m1, -1000, 1000);
        LIMIT(target_speed.m2, -1000, 1000);
        LIMIT(target_speed.m3, -1000, 1000);
        LIMIT(target_speed.m4, -1000, 1000);
    }else {
        target_speed.m1 = 0;
        target_speed.m2 = 0;
        target_speed.m3 = 0;
        target_speed.m4 = 0;
    }
}

void Chassis::calc_pid(){
    pid_output[0] = PID_Calc(&pid[0], target_speed.m1, chassis_motors[0].feedback.rotor_speed, 0, dt/1000000.0f);
    pid_output[1] = PID_Calc(&pid[1], target_speed.m2, chassis_motors[1].feedback.rotor_speed, 0, dt/1000000.0f);
    pid_output[2] = PID_Calc(&pid[2], target_speed.m3, chassis_motors[2].feedback.rotor_speed, 0, dt/1000000.0f);
    pid_output[3] = PID_Calc(&pid[3], target_speed.m4, chassis_motors[3].feedback.rotor_speed, 0, dt/1000000.0f);

}

void Chassis::send_control(){
    MOTOR_RM_Ctrl(&motors_param[0]);
}