/**
 * @file hand_pose.cpp
 * @brief Live finger-pose tuner for the Inspire hand (right hand). Needs ./inspire_g1.
 *
 *   ./hand_pose [networkInterface]      (default iface: eth0)
 *
 * Then type 6 numbers (q in [0,1], 0=closed 1=open):
 *     pinky ring middle index thumb_bend thumb_rot
 * e.g.  0 0 0 0 1 1     -> try a thumbs-up and tweak the last two values
 * Shortcuts:  open | close | q(uit)
 * The pose is applied to the RIGHT hand (left held open) and holds until the next entry.
 */
#include <unitree/idl/go2/MotorCmds_.hpp>
#include <unitree/robot/channel/channel_publisher.hpp>

#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <memory>
#include <unistd.h>

int main(int argc, char **argv)
{
    const std::string iface = argc > 1 ? argv[1] : "eth0";
    unitree::robot::ChannelFactory::Instance()->Init(0, iface);

    auto pub = std::make_shared<unitree::robot::ChannelPublisher<unitree_go::msg::dds_::MotorCmds_>>("rt/inspire/cmd");
    pub->InitChannel();
    unitree_go::msg::dds_::MotorCmds_ cmd;
    cmd.cmds().resize(12);
    usleep(300000);

    // right = indices 0-5, left = 6-11 (held open)
    auto apply = [&](const std::vector<float> &r) {
        for (int i = 0; i < 6; i++) { cmd.cmds()[i].q() = r[i]; cmd.cmds()[i + 6].q() = 1.0f; }
        for (int n = 0; n < 12; n++) { pub->Write(cmd); usleep(30000); } // ~0.36s of repeats so it lands + moves
    };

    std::cout << "Inspire RIGHT-hand pose tuner. Order: pinky ring middle index thumb_bend thumb_rot\n"
              << "q in [0,1], 0=closed 1=open. Type 6 numbers, or 'open'/'close'/'q'.\n";

    std::string line;
    while (true) {
        std::cout << "pose> " << std::flush;
        if (!std::getline(std::cin, line)) break;
        if (line == "q" || line == "quit") break;
        if (line == "open")  { apply({1, 1, 1, 1, 1, 1}); continue; }
        if (line == "close") { apply({0, 0, 0, 0, 0, 0}); continue; }

        std::istringstream iss(line);
        std::vector<float> v; float x;
        while (iss >> x) v.push_back(x);
        if (v.size() != 6) { std::cout << "  enter exactly 6 numbers (or open/close/q)\n"; continue; }
        apply(v);
        std::cout << "  applied: pinky=" << v[0] << " ring=" << v[1] << " middle=" << v[2]
                  << " index=" << v[3] << " thumb_bend=" << v[4] << " thumb_rot=" << v[5] << "\n";
    }
    std::cout << "bye.\n";
    return 0;
}