/**
 * @file hand_record.cpp
 * @brief Record & replay named hand poses (12 joint values: right 0-5, left 6-11).
 *
 * You sculpt a shape live (close/open individual fingers), name it, and it's saved
 * to a file. Records the COMMANDED values (the serial read-back is unreliable on
 * these CH340 adapters, and the hand holds whatever it's commanded anyway).
 * Needs ./inspire_g1 running.
 *
 *   ./hand_record [iface]                interactive recorder (default iface eth0)
 *   ./hand_record play <name> [iface]    command a saved pose and hold
 *   ./hand_record list                   list saved poses
 *
 * Poses file: $HOME/DFX_inspire_service/hand_poses.txt   (line: "name v0 .. v11")
 * Per-hand joint order: pinky ring middle index thumb_bend thumb_rot.  q in [0,1].
 */
#include <unitree/idl/go2/MotorCmds_.hpp>
#include <unitree/robot/channel/channel_publisher.hpp>

#include <array>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <map>
#include <sstream>
#include <string>
#include <vector>
#include <unistd.h>

using Pose = std::array<float, 12>;

static std::string posesFile()
{
    const char *h = getenv("HOME");
    return std::string(h ? h : ".") + "/DFX_inspire_service/hand_poses.txt";
}

static std::map<std::string, Pose> loadPoses()
{
    std::map<std::string, Pose> m;
    std::ifstream f(posesFile());
    std::string line;
    while (std::getline(f, line))
    {
        if (line.empty() || line[0] == '#') continue;
        std::istringstream iss(line);
        std::string name; iss >> name;
        Pose v{}; int i = 0; float x;
        while (i < 12 && (iss >> x)) v[i++] = x;
        if (i == 12) m[name] = v;
    }
    return m;
}

static void savePose(const std::string &name, const Pose &v)
{
    auto m = loadPoses();
    m[name] = v; // add or overwrite
    std::ofstream f(posesFile(), std::ios::trunc);
    f << "# name  <right: pinky ring middle index thumb_bend thumb_rot>  <left: same>\n";
    for (const auto &kv : m)
    {
        f << kv.first;
        for (float x : kv.second) f << " " << x;
        f << "\n";
    }
}

class Hand
{
public:
    Hand()
    {
        pub_ = std::make_shared<unitree::robot::ChannelPublisher<unitree_go::msg::dds_::MotorCmds_>>("rt/inspire/cmd");
        pub_->InitChannel();
        cmd_.cmds().resize(12);
    }
    void apply(const Pose &v)
    {
        for (int i = 0; i < 12; i++) cmd_.cmds()[i].q() = v[i];
        for (int n = 0; n < 8; n++) { pub_->Write(cmd_); usleep(20000); } // ~0.16s so it lands
    }
private:
    unitree_go::msg::dds_::MotorCmds_ cmd_;
    unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::MotorCmds_> pub_;
};

static void printPose(const Pose &v)
{
    std::cout << "  R:";
    for (int i = 0; i < 6; i++) std::cout << " " << v[i];
    std::cout << "   L:";
    for (int i = 6; i < 12; i++) std::cout << " " << v[i];
    std::cout << std::endl;
}

static void printMenu()
{
    std::cout <<
        "\n--- hand_record --- q in [0,1] (tune to taste), order: pinky ring middle index thumb_bend thumb_rot\n"
        "  r <6 nums>     set RIGHT hand      l <6 nums>   set LEFT hand      b <6 nums>  set BOTH\n"
        "  f <0-11> <v>   set one joint (0-5 right, 6-11 left)     open | close   all\n"
        "  save <name>    save current shape        load <name>   recall        list | print | q\n";
}

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

    if (a1 == "list")
    {
        auto m = loadPoses();
        std::cout << m.size() << " saved pose(s):\n";
        for (const auto &kv : m) { std::cout << "  " << kv.first << ":"; printPose(kv.second); }
        return 0;
    }
    if (a1 == "play")
    {
        if (argc < 3) { std::cerr << "usage: hand_record play <name> [iface]\n"; return 1; }
        std::string name = argv[2], iface = argc > 3 ? argv[3] : "eth0";
        auto m = loadPoses();
        if (!m.count(name)) { std::cerr << "no pose '" << name << "'. try: hand_record list\n"; return 1; }
        unitree::robot::ChannelFactory::Instance()->Init(0, iface);
        Hand hand; usleep(300000);
        std::cout << "playing '" << name << "'\n"; printPose(m[name]);
        hand.apply(m[name]); usleep(1500000);
        return 0;
    }

    // interactive
    std::string iface = a1.empty() ? "eth0" : a1;
    unitree::robot::ChannelFactory::Instance()->Init(0, iface);
    Hand hand; usleep(300000);

    Pose cur; cur.fill(1.0f); // start open
    hand.apply(cur);
    printMenu();

    std::string line;
    while (true)
    {
        std::cout << "rec> " << std::flush;
        if (!std::getline(std::cin, line)) break;
        std::istringstream iss(line);
        std::string op; iss >> op;
        if (op.empty()) continue;

        if (op == "q" || op == "quit") break;
        else if (op == "open")  { cur.fill(1.0f); hand.apply(cur); printPose(cur); }
        else if (op == "close") { cur.fill(0.0f); hand.apply(cur); printPose(cur); }
        else if (op == "print") { printPose(cur); }
        else if (op == "list")  { for (const auto &kv : loadPoses()) { std::cout << "  " << kv.first << ":"; printPose(kv.second); } }
        else if (op == "r" || op == "l" || op == "b")
        {
            std::vector<float> v; float x; while (iss >> x) v.push_back(x);
            if (v.size() != 6) { std::cout << "  need 6 numbers\n"; continue; }
            if (op == "r" || op == "b") for (int i = 0; i < 6; i++) cur[i] = v[i];
            if (op == "l" || op == "b") for (int i = 0; i < 6; i++) cur[i + 6] = v[i];
            hand.apply(cur); printPose(cur);
        }
        else if (op == "f")
        {
            int idx; float val;
            if (!(iss >> idx >> val) || idx < 0 || idx > 11) { std::cout << "  usage: f <0-11> <val>\n"; continue; }
            cur[idx] = val; hand.apply(cur); printPose(cur);
        }
        else if (op == "save")
        {
            std::string name; iss >> name;
            if (name.empty()) { std::cout << "  usage: save <name>\n"; continue; }
            savePose(name, cur);
            std::cout << "  saved '" << name << "' -> " << posesFile() << "\n";
        }
        else if (op == "load")
        {
            std::string name; iss >> name;
            auto m = loadPoses();
            if (!m.count(name)) { std::cout << "  no pose '" << name << "'\n"; continue; }
            cur = m[name]; hand.apply(cur); printPose(cur);
        }
        else std::cout << "  ? see menu\n";
    }
    std::cout << "bye.\n";
    return 0;
}