#pragma once

#include "rm_control/interfaces/robot_arm.h"

#include "rm_interface.h"

namespace rm_control::drivers::realman {

// 睿尔曼 RM_API2 的具体驱动实现。
// 这是 RM 子工程中唯一直接依赖 rm_interface.h 的位置之一；
// core/config/interfaces 都不应该 include 睿尔曼 SDK 头文件。
class RealManRobotArm final : public interfaces::IRobotArm {
public:
    RealManRobotArm() = default;
    RealManRobotArm(const RealManRobotArm &) = delete;
    RealManRobotArm &operator=(const RealManRobotArm &) = delete;
    ~RealManRobotArm() override;

    common::Result<void> connect(const common::RobotConnectionConfig &config) override;
    void disconnect() override;

    common::Result<common::RobotInfo> getRobotInfo() override;
    common::Result<common::ArmState> getCurrentArmState() override;
    common::Result<common::ForceData> getForceData() override;

    common::Result<void> moveJ(const std::vector<float> &joints_deg,
                               int speed_percent,
                               int blend_radius,
                               int trajectory_connect,
                               bool blocking) override;

    common::Result<void> moveL(const common::Pose &pose,
                               int speed_percent,
                               int blend_radius,
                               int trajectory_connect,
                               bool blocking) override;

    common::Result<void> startDragTeach(const common::DragTeachStartOptions &options) override;
    common::Result<void> stopDragTeach() override;
    common::Result<common::SavedVendorTrajectory>
    saveDragTeachTrajectory(const std::string &path) override;

private:
    // 所有 SDK 读写前先检查连接状态，防止空句柄传入 C SDK。
    common::Result<void> ensureConnected() const;

    // RM_API2 使用裸指针句柄，因此必须由本类析构函数集中释放。
    rm_robot_handle *handle_ = nullptr;
    bool initialized_ = false;
    int dof_ = 0;
};

} // namespace rm_control::drivers::realman
