/**
 * @file hand_bridge.cpp
 * @brief Local TCP <-> DDS bridge for the web dashboard.
 *   - "v0 v1 ... v11\n"  -> publish MotorCmds_ to rt/inspire/cmd  (q in [0,1], or <0 = release)
 *   - "R\n"              -> reply with the 12 latest rt/inspire/state q values (read-back / capture)
 *   - "S\n"              -> reply with 24 values: 12 angles then 12 forces (joint tracker)
 * Persistent publisher + state subscriber. Needs ./inspire_g1 running.
 *
 *   ./hand_bridge [iface] [port]        (default eth0 7799)
 */
#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 <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>

#include <cstdlib>
#include <cstring>
#include <cstdio>
#include <iostream>
#include <mutex>
#include <cmath>
#include <sstream>
#include <chrono>
#include <thread>
#include <atomic>
#include <cstdlib>
#include <string>
#include <vector>

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

    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);

    // Subscribe to hand state for read-back / capture.
    std::mutex mtx;
    double state[12] = {0};
    double force[12] = {0};
    double cur[12] = {0}, fset[12] = {0};      // dq = current mA, ddq = force limit
    double temp[12] = {0}, err[12] = {0}, sta[12] = {0}, lost[12] = {0};
    double rate = 0; int rn = 0; auto rt0 = std::chrono::steady_clock::now();

    // ---- force-follow (admittance) --------------------------------------------------
    // The RH56 cannot be back-driven, so pushing a finger moves it 0.000. This closes the
    // loop in software: read the press, drive the finger that way, hold when you stop. It
    // lives here rather than in the web layer because the loop needs the 30Hz state stream
    // and the command publisher in the same place -- polling over HTTP is far too slow.
    std::atomic<bool> follow{false};
    std::atomic<double> fgain{0.0006}, fdead{60.0}, fmaxrate{0.35}, fforce{400.0};
    double ftarget[12] = {0}, fbase[12] = {0};
    std::atomic<bool> fcal{false};   // tau_est, in grams — the joint tracker needs BOTH angle and
                              // force, because 'force moved but angle did not' is the whole
                              // signature of a finger that cannot be back-driven.
    auto sub = std::make_shared<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::MotorStates_>>("rt/inspire/state");
    sub->InitChannel([&](const void *m) {
        auto s = (const unitree_go::msg::dds_::MotorStates_ *)m;
        std::lock_guard<std::mutex> lk(mtx);
        for (int i = 0; i < 12 && i < (int)s->states().size(); i++) {
            state[i] = s->states()[i].q();
            force[i] = s->states()[i].tau_est();
            cur[i]   = s->states()[i].dq();               // mA
            fset[i]  = s->states()[i].ddq();              // grip force limit
            temp[i]  = s->states()[i].temperature();
            err[i]   = s->states()[i].reserve()[0];
            sta[i]   = s->states()[i].reserve()[1];
            lost[i]  = s->states()[i].lost();
        }
        // Measure the ACTUAL publish rate here — a hand whose bus has died keeps serving
        // cached values, so a healthy-looking readout with a collapsed rate is the tell.
        if (++rn >= 20) {
            auto now = std::chrono::steady_clock::now();
            double dt = std::chrono::duration<double>(now - rt0).count();
            if (dt > 0) rate = rn / dt;
            rn = 0; rt0 = now;
        }
    });

    // Admittance thread: only publishes while follow is on, so it never fights the
    // dashboard, the recorder, or anything else driving the hand.
    std::thread ftick([&]{
        auto prev = std::chrono::steady_clock::now();
        while (true) {
            std::this_thread::sleep_for(std::chrono::milliseconds(33));
            auto now = std::chrono::steady_clock::now();
            double dt = std::chrono::duration<double>(now - prev).count(); prev = now;
            if (!follow.load()) continue;
            if (dt > 0.2) dt = 0.2;
            {
                std::lock_guard<std::mutex> lk(mtx);
                if (fcal.exchange(false)) {                 // arm: zero on the current force
                    for (int i = 0; i < 12; i++) { fbase[i] = force[i]; ftarget[i] = state[i]; }
                }
                for (int i = 0; i < 12; i++) {
                    double dev = force[i] - fbase[i];
                    if (std::abs(dev) <= fdead.load()) continue;
                    dev -= (dev > 0 ? fdead.load() : -fdead.load());
                    double step = -fgain.load() * dev * dt;       // press pad (+force) -> close
                    double cap = fmaxrate.load() * dt;
                    if (step >  cap) step =  cap;
                    if (step < -cap) step = -cap;
                    ftarget[i] += step;
                    if (ftarget[i] < 0.0) ftarget[i] = 0.0;
                    if (ftarget[i] > 1.0) ftarget[i] = 1.0;
                }
                for (int i = 0; i < 12; i++) {
                    cmd.cmds()[i].q() = ftarget[i];
                    cmd.cmds()[i].kp() = fforce.load();
                }
            }
            pub->Write(cmd);
        }
    });
    ftick.detach();

    int srv = socket(AF_INET, SOCK_STREAM, 0);
    int opt = 1;
    setsockopt(srv, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
    sockaddr_in addr{};
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = inet_addr("127.0.0.1");
    addr.sin_port = htons(port);
    if (bind(srv, (sockaddr *)&addr, sizeof(addr)) < 0) { perror("bind"); return 1; }
    listen(srv, 8);
    std::cout << "hand_bridge listening on 127.0.0.1:" << port << " (iface " << iface << ")" << std::endl;

    char buf[1024];
    while (true)
    {
        int c = accept(srv, nullptr, nullptr);
        if (c < 0) continue;
        int n = read(c, buf, sizeof(buf) - 1);
        if (n > 0)
        {
            buf[n] = 0;
            if (buf[0] == 'F' || buf[0] == 'f') // follow: "F 1 gain dead maxrate force" | "F 0"
            {
                std::istringstream iss(buf + 1);
                int on = 0; double g = 0, d = 0, mr = 0, fo = 0;
                iss >> on;
                if (iss >> g && g > 0)  fgain.store(g);
                if (iss >> d && d > 0)  fdead.store(d);
                if (iss >> mr && mr > 0) fmaxrate.store(mr);
                if (iss >> fo && fo > 0) fforce.store(fo);
                if (on) fcal.store(true);          // re-zero the baseline each time it arms
                follow.store(on != 0);
                std::string o = std::string("follow ") + (on ? "on" : "off") + "\n";
                (void)!write(c, o.c_str(), o.size());
            }
            else if (buf[0] == 'D' || buf[0] == 'd') // full diagnostics: 8x12 + rate
            {
                std::string out;
                { std::lock_guard<std::mutex> lk(mtx);
                  char t[32];
                  const double *blocks[8] = {state, force, cur, fset, temp, err, sta, lost};
                  for (int b = 0; b < 8; b++)
                    for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.3f ", blocks[b][i]); out += t; }
                  snprintf(t, sizeof(t), "%.2f ", rate); out += t; }
                out += "\n";
                (void)!write(c, out.c_str(), out.size());
            }
            else if (buf[0] == 'S' || buf[0] == 's') // angle + force: 24 values
            {
                std::string out;
                { std::lock_guard<std::mutex> lk(mtx);
                  char t[32];
                  for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.3f ", state[i]); out += t; }
                  for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.1f ", force[i]); out += t; } }
                out += "\n";
                (void)!write(c, out.c_str(), out.size());
            }
            else if (buf[0] == 'R' || buf[0] == 'r') // read-back request (12 angles, legacy)
            {
                std::string out;
                { std::lock_guard<std::mutex> lk(mtx);
                  char t[32];
                  for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.3f ", state[i]); out += t; } }
                out += "\n";
                (void)!write(c, out.c_str(), out.size());
            }
            else
            {
                std::istringstream iss(buf);
                std::vector<float> v; float x;
                while (iss >> x) v.push_back(x);
                if (v.size() == 12)
                {
                    for (int i = 0; i < 12; i++) cmd.cmds()[i].q() = v[i];
                    for (int k = 0; k < 3; k++) { pub->Write(cmd); usleep(8000); }
                    (void)!write(c, "ok\n", 3);
                }
                else (void)!write(c, "err\n", 4);
            }
        }
        close(c);
    }
    return 0;
}