//
// Created by lgv on 2025/8/15.
//
#ifdef MAX_ITER
#undef MAX_ITER
#endif

#include <opencv2/opencv.hpp>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include "devices/abstract_robot.h"
#include "device_manager/device_manager.h"

namespace py = pybind11;
using namespace cmvr::device;

class PyRobotWrapper {
public:
    // 构造时只需要传入 config_path 和 robot_name（只在第一次初始化有效）
    PyRobotWrapper(const std::string& config_path, const std::string& robot_name) {
        // 如果已经初始化过，则直接返回
        if (!robot_) {
            const XmlNode config(config_path);
            if (!config.hasChild("DeviceManager")) {
                throw std::runtime_error("Device Manager node not found");
            }
            auto dmgr_cfg = config.getChild("DeviceManager");
            dmgr_ = &DeviceManager::getInstance(dmgr_cfg);
            robot_ = dmgr_->getDevice<AbstractRobot>(robot_name);
        }
    }

    void moveJ(const std::string &side, const std::vector<double> &q) {
        if (q.size() != 7)
            throw std::runtime_error("Expected 7 joint values");

        std::vector<std::string> joint_names;
        if (side == "left") {
            joint_names = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
                           "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"};
        } else if (side == "right") {
            joint_names = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
                           "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
        } else {
            throw std::runtime_error("Side must be 'left' or 'right'");
        }

        std::vector<JointPositionCmd> cmd;
        for (size_t i = 0; i < 7; ++i)
            cmd.push_back({joint_names[i], q[i]});

        robot_->moveJ(cmd, 0.8);
    }


    void torqueOn() {
        if (!robot_)
            throw std::runtime_error("Robot not initialized");
        robot_->torqueOn();
    }

    void torqueOff() {
        if (!robot_)
            throw std::runtime_error("Robot not initialized");
        robot_->torqueOff();
    }

private:
    static DeviceManager* dmgr_;
    static std::shared_ptr<AbstractRobot> robot_;
};

// 静态成员初始化
DeviceManager* PyRobotWrapper::dmgr_ = nullptr;
std::shared_ptr<AbstractRobot> PyRobotWrapper::robot_ = nullptr;

PYBIND11_MODULE(robot_wrapper, m) {
    py::class_<PyRobotWrapper>(m, "Robot")
        .def(py::init<const std::string&, const std::string&>())
        .def("moveJ", &PyRobotWrapper::moveJ, py::arg("side"), py::arg("q"))
        .def("torqueOn", &PyRobotWrapper::torqueOn)   // 直接绑定 wrapper 的方法
        .def("torqueOff", &PyRobotWrapper::torqueOff);
}