#include "inspire.h"
#include "param.h"

#include "dds/Publisher.h"
#include "dds/Subscription.h"
#include <unitree/idl/go2/MotorCmds_.hpp>
#include <unitree/idl/go2/MotorStates_.hpp>
#include <unitree/common/thread/recurrent_thread.hpp>

#include <dirent.h>
#include <fstream>
#include <algorithm>
#include <vector>
#include <cstdlib>
#include <cmath>
#include <chrono>

// One detected CH340 (1a86:7523) USB-RS485 hand adapter.
struct HandPort
{
  std::string dev;   // /dev/ttyUSBn  — NOT stable across replug/reboot
  std::string path;  // USB topology path, e.g. "1-2.2.3" — stable per physical socket
};

// Find the two CH340 hand adapters and order them by PHYSICAL USB PATH.
//
// This used to sort by ttyUSB name and assign right = ports[1]. Both dongles are
// 1a86:7523 and report the SAME USB serial, so the tty number is the only thing telling
// them apart — and the kernel hands those out in enumeration order, which changes across
// replugs, reboots and hub hiccups. When it flips, left and right silently trade places:
// nothing errors, the dashboard's port check still says "ok", and you drive the wrong hand.
// Observed live on 2026-07-30 — software "L.*" was in fact the robot's RIGHT hand.
//
// The USB topology path IS stable as long as a dongle stays in the same socket, so we
// order by that instead. Override explicitly with INSPIRE_RIGHT_PATH / INSPIRE_LEFT_PATH
// (e.g. INSPIRE_RIGHT_PATH=1-2.2.3) when the sockets are known.
static std::vector<HandPort> findCH340Ports()
{
  std::vector<std::string> ttys;
  if (DIR *d = opendir("/sys/class/tty"))
  {
    for (struct dirent *e; (e = readdir(d));)
    {
      std::string n = e->d_name;
      if (n.rfind("ttyUSB", 0) == 0) ttys.push_back(n);
    }
    closedir(d);
  }
  std::vector<HandPort> ports;
  for (const auto &t : ttys)
  {
    // Resolve the device symlink, then walk UP to the USB device dir (first
    // ancestor that has an idVendor file) and check for the CH340 id.
    char real[4096];
    std::string link = "/sys/class/tty/" + t + "/device";
    if (!realpath(link.c_str(), real)) continue;
    std::string dir = real;
    while (dir.size() > 1)
    {
      std::ifstream vf(dir + "/idVendor");
      if (vf.good())
      {
        std::string vid, pid; vf >> vid;
        std::ifstream pf(dir + "/idProduct"); pf >> pid;
        if (vid == "1a86" && pid == "7523")
        {
          // The USB device dir's basename is the topology path ("1-2.2.3").
          auto s = dir.find_last_of('/');
          ports.push_back({"/dev/" + t, s == std::string::npos ? dir : dir.substr(s + 1)});
        }
        break;
      }
      auto slash = dir.find_last_of('/');
      if (slash == std::string::npos || slash == 0) break;
      dir.resize(slash);
    }
  }
  std::sort(ports.begin(), ports.end(),
            [](const HandPort &a, const HandPort &b) { return a.path < b.path; });
  return ports;
}

class InspireRunner
{
public:
  /// One opened port plus the RS-485 id that actually answered on it (-1 = nothing answered).
  struct Opened
  {
    std::shared_ptr<inspire::InspireHand> hand;
    SerialPort::SharedPtr port;
    int id = -1;
  };

  InspireRunner()
  {
    // Dynamically detect the two CH340 hand adapters (port-independent — survives
    // being moved to a hub / different USB ports). A generic udev rule grants 0666.
    auto ports = findCH340Ports();
    if (ports.size() < 2)
    {
      std::cerr << "ERROR: found " << ports.size()
                << " CH340 hand adapter(s), need 2. Plug in both USB-RS485 dongles."
                << std::endl;
      exit(1);
    }
    // --- Work out which physical hand is on which port -----------------------------------
    //
    // The RH56 has NO handedness register — it cannot tell you it is a left or a right unit
    // (checked: only HAND_ID 1000 exists, which is just the RS-485 slave address). Both
    // dongles are CH340 1a86:7523 with the same USB serial too, so there is nothing
    // intrinsic to key on. We therefore try three things, best first:
    //
    //   1. RS-485 ID. If the hands have been given DIFFERENT ids (see example/hand_setid),
    //      that is a permanent, wiring-independent identity: id 1 = RIGHT, id 2 = LEFT.
    //      This is the real fix — immune to enumeration order, replugs and hub changes.
    //   2. INSPIRE_RIGHT_PATH / INSPIRE_LEFT_PATH env — pin by physical USB socket.
    //   3. USB topology path order — stable per socket, unlike the ttyUSB number that this
    //      used to sort by (which silently swapped left and right; observed 2026-07-30).
    Opened A = openAndIdentify(ports[0].dev, ports[0].path.c_str());
    Opened B = openAndIdentify(ports[1].dev, ports[1].path.c_str());

    int ri = 1, li = 0;                      // default: USB-path order (ports[1] = right)
    const char *how = "usb path order";

    if (A.id > 0 && B.id > 0 && A.id != B.id)
    {
      // Distinct ids => unambiguous identity, whatever the ports did.
      ri = (A.id == 1) ? 0 : 1;
      li = 1 - ri;
      how = "RS-485 id (id1=right, id2=left)";
    }
    else if (std::getenv("INSPIRE_RIGHT_PATH") || std::getenv("INSPIRE_LEFT_PATH"))
    {
      if (const char *rp = std::getenv("INSPIRE_RIGHT_PATH"))
        for (size_t i = 0; i < ports.size(); i++)
          if (ports[i].path == rp) { ri = (int)i; li = (int)(1 - i); }
      if (const char *lp = std::getenv("INSPIRE_LEFT_PATH"))
        for (size_t i = 0; i < ports.size(); i++)
          if (ports[i].path == lp) { li = (int)i; ri = (int)(1 - i); }
      how = "INSPIRE_*_PATH env";
    }
    else if (std::getenv("INSPIRE_SWAP"))
    {
      ri = 0; li = 1;
      how = "usb path order + INSPIRE_SWAP";
    }

    Opened &R = (ri == 0) ? A : B;
    Opened &L = (li == 0) ? A : B;
    serial1 = R.port;  righthand = R.hand;
    serial2 = L.port;  lefthand  = L.hand;

    std::cout << "Inspire hands identified by " << how << ":\n"
              << "   RIGHT = " << ports[ri].dev << "  (usb " << ports[ri].path
              << ", rs485 id " << R.id << ")\n"
              << "   LEFT  = " << ports[li].dev << "  (usb " << ports[li].path
              << ", rs485 id " << L.id << ")" << std::endl;
    if (!(A.id > 0 && B.id > 0 && A.id != B.id))
      std::cout << "   NOTE: both hands answer on the same RS-485 id, so identity is inferred\n"
                   "         from wiring. To make it self-identifying and permanent, run once:\n"
                   "           ./build/hand_setid <left-hand-port> 1 2\n"
                   "         (gives the LEFT hand id 2; then left/right can never swap again)"
                << std::endl;

    // Recover the actuators on startup: clear any latched fault (stall/overcurrent) and
    // ensure a non-zero speed/force (e.g. after the bad serial state from two services
    // fighting over the bus, which can leave fingers unresponsive).
    for (auto h : {righthand, lefthand})
    {
      h->ClearError();
      h->SetVelocity(1000, 1000, 1000, 1000, 1000, 1000);
      h->SetForce(500, 500, 500, 500, 500, 500);
    }

    calibrateForceOffset();

    // dds
    handcmd = std::make_shared<unitree::robot::SubscriptionBase<unitree_go::msg::dds_::MotorCmds_>>(
        "rt/" + param::ns + "/cmd");
    handcmd->msg_.cmds().resize(12);
    handstate = std::make_unique<unitree::robot::RealTimePublisher<unitree_go::msg::dds_::MotorStates_>>(
        "rt/" + param::ns + "/state");
    handstate->msg_.states().resize(12);

    // Start running.
    // 20ms, not 10ms: a cycle is 2 position writes + 4 register reads over a 115200 half-
    // duplex bus, which cannot finish in 10ms. Asking for 100Hz just made the thread run
    // back-to-back with no idle, and the measured publish rate was 28Hz. 50Hz is honest and
    // achievable, and leaves headroom so a retry doesn't push the whole loop late.
    thread = std::make_shared<unitree::common::RecurrentThread>(
      20000, std::bind(&InspireRunner::run, this)
    );
  }

  /**
   * @brief Learn each finger's resting force, and raise that finger's force limit past it.
   *
   * The RH56 stops driving a finger the instant its MEASURED force reaches the force limit.
   * So a finger whose force-sensor zero has drifted above the limit can never be driven: it
   * draws 0 current and reads exactly like a dead motor. On this G1's right hand, R.mid rests
   * at ~856g and R.index at ~570g against a 500g limit — both were mis-diagnosed as dead
   * actuators until they ran their full range once the limit was raised to 1000g.
   *
   * We measure the offset with the hand open and unloaded, then (a) subtract it from the
   * published force so touch-detection works from a true zero, and (b) add it to that
   * finger's limit so the gate still trips at the same REAL force.
   */
  void calibrateForceOffset()
  {
    std::cout << "Calibrating finger force baseline (hand open, unloaded) ..." << std::endl;
    Eigen::Matrix<double, 6, 1> open6;
    open6.setOnes();
    for (int k = 0; k < 10; k++)
    {
      righthand->SetPosition(open6);
      lefthand->SetPosition(open6);
      usleep(30000);
    }

    Eigen::Matrix<double, 12, 1> acc;
    acc.setZero();
    int n = 0;
    Eigen::Matrix<double, 6, 1> ftmp;
    for (int k = 0; k < 12; k++)
    {
      bool ok = true;
      Eigen::Matrix<double, 12, 1> one;
      if (righthand->GetForce(ftmp) == 0) one.block<6, 1>(0, 0) = ftmp; else ok = false;
      if (lefthand->GetForce(ftmp) == 0)  one.block<6, 1>(6, 0) = ftmp; else ok = false;
      if (ok) { acc += one; n++; }
      usleep(20000);
    }
    if (!n)
    {
      std::cerr << "  WARNING: no force readings - skipping offset calibration. "
                   "Fingers with a drifted sensor zero will not move." << std::endl;
      foff.setZero();
      applyForce(reqForceR, 0);
      applyForce(reqForceL, 6);
      return;
    }
    foff = acc / n;

    static const char *FN[6] = {"pinky", "ring", "mid", "index", "thumbB", "thumbR"};
    for (int lo = 0; lo <= 6; lo += 6)
    {
      double req = (lo == 0) ? reqForceR : reqForceL;
      bool flagged = false;
      for (int i = lo; i < lo + 6; i++)
      {
        if (std::abs(foff(i)) > req - kForceMargin)
        {
          if (!flagged)
          {
            std::cout << "  " << (lo == 0 ? "RIGHT" : "LEFT")
                      << " hand force-sensor ZERO HAS DRIFTED:" << std::endl;
            flagged = true;
          }
          std::cout << "    " << FN[i - lo] << " rests at " << (int)foff(i)
                    << "g vs a " << (int)req << "g limit -> would read as DEAD (0 current). "
                    << "Raising its limit." << std::endl;
        }
      }
      if (flagged)
        std::cout << "    (proper fix: re-zero the force sensor with the hand unloaded)"
                  << std::endl;
      applyForce(req, lo);
    }
  }

  /**
   * @brief Open a hand's serial port and confirm it actually ANSWERS, reopening if it doesn't.
   *
   * Measured repeatedly on this robot: after a (re)start one of the two RS-485 buses comes up
   * mute — every read for that hand fails for the whole session while the other is perfect,
   * and WHICH one is random (5 restarts: R dead, L dead, both ok, R dead, L dead). The port
   * always enumerates and open() always succeeds; the adapter just never replies, so nothing
   * upstream notices. Closing and reopening clears it.
   *
   * Probing here costs a few hundred ms at startup and turns "half the robot is silently dead
   * until someone notices" into "it retried and told you".
   */
  /// SerialPort::Init calls exit(-1) if open() fails, so never hand it a path that is not
  /// currently openable — that turns a transient into a service-wide crash.
  static bool canOpen(const std::string &dev)
  {
    int fd = ::open(dev.c_str(), O_RDWR | O_NOCTTY);
    if (fd < 0) return false;
    ::close(fd);
    return true;
  }

  /// Re-resolve a hand's tty from its (stable) USB path. Reopening a CH340 repeatedly can
  /// make the adapters re-enumerate, so a device name captured at startup can go stale
  /// mid-probe — which is exactly how the earlier probe crash-looped the service.
  static std::string resolveByPath(const std::string &usbpath, const std::string &fallback)
  {
    for (const auto &p : findCH340Ports())
      if (p.path == usbpath) return p.dev;
    return fallback;
  }

  Opened openAndIdentify(const std::string &dev_in, const char *usbpath)
  {
    Opened o;
    std::string dev = dev_in;
    // Two attempts, not four: each reopen churns the USB bus and can renumber BOTH adapters.
    // One retry recovers a genuinely stalled link; more just destabilises the other hand.
    for (int attempt = 1; attempt <= 2; attempt++)
    {
      dev = resolveByPath(usbpath, dev);
      if (!canOpen(dev))
      {
        std::cerr << "  " << dev << " (usb " << usbpath << ") cannot be opened right now"
                  << std::endl;
        usleep(300000);
        dev = resolveByPath(usbpath, dev);
        if (!canOpen(dev))
        {
          // Everything downstream dereferences righthand/lefthand unconditionally, so a
          // null hand would segfault. Exit clearly instead and let the supervisor retry —
          // this self-heals as soon as the adapter is back.
          std::cerr << "FATAL: usb " << usbpath << " has no openable tty (" << dev << "). "
                       "Adapter unplugged or re-enumerating. Exiting; supervisor will retry."
                    << std::endl;
          exit(1);
        }
      }
      o.port = std::make_shared<SerialPort>(dev, B115200, 6);
      usleep(120000);                       // let the CH340 settle after open

      // Scan the RS-485 ids. Normally only id 1 is used, but giving the two hands distinct
      // ids makes them self-identifying, so look for any of them.
      for (int id = 1; id <= 4 && !o.hand; id++)
      {
        auto h = std::make_shared<inspire::InspireHand>(o.port, (uint8_t)id);
        Eigen::Matrix<double, 6, 1> q;
        for (int t = 0; t < 2; t++)
          if (h->GetPosition(q) == 0) { o.hand = h; o.id = id; break; }
      }
      if (o.hand)
      {
        std::cout << "  " << dev << " (usb " << usbpath << ") answered at rs485 id " << o.id
                  << (attempt > 1 ? "  [after reopen #" + std::to_string(attempt) + "]" : "")
                  << std::endl;
        return o;
      }
      std::cerr << "  " << dev << " (usb " << usbpath << ") is SILENT - reopening ("
                << attempt << "/2)" << std::endl;
      o.port.reset();                       // close before retrying
      usleep(250000);                       // brief release; longer churns the bus
    }
    // Give the caller a usable object regardless, so one dead hand does not take the other
    // down. The runtime watchdog keeps retrying and the warning fires once.
    dev = resolveByPath(usbpath, dev);
    if (!canOpen(dev))
    {
      std::cerr << "FATAL: usb " << usbpath << " has no openable tty (" << dev << ") after "
                   "probing. Exiting; supervisor will retry." << std::endl;
      exit(1);
    }
    o.port = std::make_shared<SerialPort>(dev, B115200, 6);
    o.hand = std::make_shared<inspire::InspireHand>(o.port, 1);
    o.id = -1;
    std::cerr << "  WARNING: " << dev << " (usb " << usbpath << ") never answered. Check its "
                 "RS-485 cable/power. Continuing so the other hand still works." << std::endl;
    return o;
  }

  /**
   * @brief Refresh the diagnostic registers for both hands.
   *
   * These are what turn "the finger will not move" from a guess into an answer:
   *   ERROR  - locked rotor / over-temp / over-current, LATCHED until CLEAR_ERROR
   *   STATUS - what the driver thinks it is doing
   *   CURRENT- 0mA means it never energised; >0 means it is trying and blocked
   *   FORCE_SET - the limit the measured force is being compared against
   */
  void refreshDiag()
  {
    uint8_t e[6], st[6], tp[6];
    Eigen::Matrix<double, 6, 1> cu, fs;
    for (int lo = 0; lo <= 6; lo += 6)
    {
      auto h = (lo == 0) ? righthand : lefthand;
      if (h->GetError(e)  == 0) for (int i = 0; i < 6; i++) dErr[lo + i] = e[i];
      if (h->GetStatus(st)== 0) for (int i = 0; i < 6; i++) dSta[lo + i] = st[i];
      if (h->GetTemp(tp)  == 0) for (int i = 0; i < 6; i++) dTmp[lo + i] = tp[i];
      if (h->GetCurrent(cu)   == 0) dCur.block<6,1>(lo,0)  = cu;
      if (h->GetForceSet(fs)  == 0) dFset.block<6,1>(lo,0) = fs;
    }
  }

  /// Poll both hands' ERROR registers and auto-clear any latched fault.
  void checkFaults()
  {
    static const char *FN[6] = {"pinky", "ring", "mid", "index", "thumbB", "thumbR"};
    for (int lo = 0; lo <= 6; lo += 6)
    {
      auto h = (lo == 0) ? righthand : lefthand;
      uint8_t err[6] = {0};
      if (h->GetError(err) != 0) continue;      // read failed; the link watchdog covers that

      std::string hit;
      for (int i = 0; i < 6; i++)
      {
        if (!err[i]) continue;
        hit += std::string(FN[i]) + "(0x";
        const char *hex = "0123456789ABCDEF";
        hit += hex[(err[i] >> 4) & 0xF];
        hit += hex[err[i] & 0xF];
        if (err[i] & 0x01) hit += " locked-rotor";
        if (err[i] & 0x02) hit += " over-temp";
        if (err[i] & 0x04) hit += " over-current";
        if (err[i] & 0x08) hit += " abnormal";
        if (err[i] & 0x10) hit += " comms";
        hit += ") ";
      }
      if (hit.empty()) continue;

      faultClears[lo == 0 ? 0 : 1]++;
      std::cerr << "FAULT on " << (lo == 0 ? "RIGHT" : "LEFT") << " hand: " << hit
                << "-> sending CLEAR_ERROR (clear #" << faultClears[lo == 0 ? 0 : 1] << ")"
                << std::endl;
      h->ClearError();
      // A stall also drops the speed/force config on some units; restore both so the finger
      // actually moves again instead of silently staying at zero speed.
      h->SetVelocity(1000, 1000, 1000, 1000, 1000, 1000);
      applyForce(lo == 0 ? reqForceR : reqForceL, lo);
    }
  }

  /// Per-finger force limit = requested + that finger's sensor offset, capped at the 1000g max.
  void applyForce(double req, int lo)
  {
    uint16_t f[6];
    for (int i = 0; i < 6; i++)
    {
      double v = req + std::abs(foff(lo + i));
      f[i] = (uint16_t)(v > kForceMax ? kForceMax : (v < 0 ? 0 : v));
    }
    auto h = (lo == 0) ? righthand : lefthand;
    h->SetForce(f[0], f[1], f[2], f[3], f[4], f[5]);
  }

  void run()
  {
    // Set command (write BOTH hands first)
    if(!handcmd->isTimeout())
    {
      for(int i(0); i<12; i++)
      {
        qcmd(i) = handcmd->msg_.cmds()[i].q();
      }
      // Optional grip-force override via the cmd's kp field (<=0 = keep default).
      // Lets the teacher drop to a low, back-drivable force so the fingers can be
      // moved by hand for recording, then restore the strong default afterwards.
      // Only re-sent when the requested value changes (avoids serial spam).
      // Applied through applyForce() so each finger's sensor offset is still added on top
      // — otherwise an override would re-gate a drifted finger back to "dead".
      double rf = handcmd->msg_.cmds()[0].kp();
      double lf = handcmd->msg_.cmds()[6].kp();
      if(rf > 0 && rf != lastForceR)
      { reqForceR = rf; applyForce(rf, 0); lastForceR = rf; }
      if(lf > 0 && lf != lastForceL)
      { reqForceL = lf; applyForce(lf, 6); lastForceL = lf; }

      righthand->SetPosition(qcmd.block<6, 1>(0, 0));
      lefthand->SetPosition(qcmd.block<6, 1>(6, 0));
    }

    // Recv state. GetPosition() is self-retrying: it flushes stale RX and re-sends the
    // angleAct query, recovering when the hand is briefly busy after a write (a single
    // one-shot query gets ignored and reads back all-zeros — worst inside Docker / a tight
    // loop). A short settle after the writes still helps the very first query land.
    Eigen::Matrix<double, 6, 1> qtemp;
    usleep(1500); // settle after the writes (was 3000)

    if(righthand->GetPosition(qtemp) == 0)
    {
      qstate.block<6, 1>(0, 0) = qtemp;
      posFailR = 0;
      everOkR = true;
    }
    else
    {
      posFailR++;
      for(int i(0); i<6; i++)
      {
        handstate->msg_.states()[i].lost()++;
      }
    }

    if(lefthand->GetPosition(qtemp) == 0)
    {
      qstate.block<6, 1>(6, 0) = qtemp;
      posFailL = 0;
      everOkL = true;
    }
    else
    {
      posFailL++;
      for(int i(0); i<6; i++)
      {
        handstate->msg_.states()[i+6].lost()++;
      }
    }

    // Force (grams, signed) — published as tau_est. A push on a finger registers
    // here even though the non-backdrivable finger can't move; used for
    // push-to-teach. Reuses the robust re-query reader.
    //
    // A FAILED read used to silently keep the previous value and republish it as if it were
    // fresh, with no counter (positions had one, force did not). A frozen force channel is
    // indistinguishable from a genuinely still finger, so push-to-teach would quietly stop
    // detecting touches and nothing would say so. Now failures bump lost() and get logged.
    Eigen::Matrix<double, 6, 1> ftemp;
    bool fr = (righthand->GetForce(ftemp) == 0);
    if(fr) fstate.block<6, 1>(0, 0) = ftemp;
    bool fl = (lefthand->GetForce(ftemp) == 0);
    if(fl) fstate.block<6, 1>(6, 0) = ftemp;

    if(!fr) { forceFailR++; for(int i(0); i<6; i++)  handstate->msg_.states()[i].lost()++; }
    else    { forceFailR = 0; everOkR = true; }
    if(!fl) { forceFailL++; for(int i(6); i<12; i++) handstate->msg_.states()[i].lost()++; }
    else    { forceFailL = 0; everOkL = true; }

    if(forceFailR == kStaleWarn)
      std::cerr << "WARNING: right hand force reads failing - published force is STALE "
                   "(push-to-teach will not detect touches)" << std::endl;
    if(forceFailL == kStaleWarn)
      std::cerr << "WARNING: left hand force reads failing - published force is STALE "
                   "(push-to-teach will not detect touches)" << std::endl;

    // A hand whose reads all fail is either (a) a stale link that a reopen would fix, or
    // (b) physically gone — unplugged, unpowered, or not answering on RS-485.
    //
    // Only (a) is worth exiting for. Exiting on (b) would boot-loop forever: the supervisor
    // restarts us every 3s, the hand is still absent, we exit again. So restart ONLY if this
    // hand was working earlier in this process — that is the signature of a link that died
    // and might come back on a reopen. If it has never answered since startup, stay up and
    // keep serving the healthy hand, loudly and once.
    bool deadR = (forceFailR > kDeadCycles) && (posFailR > kDeadCycles);
    bool deadL = (forceFailL > kDeadCycles) && (posFailL > kDeadCycles);
    if((deadR && everOkR) || (deadL && everOkL))
    {
      std::cerr << "FATAL: " << ((deadR && everOkR) ? "right" : "left")
                << " hand serial link DIED after working (" << kDeadCycles
                << " consecutive failed reads). Exiting so the supervisor can restart and "
                   "reopen/re-detect the adapters." << std::endl;
      exit(1);
    }
    if(deadR && !everOkR && !warnedNeverR)
    { warnedNeverR = true;
      std::cerr << "WARNING: RIGHT hand has NEVER answered since startup - check its RS-485 "
                   "cable/power. Continuing with the left hand only." << std::endl; }
    if(deadL && !everOkL && !warnedNeverL)
    { warnedNeverL = true;
      std::cerr << "WARNING: LEFT hand has NEVER answered since startup - check its RS-485 "
                   "cable/power. Continuing with the right hand only." << std::endl; }

    if(handstate->trylock())
    {
        for(int i(0); i<12; i++)
        {
            handstate->msg_.states()[i].q() = qstate(i);
            // Piggyback the diagnostics on fields this hand never populates, so no IDL
            // change is needed: dq=current(mA), ddq=force limit, temperature=degC,
            // reserve[0]=ERROR bits, reserve[1]=STATUS.
            handstate->msg_.states()[i].dq()          = dCur(i);
            handstate->msg_.states()[i].ddq()         = dFset(i);
            handstate->msg_.states()[i].temperature() = dTmp[i];
            handstate->msg_.states()[i].reserve()[0]  = dErr[i];
            handstate->msg_.states()[i].reserve()[1]  = dSta[i];
            // Subtract the calibrated resting offset so a drifted sensor zero doesn't look
            // like a permanent 850g press, and touch deltas start from a true zero.
            handstate->msg_.states()[i].tau_est() = fstate(i) - foff(i);
        }
        handstate->unlockAndPublish();
    }

    // Fault watchdog. ClearError() used to run ONLY in the constructor, so a finger that
    // stalled mid-session stayed latched and dead until the whole service was restarted —
    // it ignored every target at every force limit, and a release did not reset it, while
    // still reporting a plausible angle and a low force. Poll the ERROR register ~1x/s and
    // clear it automatically, naming the finger so a repeat offender is visible.
    if(cycles % 50 == 0)
    {
      refreshDiag();
      checkFaults();
    }

    // Report the rate we ACTUALLY achieve, so the gap between the requested period and the
    // serial bus's real throughput is visible instead of assumed.
    if(++cycles % 250 == 0)
    {
      auto now = std::chrono::steady_clock::now();
      double dt = std::chrono::duration<double>(now - lastReport).count();
      if(dt > 0)
      {
        pubHz = 250.0 / dt;
        std::cout << "[inspire_g1] " << pubHz << " Hz" << std::endl;
      }
      lastReport = now;
    }
  }

  unitree::common::ThreadPtr thread;

  // inspire
  SerialPort::SharedPtr serial1;
  SerialPort::SharedPtr serial2;
  std::shared_ptr<inspire::InspireHand> lefthand;
  std::shared_ptr<inspire::InspireHand> righthand;
  Eigen::Matrix<double, 12, 1> qcmd, qstate, fstate;
  double lastForceR = -1, lastForceL = -1; // last grip force applied per hand (via cmd kp)

  // Force-sensor zero offset per finger, measured at startup with the hand open+unloaded.
  // Subtracted from published force, and added to each finger's force limit.
  Eigen::Matrix<double, 12, 1> foff = Eigen::Matrix<double, 12, 1>::Zero();
  double reqForceR = 500.0, reqForceL = 500.0;   // grip force asked for, before offset
  static constexpr double kForceMax = 1000.0;    // RH56 hardware max
  static constexpr double kForceMargin = 250.0;  // headroom kept above a drifted zero

  // Read-failure tracking. Consecutive failures per hand; force used to have no counter at
  // all, so a dead force channel republished stale values indefinitely and looked healthy.
  int forceFailR = 0, forceFailL = 0, posFailR = 0, posFailL = 0;
  static constexpr int kStaleWarn = 25;    // ~0.5s at 50Hz -> warn once
  static constexpr int kDeadCycles = 150;  // ~3s of total silence -> link is gone, restart

  // Has this hand EVER answered since this process started? Distinguishes a link that died
  // (worth exiting for, a reopen may fix it) from a hand that was never there (exiting would
  // just boot-loop every 3s). Set on any successful read.
  bool everOkR = false, everOkL = false;
  bool warnedNeverR = false, warnedNeverL = false;   // warn once, not every cycle

  // Slow-poll diagnostics: ERROR/STATUS/TEMP/CURRENT/FORCE_SET, refreshed ~1x/s and
  // published in MotorState_ fields this hand does not otherwise use. Costs one extra
  // register read per second per hand, not per cycle.
  uint8_t dErr[12] = {0}, dSta[12] = {0}, dTmp[12] = {0};
  Eigen::Matrix<double, 12, 1> dCur = Eigen::Matrix<double, 12, 1>::Zero();
  Eigen::Matrix<double, 12, 1> dFset = Eigen::Matrix<double, 12, 1>::Zero();
  double pubHz = 0.0;

  long cycles = 0;
  long faultClears[2] = {0, 0};   // per hand; a climbing count = something keeps stalling
  std::chrono::steady_clock::time_point lastReport = std::chrono::steady_clock::now();

  // dds
  std::unique_ptr<unitree::robot::RealTimePublisher<unitree_go::msg::dds_::MotorStates_>> handstate;
  std::shared_ptr<unitree::robot::SubscriptionBase<unitree_go::msg::dds_::MotorCmds_>> handcmd;
};

int main(int argc, char ** argv)
{
  auto vm = param::helper(argc, argv);
  unitree::robot::ChannelFactory::Instance()->Init(0, param::network);

  std::cout << " --- Unitree Robotics --- " << std::endl;
  std::cout << "  Inspire Hand Controller  " << std::endl;

  InspireRunner runner;
 
  while (true)
  {
    sleep(1);
  }
  return 0;
}