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


#include "device_manager/device_manager.h"
#include "gtest/gtest.h"
#include "controller/touch_controller.h"
#include "cmvr/msgs/geometry.pb.h"


using namespace cmvr::device;
using namespace cmvr::msgs;
using namespace cmvr::ctrl;

TEST(TouchControllerTest,MyTest) {
    //
    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";
    }
    auto dmgr_cfg = config.getChild("DeviceManager");
    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);

    auto robot = dmgr.getDevice<AbstractRobot>("hc01");
    auto hand = dmgr.getDevice<AbstractDexHand>("hand1");
    auto cam = dmgr.getDevice<AbstractCamera>("cam4");

    cmvr::msgs::Pose3d pose;

    pose.mutable_position()->set_x( 1.49969573e-01);
    pose.mutable_position()->set_y(-4.00100001e-01);
    pose.mutable_position()->set_z(-1.00102800e-01);

    pose.mutable_euler()->set_rx(0);
    pose.mutable_euler()->set_ry(0);
    pose.mutable_euler()->set_rz(1.57);

    robot->moveJ("PELVIS_S","R_WRIST_R_S",pose);
    // robot->seJ("PELVIS_S","R_WRIST_R_S",pose);
    // robot->servoJ("PELVIS_S","R_WRIST_R_S",pose,0.5);

    cmvr::msgs::Pose3d delta_pose;
    delta_pose.mutable_position()->set_x( 0.01);
    delta_pose.mutable_position()->set_y(0);
    delta_pose.mutable_position()->set_z(0);

    delta_pose.mutable_euler()->set_rx(0);
    delta_pose.mutable_euler()->set_ry(0);
    delta_pose.mutable_euler()->set_rz(0);


    std::this_thread::sleep_for(std::chrono::milliseconds(10000));
    robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.02);

    Pose3d offset;

    TouchController controller(robot, hand,cam);

    while (true) {
        // controller.touch(pose,offset,500);
        if (controller.isArrive(500)) {
            delta_pose.mutable_position()->set_x( -0.01);
            robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.05);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

}