#include "module/arm.hpp"
#include "component/pid.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

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

void Arm::init(){
    for(int i = 0; i < 6; 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]);
    }
    
    BSP_TIME_Delay(1000); //等待电机上线
    read_adta();
    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_adta();
    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();
    // 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_adta();
    //根据遥控器计算关节角度
    calc_motor_angle();
    //根据关节角度计算电机角度
    // calc_motor_angle();
    calc_pid();
    //发送控制命令
    send_control();
    // arm_relax();
    // //控制数据
    // for (int i = 1; i < 3; i++) {
    //     arm_dm_output[i].angle = arm_dm_motor[i].motor.feedback.rotor_abs_angle + STEP;
    //     MOTOR_DM_MITCtrl(&dm_motor_params[i], &arm_dm_output[i]);
    // }

    // arm_lz_output[0].target_angle = arm_lz_motor[0].motor.feedback.rotor_abs_angle + STEP;
    // MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_output[0]);
}

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

void Arm::read_adta(){
   //读取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;

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

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];  // 保持当前角度
    }
}

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

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]);
    
    // if(dr16.data.sw_r == DR16_SW_MID){
    //     if(dr16.data.ch_r_x >0.4f || dr16.data.ch_r_x < -0.4f){
    //         MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_output[0]);

    //     }if(dr16.data.ch_r_y >0.4f || dr16.data.ch_r_y < -0.4f){
    //         MOTOR_LZ_MotionControl(&lz_motor_params[1], &arm_lz_output[1]);

    //     }if(dr16.data.ch_l_y > 0.4f || dr16.data.ch_l_y < -0.4f || dr16.data.ch_r_y > 0.4f || dr16.data.ch_r_y < -0.4f){
    //         MOTOR_LZ_MotionControl(&lz_motor_params[2], &arm_lz_output[2]);

    //     }if(dr16.data.ch_l_x >0.4f || dr16.data.ch_l_x < -0.4f){
            
    //         MOTOR_DM_MITCtrl(&dm_motor_params[0], &arm_dm_output[0]);
    //     }
    // }else if(dr16.data.sw_r == DR16_SW_UP){
    //     if(dr16.data.ch_r_y >0.4f || dr16.data.ch_r_y < -0.4f){
            
    //         MOTOR_DM_MITCtrl(&dm_motor_params[1], &arm_dm_output[1]);

    //     }if(dr16.data.ch_r_x >0.4f || dr16.data.ch_r_x < -0.4f){
               
    //         MOTOR_DM_MITCtrl(&dm_motor_params[2], &arm_dm_output[2]);

    //     }
    // }
}