//
// Created by lgv on 2025/8/13.
//

#include <iostream>
#include <vector>
#include <string>
#include <cstdlib> // for std::atof

#include <glog/logging.h>
#include "../src/device_manager/include/device_manager.h"
#include <libgen.h>

using namespace cmvr::device;

int main(int argc, char* argv[]) {
    // 需要传入 side + 7 个关节值
    if (argc != 9) {
         LOG(ERROR) << "Usage: " << argv[0] << " <side:left|right> J1 J2 J3 J4 J5 J6 J7" << std::endl;
        return 1;
    }

    std::string side = argv[1];
    if (side != "left" && side != "right") {
         LOG(ERROR) << "side must be 'left' or 'right'" << std::endl;
        return 1;
    }

    // 输入的 7 个关节
    std::vector<double> arm_q(7);
    for (int i = 0; i < 7; ++i) {
        arm_q[i] = std::atof(argv[i + 2]);
    }

    // 左右臂关节名称
    std::vector<std::string> left_joint_names = {
        "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
        "L_ELBOW_R",    "L_WRIST_P",    "L_WRIST_Y",    "L_WRIST_R"
    };
    std::vector<std::string> right_joint_names = {
        "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
        "R_ELBOW_R",    "R_WRIST_P",    "R_WRIST_Y",    "R_WRIST_R"
    };

    std::vector<JointPositionCmd> cmd;
    cmd.reserve(7);

    if (side == "left") {
        for (size_t i = 0; i < 7; ++i)
            cmd.push_back({left_joint_names[i], arm_q[i]});
    } else {
        for (size_t i = 0; i < 7; ++i)
            cmd.push_back({right_joint_names[i], arm_q[i]});
    }

    // 获取 robot
    std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
    const XmlNode config(config_path);

    if (!config.hasChild("DeviceManager")) {
        LOG(ERROR) << "Device Manager node not found";
        return 1;
    }

    auto dmgr_cfg = config.getChild("DeviceManager");
    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);

    auto robot = dmgr.getDevice<AbstractRobot>("hc01");

    robot->moveJ(cmd, 0.8);

    return 0;
}