#include "arm_driver.hpp"
#include "rm_arm_driver.hpp"
#include <cstring>

RmArmDriver::RmArmDriver()
{
    rmService_ = nullptr;
    initFlag_ = RM_DRIVER_UNINIT;
}

RmArmDriver::~RmArmDriver()
{
    if (rmService_ != nullptr) {
        rmService_->rm_destroy();
        delete rmService_;
    }
}

RmArmDriver::RmArmStatusCallbackFun RmArmDriver::armStatusCallback_ = nullptr;
RmArmDriver::ApiLogFunc RmArmDriver::customApiLog_ = nullptr;
rm_robot_handle *RmArmDriver::leftRobotHandle_;
rm_robot_handle *RmArmDriver::rightRobotHandle_;
RM_Service *RmArmDriver::rmService_ = nullptr;

int RmArmDriver::ArmMove(const float points[USED_ARM_DOF], int armId)
{
    float joints[ARM_DOF] = {0};
    for (size_t i = 0; i < USED_ARM_DOF; i++) {
        joints[i] = points[i];
    }
    if (armId == LEFT_ARM) {
        int result = rmService_->rm_movej_canfd(leftRobotHandle_, joints, true, 0);
        if (result != 0) {
            api_log("Error moving left arm to joint space!");
            return UNKNOWN_ERR;
        }
    } else if (armId == RIGHT_ARM) {
        int result = rmService_->rm_movej_canfd(rightRobotHandle_, joints, true, 0);
        if (result != 0) {
            api_log("Error moving right arm to joint space!");
            return UNKNOWN_ERR;
        }
    } else {
        api_log("Invalid armId: %d", armId);
        return UNKNOWN_ERR;
    }
    return OK;
}

void RmArmDriver::CallbackRmRealtimeArmJointState(rm_realtime_arm_joint_state_t data) 
{
    ArmStatusCallbackData callbackData;
    callbackData.errCode = 0;
    if (armStatusCallback_ == nullptr) {
        return;
    }
    if (strcmp(data.arm_ip, LEFT_ARM_IP_ADDRESS) == 0) {
        callbackData.armId = LEFT_ARM;
    }
    else if (strcmp(data.arm_ip, RIGHT_ARM_IP_ADDRESS) == 0) {
        callbackData.armId = RIGHT_ARM;
    } else {
        api_log("Unknown arm ip: %s", data.arm_ip);
        return;
    }

    callbackData.errCode = data.errCode;
    for (size_t i = 0; i < USED_ARM_DOF; i++) {
        callbackData.jointCurrent[i] = data.joint_status.joint_current[i];
        callbackData.jointEnFlag[i] = data.joint_status.joint_en_flag[i];
        callbackData.jointAngle[i] = data.joint_status.joint_position[i];
        callbackData.jointSpeed[i] = data.joint_status.joint_speed[i];
        callbackData.jointTemp[i] = data.joint_status.joint_temperature[i];
        callbackData.jointVoltage[i] = data.joint_status.joint_voltage[i];
        callbackData.jointTorque[i] = data.force_sensor.force[i];
        callbackData.jointErrCode[i] = data.joint_status.joint_err_code[i];
    }

    armStatusCallback_(&callbackData);
}

void RmArmDriver::api_log(const char* format, ...)
{
    va_list args;
    va_start(args, format);// 初始化可变参数列表
    customApiLog_(format, args);// 调用函数指针，传递 va_list
    va_end(args);// 清理可变参数列表
}

int RmArmDriver::InitHandle(rm_robot_handle **robotHandle_, const char *robot_ip_address, int robot_port, rm_realtime_push_config_t *config)
{
    *robotHandle_ = rmService_->rm_create_robot_arm(robot_ip_address, robot_port);
    if (*robotHandle_ == NULL) {
        api_log("Failed to create arm(%s) handle.", robot_ip_address);
        return UNKNOWN_ERR;
    }
    if ((*robotHandle_)->id <= 0) {
        api_log("Failed to create arm(%s) handle. %d", robot_ip_address, (*robotHandle_)->id);
        return UNKNOWN_ERR;
    }

    int result = rmService_-> rm_set_realtime_push(*robotHandle_, *config);
    if (result != 0) {
        api_log("Failed to set realtime push for arm(%s).", robot_ip_address);
        return UNKNOWN_ERR;
    }
    result = rm_set_arm_max_line_speed(*robotHandle_, 0.01f);
    if (result != 0) {
        api_log("Failed to set max line speed for arm(%s).", robot_ip_address);
        return UNKNOWN_ERR;
    }

    rm_robot_info_t robot_info;
    int info_result = rmService_->rm_get_robot_info(*robotHandle_, &robot_info);
    if (info_result != 0) {
        api_log("Failed to get robot info(%s)", robot_ip_address);
        return UNKNOWN_ERR;
    }
    int dof = robot_info.arm_dof;
    if (dof != USED_ARM_DOF) {
        api_log("Invalid degree of freedom(%s)", robot_ip_address);
        return UNKNOWN_ERR;
    }
    return 0;
}

int RmArmDriver::Init()
{
    if (initFlag_ == RM_DRIVER_INIT_OK) {
        return OK;
    }
    if (customApiLog_ == nullptr || armStatusCallback_ == nullptr) {
        return UNKNOWN_ERR;
    }
    rmService_ = new RM_Service();
    if (rmService_ == nullptr) {
        return UNKNOWN_ERR;
    }
    int result = rmService_->rm_init(RM_TRIPLE_MODE_E);
    if (result != 0) {
        rmService_->rm_destroy();
        delete rmService_;
        return UNKNOWN_ERR;
    }

    rmService_->rm_set_log_call_back(customApiLog_, 3);
    rmService_->rm_realtime_arm_state_call_back(CallbackRmRealtimeArmJointState);

    int robot_port = ARM_IP_PORT;
    rm_realtime_push_config_t config = {20, true, 8089, 0, "192.168.2.3"};
    result = InitHandle(&leftRobotHandle_, LEFT_ARM_IP_ADDRESS, robot_port, &config);
    if (result != 0) {
        rmService_->rm_destroy();
        delete rmService_;
        return UNKNOWN_ERR;
    }
    config = {20, true, 8089, 0, "192.168.2.19"};
    result = InitHandle(&rightRobotHandle_, RIGHT_ARM_IP_ADDRESS, robot_port, &config);
    if (result != 0) {
        rmService_->rm_destroy();
        delete rmService_;
        return UNKNOWN_ERR;
    }
    
    initFlag_ = RM_DRIVER_INIT_OK;
    return OK;
}