#include "rm_control/drivers/realman/realman_robot_arm.h"

#include <cstdarg>
#include <cstdio>
#include <string>

namespace rm_control::drivers::realman {
namespace {

void apiLogCallback(const char *message, va_list args) {
    // RM_API2 的日志回调是 C 风格 va_list，这里统一转到 stderr。
    if (message == nullptr) {
        return;
    }

    char buffer[1024] = {};
    std::vsnprintf(buffer, sizeof(buffer), message, args);
    std::fprintf(stderr, "[rm_api] %s\n", buffer);
}

int destroyApi() {
// 兼容不同版本 RM_API2：有些头文件拼写为 rm_destroy，有些为 rm_destory。
#if defined(REALMAN_RM_API_HAS_RM_DESTROY)
    return rm_destroy();
#else
    return rm_destory();
#endif
}

common::Pose convertPose(const rm_pose_t &pose) {
    // 只把上层需要的位置和欧拉角带出去，四元数留在驱动内部。
    common::Pose result;
    result.x = pose.position.x;
    result.y = pose.position.y;
    result.z = pose.position.z;
    result.rx = pose.euler.rx;
    result.ry = pose.euler.ry;
    result.rz = pose.euler.rz;
    return result;
}

rm_pose_t toRmPose(const common::Pose &pose) {
    // RM_API2 的 rm_pose_t 同时包含四元数和欧拉角。
    // 当前 JSON 使用欧拉角表达，四元数给单位值作为安全默认。
    rm_pose_t result = {};
    result.position.x = pose.x;
    result.position.y = pose.y;
    result.position.z = pose.z;
    result.euler.rx = pose.rx;
    result.euler.ry = pose.ry;
    result.euler.rz = pose.rz;
    result.quaternion.w = 1.0F;
    return result;
}

} // namespace

RealManRobotArm::~RealManRobotArm() {
    // RAII：无论调用方是否显式 disconnect，析构时都释放 SDK 资源。
    disconnect();
}

common::Result<void> RealManRobotArm::connect(const common::RobotConnectionConfig &config) {
    // 先断开旧连接，保证重复 connect 不会泄漏旧句柄。
    disconnect();

    rm_set_log_call_back(apiLogCallback, 2);
    rm_set_timeout(config.timeout_ms);

    const int init_result = rm_init(RM_TRIPLE_MODE_E);
    initialized_ = init_result == 0;
    if (!initialized_) {
        return common::Result<void>::failure("rm_init failed, error=" + std::to_string(init_result));
    }

    handle_ = rm_create_robot_arm(config.ip.c_str(), config.port);
    if (handle_ == nullptr || handle_->id < 0) {
        disconnect();
        return common::Result<void>::failure("rm_create_robot_arm failed: " + config.ip + ":" +
                                            std::to_string(config.port));
    }

    auto info = getRobotInfo();
    if (!info.ok) {
        disconnect();
        return common::Result<void>::failure(info.error_message);
    }
    dof_ = info.value.arm_dof;
    // dof_ 后续用于校验 moveJ 目标长度，避免给 7 轴机械臂传错数组长度。
    return common::Result<void>::success();
}

void RealManRobotArm::disconnect() {
    // RM_API2 要求先 delete robot arm，再 destroy API。
    if (handle_ != nullptr) {
        rm_delete_robot_arm(handle_);
        handle_ = nullptr;
    }
    if (initialized_) {
        destroyApi();
        initialized_ = false;
    }
    dof_ = 0;
}

common::Result<common::RobotInfo> RealManRobotArm::getRobotInfo() {
    // 把 SDK 原始枚举转成 int，避免公共头文件暴露 RM SDK 类型。
    auto connected = ensureConnected();
    if (!connected.ok) {
        return common::Result<common::RobotInfo>::failure(connected.error_message);
    }

    rm_robot_info_t info = {};
    const int result = rm_get_robot_info(handle_, &info);
    if (result != 0) {
        return common::Result<common::RobotInfo>::failure(
            "rm_get_robot_info failed, error=" + std::to_string(result));
    }

    common::RobotInfo output;
    output.arm_dof = static_cast<int>(info.arm_dof);
    output.arm_model = static_cast<int>(info.arm_model);
    output.force_type = static_cast<int>(info.force_type);
    output.controller_version = static_cast<int>(info.robot_controller_version);
    return common::Result<common::RobotInfo>::success(output);
}

common::Result<common::ArmState> RealManRobotArm::getCurrentArmState() {
    // 当前状态是所有相对运动和安全校验的基础数据。
    auto connected = ensureConnected();
    if (!connected.ok) {
        return common::Result<common::ArmState>::failure(connected.error_message);
    }

    rm_current_arm_state_t state = {};
    const int result = rm_get_current_arm_state(handle_, &state);
    if (result != 0) {
        return common::Result<common::ArmState>::failure(
            "rm_get_current_arm_state failed, error=" + std::to_string(result));
    }

    const int dof = dof_ > 0 ? dof_ : ARM_DOF;
    common::ArmState output;
    output.joints_deg.assign(state.joint, state.joint + dof);
    output.tcp_pose = convertPose(state.pose);
    for (int i = 0; i < static_cast<int>(state.err.err_len) && i < 24; ++i) {
        output.error_codes.push_back(state.err.err[i]);
    }
    return common::Result<common::ArmState>::success(output);
}

common::Result<common::ForceData> RealManRobotArm::getForceData() {
    // 力数据当前只读不控制，主要用于状态观察和后续力控模块。
    auto connected = ensureConnected();
    if (!connected.ok) {
        return common::Result<common::ForceData>::failure(connected.error_message);
    }

    rm_force_data_t data = {};
    const int result = rm_get_force_data(handle_, &data);
    if (result != 0) {
        return common::Result<common::ForceData>::failure(
            "rm_get_force_data failed, error=" + std::to_string(result));
    }

    common::ForceData output;
    for (size_t i = 0; i < output.raw.size(); ++i) {
        output.raw[i] = data.force_data[i];
        output.zero[i] = data.zero_force_data[i];
        output.work_zero[i] = data.work_zero_force_data[i];
        output.tool_zero[i] = data.tool_zero_force_data[i];
    }
    return common::Result<common::ForceData>::success(output);
}

common::Result<void> RealManRobotArm::moveJ(const std::vector<float> &joints_deg,
                                            int speed_percent,
                                            int blend_radius,
                                            int trajectory_connect,
                                            bool blocking) {
    // IRobotArm 接口使用 vector，但 RM_API2 需要固定长度 C 数组。
    // 这里集中做长度校验和数组转换。
    auto connected = ensureConnected();
    if (!connected.ok) {
        return connected;
    }
    if (static_cast<int>(joints_deg.size()) != dof_) {
        return common::Result<void>::failure("moveJ target size must match robot DOF");
    }

    float target[ARM_DOF] = {};
    for (int i = 0; i < dof_; ++i) {
        target[i] = joints_deg[static_cast<size_t>(i)];
    }

    const int result = rm_movej(handle_, target, speed_percent, blend_radius,
                                trajectory_connect, blocking ? 1 : 0);
    if (result != 0) {
        return common::Result<void>::failure("rm_movej failed, error=" + std::to_string(result));
    }
    return common::Result<void>::success();
}

common::Result<void> RealManRobotArm::moveL(const common::Pose &pose,
                                            int speed_percent,
                                            int blend_radius,
                                            int trajectory_connect,
                                            bool blocking) {
    // 笛卡尔直线运动保持在驱动层，core 只负责安全限幅和流程编排。
    auto connected = ensureConnected();
    if (!connected.ok) {
        return connected;
    }

    const rm_pose_t target = toRmPose(pose);
    const int result = rm_movel(handle_, target, speed_percent, blend_radius,
                                trajectory_connect, blocking ? 1 : 0);
    if (result != 0) {
        return common::Result<void>::failure("rm_movel failed, error=" + std::to_string(result));
    }
    return common::Result<void>::success();
}

common::Result<void> RealManRobotArm::startDragTeach(const common::DragTeachStartOptions &options) {
    auto connected = ensureConnected();
    if (!connected.ok) {
        return connected;
    }

    if (options.mode == common::DragTeachMode::OrdinaryRecorded) {
        const int record_flag = options.record_vendor_trajectory ? 1 : 0;
        const int result = rm_start_drag_teach(handle_, record_flag);
        if (result != 0) {
            return common::Result<void>::failure(
                "rm_start_drag_teach failed, error=" + std::to_string(result));
        }
        return common::Result<void>::success();
    }

    // RM75 三代控制器支持复合拖动示教；mode=3 表示六维力位置和姿态同时可拖动。
    const int mode = 3;
    const int singular_wall = options.enable_singular_wall ? 1 : 0;
    const int result = rm_start_multi_drag_teach(handle_, mode, singular_wall);
    if (result != 0) {
        return common::Result<void>::failure(
            "rm_start_multi_drag_teach(mode=3) failed, error=" + std::to_string(result));
    }
    return common::Result<void>::success();
}

common::Result<void> RealManRobotArm::stopDragTeach() {
    auto connected = ensureConnected();
    if (!connected.ok) {
        return connected;
    }

    const int result = rm_stop_drag_teach(handle_);
    if (result != 0) {
        return common::Result<void>::failure("rm_stop_drag_teach failed, error=" + std::to_string(result));
    }
    return common::Result<void>::success();
}

common::Result<common::SavedVendorTrajectory>
RealManRobotArm::saveDragTeachTrajectory(const std::string &path) {
    auto connected = ensureConnected();
    if (!connected.ok) {
        return common::Result<common::SavedVendorTrajectory>::failure(connected.error_message);
    }
    if (path.empty()) {
        return common::Result<common::SavedVendorTrajectory>::failure("vendor trajectory path must not be empty");
    }
    if (path.size() > 299) {
        return common::Result<common::SavedVendorTrajectory>::failure(
            "vendor trajectory path must be shorter than 300 characters for rm_save_trajectory");
    }

    int point_count = 0;
    const int result = rm_save_trajectory(handle_, path.c_str(), &point_count);
    if (result != 0) {
        return common::Result<common::SavedVendorTrajectory>::failure(
            "rm_save_trajectory failed, error=" + std::to_string(result));
    }

    common::SavedVendorTrajectory output;
    output.path = path;
    output.point_count = point_count;
    return common::Result<common::SavedVendorTrajectory>::success(output);
}

common::Result<void> RealManRobotArm::ensureConnected() const {
    // 所有 SDK 调用前统一检查，避免空指针进入 C SDK。
    if (!initialized_ || handle_ == nullptr || handle_->id < 0) {
        return common::Result<void>::failure("RealMan robot is not connected");
    }
    return common::Result<void>::success();
}

} // namespace rm_control::drivers::realman
