/**
 * @file hand_gestures.cpp
 * @brief Named gestures for the Inspire RH56 hands on G1, over DDS (rt/inspire/cmd).
 *
 * Requires the service running in another terminal:  ./inspire_g1
 *
 * Usage:
 *   ./hand_gestures                      # interactive menu (pick gestures repeatedly)
 *   ./hand_gestures <gesture> [iface]    # run one gesture and exit
 *   gesture = open | close | box | thumbup | handshake | combo | demo
 *
 * Per-hand DOF order: [pinky, ring, middle, index, thumb_bend, thumb_rotation]
 * q in [0,1]:  0 = closed,  1 = open.  Indices 0-5 = right hand, 6-11 = left hand.
 *
 * NOTE: this controls the HAND (fingers) only. Raising the arm is a separate
 * subsystem (G1 arm joints) and is not driven here.
 */

#include <unitree/idl/go2/MotorCmds_.hpp>
#include <unitree/idl/go2/MotorStates_.hpp>
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <unitree/common/thread/thread.hpp>

#include <eigen3/Eigen/Dense>
#include <iostream>
#include <string>
#include <mutex>
#include <unistd.h>

using Vec6 = Eigen::Matrix<float, 6, 1>;

class HandController
{
public:
    HandController() { Init(); }

    void set(const Vec6 &right, const Vec6 &left)
    {
        for (size_t i = 0; i < 6; i++)
        {
            cmd.cmds()[i].q()     = right(i);
            cmd.cmds()[i + 6].q() = left(i);
        }
        handcmd->Write(cmd);
    }
    void setBoth(const Vec6 &q) { set(q, q); }

    unitree_go::msg::dds_::MotorCmds_   cmd;
    unitree_go::msg::dds_::MotorStates_ state;

private:
    void Init()
    {
        handcmd = std::make_shared<unitree::robot::ChannelPublisher<unitree_go::msg::dds_::MotorCmds_>>("rt/inspire/cmd");
        handcmd->InitChannel();
        cmd.cmds().resize(12);
        handstate = std::make_shared<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::MotorStates_>>("rt/inspire/state");
        handstate->InitChannel([this](const void *message) {
            std::lock_guard<std::mutex> lock(mtx);
            state = *(unitree_go::msg::dds_::MotorStates_ *)message;
        });
        state.states().resize(12);
    }

    std::mutex mtx;
    unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::MotorCmds_>  handcmd;
    unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::MotorStates_> handstate;
};

static Vec6 V(float pinky, float ring, float middle, float index, float thumb_bend, float thumb_rot)
{
    Vec6 v; v << pinky, ring, middle, index, thumb_bend, thumb_rot; return v;
}
static void hold(int ms) { usleep(ms * 1000); }

// --- Pose library (tune freely; values are [0,1], 0=closed 1=open) ---
static const Vec6 OPEN        = V(1, 1, 1, 1, 1, 1);
static const Vec6 CLOSE       = V(0, 0, 0, 0, 0, 0);
static const Vec6 BOX         = V(0.5, 0.5, 0.5, 0.5, 0.4, 0.1);
static const Vec6 THUMBUP     = V(0, 0, 0, 0, 1, 1);
static const Vec6 SHAKE_READY = V(0.85, 0.85, 0.85, 0.85, 0.85, 0.6);
static const Vec6 SHAKE_GRIP  = V(0.2, 0.2, 0.2, 0.2, 0.25, 0.1);

// --- Gestures (social ones use the RIGHT hand, left held open) ---
static void g_pose(HandController &h, const Vec6 &q, const char *name, int ms = 1500)
{
    std::cout << "  [" << name << "]" << std::endl;
    h.setBoth(q);
    hold(ms);
}
static void g_thumbup(HandController &h, int ms = 2000)
{
    std::cout << "  [thumbup] (right hand)" << std::endl;
    h.set(THUMBUP, OPEN);
    hold(ms);
}
static void g_handshake(HandController &h)
{
    std::cout << "  [handshake] (right hand)" << std::endl;
    h.set(SHAKE_READY, OPEN); hold(1200);          // reach out, open
    h.set(SHAKE_GRIP,  OPEN); hold(800);            // grip
    for (int k = 0; k < 3; k++)                     // a few firm "shakes"
    {
        h.set(SHAKE_GRIP * 0.85f, OPEN); hold(180);
        h.set(SHAKE_GRIP,          OPEN); hold(180);
    }
    h.set(THUMBUP, OPEN); hold(1500);              // finish with a thumbs-up
    h.set(OPEN,    OPEN); hold(600);               // relax
}
static void g_demo(HandController &h)
{
    g_pose(h, OPEN, "open");
    g_pose(h, CLOSE, "close");
    g_pose(h, BOX, "box");
    g_pose(h, OPEN, "open");
    g_thumbup(h);
    g_handshake(h);
    g_pose(h, OPEN, "open");
}

// Dispatch by name or menu number. Returns false only on quit.
static bool runGesture(HandController &h, const std::string &g)
{
    if      (g == "open"      || g == "1") g_pose(h, OPEN, "open");
    else if (g == "close"     || g == "2") g_pose(h, CLOSE, "close");
    else if (g == "box"       || g == "3") g_pose(h, BOX, "box");
    else if (g == "thumbup"   || g == "4") g_thumbup(h);
    else if (g == "handshake" || g == "5") g_handshake(h);
    else if (g == "combo"     || g == "6") { g_thumbup(h); g_handshake(h); }
    else if (g == "demo"      || g == "7") g_demo(h);
    else if (g == "q" || g == "quit" || g == "0") return false;
    else std::cout << "  unknown option: '" << g << "'" << std::endl;
    return true;
}

static void printMenu()
{
    std::cout << "\n=== Inspire Hand Gestures ===\n"
              << "  1) open\n"
              << "  2) close      (fist)\n"
              << "  3) box        (power grasp)\n"
              << "  4) thumbup    (right hand)\n"
              << "  5) handshake  (right hand, ends thumbs-up)\n"
              << "  6) combo      (thumbup + handshake)\n"
              << "  7) demo       (run all)\n"
              << "  q) quit\n"
              << "Select: " << std::flush;
}

static std::string trim(std::string s)
{
    const char *ws = " \t\r\n";
    size_t a = s.find_first_not_of(ws);
    size_t b = s.find_last_not_of(ws);
    return a == std::string::npos ? "" : s.substr(a, b - a + 1);
}

int main(int argc, char **argv)
{
    std::string arg1  = argc > 1 ? argv[1] : "";
    std::string iface = argc > 2 ? argv[2] : "";

    unitree::robot::ChannelFactory::Instance()->Init(0, iface);
    auto hand = std::make_shared<HandController>();
    usleep(300000); // let DDS match the service

    // Direct mode: run one gesture and exit (backward compatible).
    if (!arg1.empty())
    {
        runGesture(*hand, arg1);
        std::cout << "done." << std::endl;
        return 0;
    }

    // Interactive menu: fire gestures repeatedly over one DDS connection.
    std::string line;
    while (true)
    {
        printMenu();
        if (!std::getline(std::cin, line)) break; // EOF / Ctrl-D
        line = trim(line);
        if (line.empty()) continue;
        if (!runGesture(*hand, line)) break;
    }
    std::cout << "bye." << std::endl;
    return 0;
}