#include "module/arm.hpp"
#include "bsp/time.h"

Arm::Arm(){
    dm_motor_params[0] = MOTOR_DM_Param_t{
        .can = BSP_CAN_1,
        .master_id = 0x11,
        .can_id = 1,
        .module = MOTOR_DM_J4310,
        .reverse = false
    };
    dm_motor_params[1] = MOTOR_DM_Param_t{
        .can = BSP_CAN_1,
        .master_id = 0x12,
        .can_id = 1,
        .module = MOTOR_DM_J4310,
        .reverse = false
    };
    dm_motor_params[2] = MOTOR_DM_Param_t{
        .can = BSP_CAN_1,
        .master_id = 0x13,
        .can_id = 1,
        .module = MOTOR_DM_J4310,
        .reverse = false
    };
    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
    };
}

void Arm::init(){
    MOTOR_LZ_Init();
    MOTOR_LZ_Register(&lz_motor_params[0]);
    MOTOR_LZ_Enable(&lz_motor_params[0]);
    BSP_TIME_Delay(2);
    MOTOR_LZ_Register(&lz_motor_params[1]);
    MOTOR_LZ_Enable(&lz_motor_params[1]);
    BSP_TIME_Delay(2);
    MOTOR_LZ_Register(&lz_motor_params[2]);
    MOTOR_LZ_Enable(&lz_motor_params[2]);
    BSP_TIME_Delay(2);

    MOTOR_DM_Register(&dm_motor_params[0]);
    MOTOR_DM_Enable(&dm_motor_params[0]);
    BSP_TIME_Delay(2);
    MOTOR_DM_Register(&dm_motor_params[1]);
    MOTOR_DM_Enable(&dm_motor_params[1]);
    BSP_TIME_Delay(2);
    MOTOR_DM_Register(&dm_motor_params[2]);
    MOTOR_DM_Enable(&dm_motor_params[2]);
    BSP_TIME_Delay(2);

}

void Arm::operator()(){
    MOTOR_DM_Relax(&dm_motor_params[0]);
    BSP_TIME_Delay(2);
    MOTOR_DM_Relax(&dm_motor_params[1]);
    BSP_TIME_Delay(2);
    MOTOR_DM_Relax(&dm_motor_params[2]);
    BSP_TIME_Delay(2);

    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;

    arm_output[0].angle = 1.0f;
    arm_output[0].velocity = 1.0f;
    arm_output[0].kp = 0.5f;
    arm_output[0].kd = 1.0f;
    // MOTOR_DM_MITCtrl(&dm_motor_params[0], &arm_output[0]);

    arm_state = MOTOR_LZ_Relax(&lz_motor_params[0]);
    BSP_TIME_Delay(2);
    MOTOR_LZ_Relax(&lz_motor_params[1]);
    BSP_TIME_Delay(2);
    MOTOR_LZ_Relax(&lz_motor_params[2]);
    BSP_TIME_Delay(2);

    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;
    // arm_lz_motion[0].target_angle = 1.0f;
    arm_lz_motion[0].target_velocity = 1.0f;
    // arm_lz_motion[0].kp = 0.5f;
    arm_lz_motion[0].kd = 0.5f;
    // MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_motion[0]);

}