#include "module/arm.hpp"
#include "component/pid.h"
#include "device/dr16.h"
#include "device/motor_rm.h"
#include "module/dr16.h"
#include "task/user_task.h"
#include "bsp/time.h"
#define STEP 0.002f
#define STEP_DE 0.002f

//限位定义
#define AXIS_1_MIN_ANGLE -6.0f
#define AXIS_1_MAX_ANGLE 6.0f
#define AXIS_2_MIN_ANGLE -4.0f
#define AXIS_2_MAX_ANGLE 4.0f
#define AXIS_3_MIN_ANGLE -2.0f
#define AXIS_3_MAX_ANGLE 2.0f
#define AXIS_4_MIN_ANGLE -1.0f
#define AXIS_4_MAX_ANGLE 1.0f
#define AXIS_5_MIN_ANGLE -0.5ff
#define AXIS_5_MAX_ANGLE 0.5f
#define AXIS_6_MIN_ANGLE -0.25f
#define AXIS_6_MAX_ANGLE 0.25f

#define DJI_MOTOR_OPEN 0.2f
#define DJI_MOTOR_CLOSE 1.37f

Arm::Arm(){
    //dm电机参数设置
    dm_motor_params[0] = MOTOR_DM_Param_t{
        .can = BSP_CAN_1,
        .master_id = 0x14,
        .can_id = 0x04,
        .module = MOTOR_DM_J4310,
        .reverse = false
    };
    dm_motor_params[1] = MOTOR_DM_Param_t{
        .can = BSP_CAN_1,
        .master_id = 0x15,
        .can_id = 0x05,
        .module = MOTOR_DM_J4310,
        .reverse = false
    };
    dm_motor_params[2] = MOTOR_DM_Param_t{
        .can = BSP_CAN_1,
        .master_id = 0x16,
        .can_id = 0x06,
        .module = MOTOR_DM_J4310,
        .reverse = false
    };
    //lz电机参数设置
    lz_motor_params[0] = MOTOR_LZ_Param_t{
        .can = BSP_CAN_1,
        .motor_id = 127,
        .host_id = 0xff,
        .module= MOTOR_LZ_RSO3,
        .reverse = false,
        .mode = MOTOR_LZ_MODE_MOTION
    };
    lz_motor_params[1] = MOTOR_LZ_Param_t{
        .can = BSP_CAN_1,
        .motor_id = 126,
        .host_id = 0xff,
        .module= MOTOR_LZ_RSO3,
        .reverse = false,
        .mode = MOTOR_LZ_MODE_MOTION
    };
    lz_motor_params[2] = MOTOR_LZ_Param_t{
        .can = BSP_CAN_1,
        .motor_id = 125,
        .host_id = 0xff,
        .module= MOTOR_LZ_RSO3,
        .reverse = false,
        .mode = MOTOR_LZ_MODE_MOTION
    };
    motor_dji_param = {
        .can = BSP_CAN_1,
        .id = 0x201,
        .module = MOTOR_M3508,
        .reverse = false,
        .gear = true
    };
    //dm电机控制参数初始化
    for (int i = 0; i < 3; i++) {
        arm_dm_output[i].velocity = 0;
        arm_dm_output[i].kp = 0;
        arm_dm_output[i].kd = 0.8;
        arm_dm_output[i].angle = 0;
        arm_dm_output[i].torque = 0;
    }
    //lz电机控制参数初始化
    for (int i = 0; i < 3; i++) {
        arm_lz_output[i].target_velocity = 0;
        arm_lz_output[i].kp = 0;
        arm_lz_output[i].kd = 0;
        arm_lz_output[i].target_angle = 0;
        arm_lz_output[i].torque = 0;
    }

    pid_params[0] = KPID_Params_t{
        .k=13,
        .p=1.5,
        .i=0,
        .d=0,
        .i_limit=0,
        .out_limit=100,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_params[1] = KPID_Params_t{
        .k=13,
        .p=1.5,
        .i=0,
        .d=0,
        .i_limit=0,
        .out_limit=100,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_params[2] = KPID_Params_t{
        .k=13,
        .p=1.5,
        .i=0,
        .d=0,
        .i_limit=10,
        .out_limit=20,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_params[3] = KPID_Params_t{
        .k=4.5,
        .p=1,
        .i=0,
        .d=0,
        .i_limit=10,
        .out_limit=20,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_params[4] = KPID_Params_t{
        .k=5.5,
        .p=1,
        .i=0,
        .d=0,
        .i_limit=10,
        .out_limit=10,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_params[5] = KPID_Params_t{
        .k=5,
        .p=1,
        .i=0,
        .d=0,
        .i_limit=10,
        .out_limit=20,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_v_params[0] = KPID_Params_t{
        .k=8,
        .p=1,
        .i=0,
        .d=0,
        .i_limit=10,
        .out_limit=20,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_v_params[1] = KPID_Params_t{
        .k=8,
        .p=1,
        .i=0,
        .d=0,
        .i_limit=10,
        .out_limit=20,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_v_params[2] = KPID_Params_t{
        .k=8,
        .p=1,
        .i=0,
        .d=0,
        .i_limit=10,
        .out_limit=20,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_v_params[3] = KPID_Params_t{
        .k=2,
        .p=0.5,
        .i=0,
        .d=0,
        .i_limit=10,
        .out_limit=12,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_v_params[4] = KPID_Params_t{
        .k=2.5,
        .p=0.5,
        .i=0,
        .d=0,
        .i_limit=10,
        .out_limit=12,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_v_params[5] = KPID_Params_t{
        .k=2.5,
        .p=0.5,
        .i=0,
        .d=0,
        .i_limit=10,
        .out_limit=12,
        .d_cutoff_freq=100,
        .range = -1,
    };
    pid_params[6] = KPID_Params_t{
        .k=1,
        .p=3,
        .i=0,
        .d=0,
        .i_limit=2,
        .out_limit=50,
        .d_cutoff_freq=-1.0f,
        .range = -1,
    };
    pid_v_params[6] = KPID_Params_t{
        .k=0.2,
        .p=0.15,
        .i=0,
        .d=0,
        .i_limit=0.5,
        .out_limit=1,
        .d_cutoff_freq=-1,
        .range = -1,
    };
}

void Arm::init(){
    for(int i = 0; i < 7; i++){
        PID_Init(&pid[i], KPID_MODE_NO_D, CTRL_ARM_FREQ, &pid_params[i]);
        PID_Init(&pid_v[i], KPID_MODE_NO_D, CTRL_ARM_FREQ, &pid_v_params[i]);
    }
    MOTOR_LZ_Init();
    for (int i = 0; i < 3; i++) {
        MOTOR_LZ_Register(&lz_motor_params[i]);
        MOTOR_LZ_Enable(&lz_motor_params[i]);
    }

    for (int i = 0; i < 3; i++) {
        MOTOR_DM_Register(&dm_motor_params[i]);
        MOTOR_DM_Enable(&dm_motor_params[i]);
    }

    MOTOR_RM_Register(&motor_dji_param);
    
    BSP_TIME_Delay(1000); //等待电机上线
    read_data();
    joint_angle[0] = arm_lz_motor[0].lz_feedback.current_angle;
    joint_angle[1] = arm_lz_motor[1].lz_feedback.current_angle;
    joint_angle[2] = arm_lz_motor[2].lz_feedback.current_angle;
    joint_angle[3] = arm_dm_motor[0].motor.feedback.rotor_abs_angle;
    joint_angle[4] = arm_dm_motor[1].motor.feedback.rotor_abs_angle;
    joint_angle[5] = arm_dm_motor[2].motor.feedback.rotor_abs_angle;
    arm_relax();
    BSP_TIME_Delay(1000); //等待电机上线
    read_data();
    joint_angle[0] = arm_lz_motor[0].lz_feedback.current_angle;
    joint_angle[1] = arm_lz_motor[1].lz_feedback.current_angle;
    joint_angle[2] = arm_lz_motor[2].lz_feedback.current_angle;
    joint_angle[3] = arm_dm_motor[0].motor.feedback.rotor_abs_angle;
    joint_angle[4] = arm_dm_motor[1].motor.feedback.rotor_abs_angle;
    joint_angle[5] = arm_dm_motor[2].motor.feedback.rotor_abs_angle;
    arm_relax();
    last_time = BSP_TIME_Get_us();
    last_sw_l = dr16.data.sw_l;
    // MOTOR_LZ_SetZero(&lz_motor_params[2]);
    // BSP_TIME_Delay(2);
    // MOTOR_LZ_SetZero(&lz_motor_params[1]);
    // BSP_TIME_Delay(2);

}

void Arm::operator()(){
    last_time = time;
    time = BSP_TIME_Get_us();
    dt = time - last_time;
    // arm_relax();
    //读取反馈数据
    read_data();
    //根据遥控器计算关节角度
    calc_motor_angle();
    //根据关节角度计算电机角度
    // calc_motor_angle();
    calc_pid();
    //发送控制命令
    if(dr16.data.sw_l == DR16_SW_DOWN){
    // send_control();
        arm_relax();
    }else {
        send_control();
    }
    if(last_sw_l != dr16.data.sw_l && last_sw_l != DR16_SW_UP){
        arm_enable();
    }
    last_sw_l = dr16.data.sw_l;
    
}

void Arm::arm_relax(){
    MOTOR_DM_Relax(&dm_motor_params[0]);
    MOTOR_DM_Relax(&dm_motor_params[1]);
    MOTOR_DM_Relax(&dm_motor_params[2]);
    MOTOR_LZ_Relax(&lz_motor_params[0]);
    MOTOR_LZ_Relax(&lz_motor_params[1]);        
    MOTOR_LZ_Relax(&lz_motor_params[2]);
    MOTOR_RM_Relax(&motor_dji_param);
    MOTOR_RM_Ctrl(&motor_dji_param);
}

void Arm::arm_enable(){
    MOTOR_DM_Enable(&dm_motor_params[0]);
    MOTOR_DM_Enable(&dm_motor_params[1]);
    MOTOR_DM_Enable(&dm_motor_params[2]);
    MOTOR_LZ_Enable(&lz_motor_params[0]);
    MOTOR_LZ_Enable(&lz_motor_params[1]);        
    MOTOR_LZ_Enable(&lz_motor_params[2]);
    BSP_TIME_Delay(2);
    read_data();
    joint_angle[0] = arm_lz_motor[0].lz_feedback.current_angle;
    joint_angle[1] = arm_lz_motor[1].lz_feedback.current_angle;
    joint_angle[2] = arm_lz_motor[2].lz_feedback.current_angle;
    joint_angle[3] = arm_dm_motor[0].motor.feedback.rotor_abs_angle;
    joint_angle[4] = arm_dm_motor[1].motor.feedback.rotor_abs_angle;
    joint_angle[5] = arm_dm_motor[2].motor.feedback.rotor_abs_angle;
    arm_relax();
}

void Arm::read_data(){
   //读取dm数据
    MOTOR_DM_UpdateAll();

    MOTOR_DM_t *motor_dm = NULL;
    motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[0]);
    arm_dm_motor[0] = *motor_dm;
    motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[1]);
    arm_dm_motor[1] = *motor_dm;
    motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[2]);
    arm_dm_motor[2] = *motor_dm;
    //读取lz数据
    MOTOR_LZ_UpdateAll();

    MOTOR_LZ_t *motor_lz = NULL;
    motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[0]);
    arm_lz_motor[0] = *motor_lz;
    motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[1]);
    arm_lz_motor[1] = *motor_lz;
    motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[2]);
    arm_lz_motor[2] = *motor_lz;

    MOTOR_RM_UpdateAll();

    MOTOR_RM_t *motor_dji = NULL;
    motor_dji = MOTOR_RM_GetMotor(&motor_dji_param);
    arm_motor_dji = *motor_dji;

    current_pos[0] = arm_lz_motor[0].lz_feedback.current_angle;
    current_pos[1] = arm_lz_motor[1].lz_feedback.current_angle;
    current_pos[2] = arm_lz_motor[2].lz_feedback.current_angle;
    current_pos[3] = arm_dm_motor[0].motor.feedback.rotor_abs_angle;
    current_pos[4] = arm_dm_motor[1].motor.feedback.rotor_abs_angle;
    current_pos[5] = arm_dm_motor[2].motor.feedback.rotor_abs_angle;
    current_vel[0] = arm_lz_motor[0].lz_feedback.current_velocity;
    current_vel[1] = arm_lz_motor[1].lz_feedback.current_velocity;
    current_vel[2] = arm_lz_motor[2].lz_feedback.current_velocity;
    current_vel[3] = arm_dm_motor[0].motor.feedback.rotor_speed;
    current_vel[4] = arm_dm_motor[1].motor.feedback.rotor_speed;
    current_vel[5] = arm_dm_motor[2].motor.feedback.rotor_speed;
    current_pos[6] = arm_motor_dji.feedback.rotor_abs_angle;
    current_vel[6] = arm_motor_dji.feedback.rotor_speed;
}

void Arm::calc_joint_angle(){
    
}

void Arm::calc_motor_angle(){
    if(dr16.data.sw_r == DR16_SW_MID){
        if(dr16.data.ch_r_x > 0.4f){
            joint_angle[0] = joint_angle[0] + dr16.data.ch_r_x * STEP;
        }else if(dr16.data.ch_r_x < -0.4f){
            joint_angle[0] = joint_angle[0] + dr16.data.ch_r_x * STEP_DE;
        }else{
            joint_angle[0] = joint_angle[0];  // 保持当前角度
        }
        
        if(dr16.data.ch_r_y > 0.4f){
            joint_angle[1] = joint_angle[1] - dr16.data.ch_r_y * STEP_DE;
            joint_angle[2] = joint_angle[2] + dr16.data.ch_r_y * STEP;
        }else if(dr16.data.ch_r_y < -0.4f){
            joint_angle[1] = joint_angle[1] - dr16.data.ch_r_y * STEP;
            joint_angle[2] = joint_angle[2] + dr16.data.ch_r_y * STEP_DE;
        }else{
            joint_angle[1] = joint_angle[1];  // 保持当前角度
            joint_angle[2] = joint_angle[2];  // 保持当前角度
        }
        
        if(dr16.data.ch_l_y > 0.4f){
            joint_angle[2] = joint_angle[2] - dr16.data.ch_l_y * STEP_DE;
        }else if(dr16.data.ch_l_y < -0.4f){
            joint_angle[2] = joint_angle[2] - dr16.data.ch_l_y * STEP;
        }else{
            joint_angle[2] = joint_angle[2];
        }
        
        if(dr16.data.ch_l_x > 0.4f){
            joint_angle[3] = joint_angle[3] + dr16.data.ch_l_x * STEP;
        }else if(dr16.data.ch_l_x < -0.4f){
            joint_angle[3] = joint_angle[3] + dr16.data.ch_l_x * STEP_DE;
        }else{
            joint_angle[3] = joint_angle[3];  // 保持当前角度
        }
        joint_angle[4] = joint_angle[4];  // 保持当前角度
        joint_angle[5] = joint_angle[5];  // 保持当前角度
    
    }else if(dr16.data.sw_r == DR16_SW_UP){
        if(dr16.data.ch_r_y > 0.4f){
            joint_angle[4] = joint_angle[4] + dr16.data.ch_r_y * STEP;
        }else if(dr16.data.ch_r_y < -0.4f){
            joint_angle[4] = joint_angle[4] + dr16.data.ch_r_y * STEP_DE;
        }else{
            joint_angle[4] = joint_angle[4];  // 保持当前角度
        }
        
        if(dr16.data.ch_r_x > 0.4f){
            joint_angle[5] = joint_angle[5] + dr16.data.ch_r_x * STEP;
        }else if(dr16.data.ch_r_x < -0.4f){
            joint_angle[5] = joint_angle[5] + dr16.data.ch_r_x * STEP_DE;
        }else{
            joint_angle[5] = joint_angle[5];  // 保持当前角度
        }
        joint_angle[0] = joint_angle[0];  // 保持当前角度
        joint_angle[1] = joint_angle[1];  // 保持当前角度
        joint_angle[2] = joint_angle[2];  // 保持当前角度
        joint_angle[3] = joint_angle[3];  // 保持当前角度
    }else {
        joint_angle[0] = joint_angle[0];  // 保持当前角度
        joint_angle[1] = joint_angle[1];  // 保持当前角度
        joint_angle[2] = joint_angle[2];  // 保持当前角度
        joint_angle[3] = joint_angle[3];  // 保持当前角度
        joint_angle[4] = joint_angle[4];  // 保持当前角度
        joint_angle[5] = joint_angle[5];  // 保持当前角度
    }
    if(dr16.data.sw_l == DR16_SW_UP){
        joint_angle[6] = DJI_MOTOR_CLOSE;
    }else {
        joint_angle[6] = DJI_MOTOR_OPEN;
    }
    
}

void Arm::calc_pid(){
    float delta = 0;
    for(int i = 0; i < 6; i++){
        delta = PID_Calc(&pid[i], joint_angle[i], current_pos[i],current_vel[i], dt/1000000.0f);
        output[i] = PID_Calc(&pid_v[i], delta, current_vel[i],0, dt/1000000.0f);
    }
    
    if(dr16.data.sw_l == DR16_SW_UP){
        output[6] = 0.2f;
    }else if(dr16.data.sw_l == DR16_SW_MID){
        output[6] = -0.2f;
    }else {
        output[6] = 0.0f;
    }
    arm_lz_output[0].torque = output[0];
    arm_lz_output[1].torque = output[1];
    arm_lz_output[2].torque = output[2];
    arm_dm_output[0].torque = output[3];
    arm_dm_output[1].torque = output[4];
    arm_dm_output[2].torque = output[5];
    MOTOR_RM_SetOutput(&motor_dji_param, output[6]);
}

void Arm::send_control(){
    
    MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_output[0]);
    MOTOR_LZ_MotionControl(&lz_motor_params[1], &arm_lz_output[1]);
    MOTOR_LZ_MotionControl(&lz_motor_params[2], &arm_lz_output[2]);
    MOTOR_DM_MITCtrl(&dm_motor_params[0], &arm_dm_output[0]);
    MOTOR_DM_MITCtrl(&dm_motor_params[1], &arm_dm_output[1]);
    MOTOR_DM_MITCtrl(&dm_motor_params[2], &arm_dm_output[2]);
    MOTOR_RM_SetOutput(&motor_dji_param, output[6]);
    MOTOR_RM_Ctrl(&motor_dji_param);
    
}
