/**
 * @file hand_diag.cpp
 * @brief Read the Inspire RH56 fault registers per finger: ERROR, STATUS, TEMP,
 *        CURRENT. Tells you exactly WHY a finger won't move (locked rotor,
 *        over-temperature, over-current, ...) instead of guessing.
 *
 * Register map (from the Inspire Modbus map; this serial protocol encodes the
 * address as low,high in bytes 5,6 — verified: CLEAR_ERROR 1004 = 0x03EC = {0xEC,0x03}):
 *   CURRENT 1594 (0x063A, 12 bytes)   ERROR 1606 (0x0646, 6)
 *   STATUS  1612 (0x064C, 6)          TEMP  1618 (0x0652, 6)
 *   CLEAR_ERROR 1004 (0x03EC)         RESET_PARA 1006 (0x03EE)
 *
 *   g++ -std=c++17 -Iinclude example/hand_diag.cpp -o /tmp/hand_diag
 *   /tmp/hand_diag /dev/ttyUSB1 [clear]      # 'clear' also sends CLEAR_ERROR
 *
 * Needs the port free (stop the container first).
 */
#include "SerialPort.h"
#include <cstdio>
#include <cstdint>
#include <cstring>
#include <string>
#include <memory>
#include <unistd.h>

static const char *FING[6] = {"pinky", "ring", "mid", "index", "thumbB", "thumbR"};

static uint8_t cksum(const uint8_t *d, int len)
{
    uint8_t s = 0;
    for (int i = 2; i < len - 1; i++) s += d[i];
    return s;
}

// Read `n` bytes from register `addr`, robustly (flush + re-query).
static bool readReg(SerialPort &sp, uint16_t addr, uint8_t n, uint8_t *out, uint8_t id = 1)
{
    uint8_t q[9] = {0xEB, 0x90, id, 0x04, 0x11,
                    (uint8_t)(addr & 0xFF), (uint8_t)(addr >> 8), n, 0x00};
    q[8] = cksum(q, 9);
    uint8_t buf[128];
    for (int att = 0; att < 8; att++)
    {
        sp.flush();
        sp.send(q, 9);
        usleep(8000);
        size_t got = sp.recv(buf, sizeof(buf));
        for (size_t off = 0; off + 7 + n <= got; off++)
        {
            if (!((buf[off] == 0x90 && buf[off + 1] == 0xEB) ||
                  (buf[off] == 0xEB && buf[off + 1] == 0x90))) continue;
            if (buf[off + 4] != 0x11) continue;
            uint16_t got_addr = buf[off + 5] | (buf[off + 6] << 8);
            if (got_addr != addr) continue;
            int frame = 7 + n + 1;                       // header+addr+data+checksum
            if (off + frame > got) continue;
            if (buf[off + frame - 1] != cksum(buf + off, frame)) continue;
            memcpy(out, buf + off + 7, n);
            return true;
        }
    }
    return false;
}

static void writeReg16(SerialPort &sp, uint16_t addr, uint16_t val, uint8_t id = 1)
{
    uint8_t c[11] = {0xEB, 0x90, id, 0x06, 0x12,
                     (uint8_t)(addr & 0xFF), (uint8_t)(addr >> 8),
                     (uint8_t)(val & 0xFF), (uint8_t)(val >> 8), 0x00, 0x00};
    c[9] = cksum(c, 10);
    sp.send(c, 10);
    usleep(5000);
    uint8_t b[32];
    sp.recv(b, sizeof(b));
}

static std::string errStr(uint8_t e)
{
    if (!e) return "OK";
    std::string s;
    if (e & 0x01) s += "LOCKED_ROTOR ";
    if (e & 0x02) s += "OVER_TEMPERATURE ";
    if (e & 0x04) s += "OVER_CURRENT ";
    if (e & 0x08) s += "ABNORMAL_OPERATION ";
    if (e & 0x10) s += "COMMUNICATION_ERROR ";
    return s;
}

// Command all 6 fingers to a position (ANGLE_SET 1486 = 0x05CE, 12 bytes).
static void setAll(SerialPort &sp, int16_t val, uint8_t id = 1)
{
    uint8_t c[20];
    c[0] = 0xEB; c[1] = 0x90; c[2] = id; c[3] = 0x0F; c[4] = 0x12;
    c[5] = 0xCE; c[6] = 0x05;
    for (int i = 0; i < 6; i++)
    {
        c[7 + 2 * i] = val & 0xFF;
        c[8 + 2 * i] = (val >> 8) & 0xFF;
    }
    c[19] = cksum(c, 20);
    sp.send(c, 20);
    usleep(5000);
    uint8_t b[32];
    sp.recv(b, sizeof(b));
}

// Write all 6 force limits (FORCE_SET 1498 = 0x05DA, 12 bytes).
static void setForceAll(SerialPort &sp, uint16_t val, uint8_t id = 1)
{
    uint8_t c[20];
    c[0] = 0xEB; c[1] = 0x90; c[2] = id; c[3] = 0x0F; c[4] = 0x12;
    c[5] = 0xDA; c[6] = 0x05;
    for (int i = 0; i < 6; i++)
    {
        c[7 + 2 * i] = val & 0xFF;
        c[8 + 2 * i] = (val >> 8) & 0xFF;
    }
    c[19] = cksum(c, 20);
    sp.send(c, 20);
    usleep(5000);
    uint8_t b[32];
    sp.recv(b, sizeof(b));
}

// Does the motor actually energize when told to move?  current>0 => it's trying
// (mechanically blocked);  current==0 => it never energizes.
//
// IMPORTANT: current==0 alone does NOT mean the actuator is dead. The RH56 stops driving a
// finger the instant its MEASURED force (FORCE_ACT 1582) reaches its force LIMIT
// (FORCE_SET 1498), so a finger whose force-sensor zero has drifted above the limit draws
// 0 mA and is indistinguishable from a dead motor. This test used to inherit whatever limit
// the service last wrote (500g) and so reproduced exactly that failure while looking like an
// independent raw-serial check — it is how the G1's right mid+index were wrongly called dead
// when they in fact run their full range at a 1000g limit. It now sweeps the limit itself.
static void moveTest(SerialPort &sp, uint16_t force)
{
    printf("\n=== MOVE TEST: force limit %ug, command CLOSE, watch current + force + angle ===\n",
           force);
    setForceAll(sp, force);
    setAll(sp, 1000);              // open first
    usleep(800000);

    uint8_t f0[12] = {0};
    if (readReg(sp, 1582, 12, f0))
    {
        printf("resting force vs the %ug limit (>= limit => the driver refuses to move it):\n  ",
               force);
        for (int i = 0; i < 6; i++)
        {
            int16_t F = (int16_t)(f0[2 * i] | (f0[2 * i + 1] << 8));
            printf("%s=%dg%s  ", FING[i], F, (F >= (int)force || -F >= (int)force) ? "<<GATED" : "");
        }
        printf("\n");
    }

    setAll(sp, 0);                 // CLOSE
    for (int k = 0; k < 6; k++)
    {
        usleep(400000);
        uint8_t ang[12] = {0}, cur[12] = {0}, sta[6] = {0};
        bool ra = readReg(sp, 1546, 12, ang);
        bool rc = readReg(sp, 1594, 12, cur);
        bool rs = readReg(sp, 1612, 6, sta);
        printf("t=%.1fs  reads: angle=%s current=%s status=%s\n",
               k * 0.4, ra ? "ok" : "FAIL", rc ? "ok" : "FAIL", rs ? "ok" : "FAIL");
        if (!(ra || rc || rs)) continue;
        printf("        ");
        for (int i = 0; i < 6; i++)                 // ALL fingers: healthy ones are the control
        {
            uint16_t A = ang[2 * i] | (ang[2 * i + 1] << 8);
            int16_t C = (int16_t)(cur[2 * i] | (cur[2 * i + 1] << 8));
            printf("%s:a=%u,i=%d,s=%d  ", FING[i], A, C, sta[i]);
        }
        printf("\n");
    }
    setAll(sp, 1000);
    usleep(500000);
    setForceAll(sp, 500);          // restore the service default
    printf("\nIf a finger stayed put at 500g but moves here, it is NOT dead — its force-sensor\n"
           "zero has drifted. Re-zero the sensor with the hand unloaded (FORCE_SENSOR_CALIB),\n"
           "or keep its limit above the offset.  Compare: '%s move' vs '%s move 1000'.\n",
           "hand_diag <port>", "hand_diag <port>");
}

// Are the dead channels merely CONFIGURED not to drive?  A speed / force / current
// limit of 0 would give exactly the observed signature: no motion, no current, no
// error, status "reached target".  That would be software-fixable.
static void paramDump(SerialPort &sp)
{
    struct { const char *name; uint16_t addr; uint8_t bytes; } regs[] = {
        {"ANGLE_SET   ", 1486, 12},
        {"FORCE_SET   ", 1498, 12},
        {"SPEED_SET   ", 1522, 12},
        {"CURRENT_LIM ", 1020, 12},
        {"ANGLE_ACT   ", 1546, 12},
    };
    printf("%-13s", "register");
    for (int i = 0; i < 6; i++) printf("%9s", FING[i]);
    printf("\n");
    for (auto &r : regs)
    {
        uint8_t b[16] = {0};
        bool ok = readReg(sp, r.addr, r.bytes, b);
        printf("%-13s", r.name);
        for (int i = 0; i < 6; i++)
        {
            if (!ok) { printf("%9s", "FAIL"); continue; }
            printf("%9d", (int16_t)(b[2 * i] | (b[2 * i + 1] << 8)));
        }
        printf("\n");
    }
    printf("\n(a 0 in SPEED_SET / FORCE_SET / CURRENT_LIM for a finger would explain\n"
           " no motion + no current + no error, and would be software-fixable.)\n");
}

int main(int argc, char **argv)
{
    const char *dev = argc > 1 ? argv[1] : "/dev/ttyUSB1";
    std::string opt = argc > 2 ? argv[2] : "";
    bool doClear = (opt == "clear");
    bool doMove = (opt == "move");
    SerialPort sp(dev, B115200, 8);
    if (opt == "params") { paramDump(sp); return 0; }
    // Command a CLOSE, then read ANGLE_SET back: does the setpoint actually LAND in
    // the dead channels?  ANGLE_SET=0 but no motion  => setpoint accepted, motor dead.
    // ANGLE_SET stays 1000                          => the channel rejects the write.
    if (opt == "setclose")
    {
        printf("commanding CLOSE (ANGLE_SET=0) ...\n\n");
        setAll(sp, 0);
        usleep(1500000);
        paramDump(sp);
        setAll(sp, 1000);
        return 0;
    }
    // 'move [force]' — force defaults to the RH56 max so a drifted sensor zero can't
    // silently gate a healthy finger off and make it look dead.
    if (doMove)
    {
        uint16_t f = argc > 3 ? (uint16_t)atoi(argv[3]) : 1000;
        moveTest(sp, f);
        return 0;
    }

    if (doClear)
    {
        printf("sending CLEAR_ERROR (1004) ...\n");
        writeReg16(sp, 1004, 1);
        usleep(300000);
    }

    uint8_t err[6] = {0}, sta[6] = {0}, tmp[6] = {0}, cur[12] = {0};
    uint8_t fact[12] = {0}, fset[12] = {0};
    bool eok = readReg(sp, 1606, 6, err);
    bool sok = readReg(sp, 1612, 6, sta);
    bool tok = readReg(sp, 1618, 6, tmp);
    bool cok = readReg(sp, 1594, 12, cur);
    bool faok = readReg(sp, 1582, 12, fact);   // FORCE_ACT — measured
    bool fsok = readReg(sp, 1498, 12, fset);   // FORCE_SET — the limit

    printf("port %s   (error=%s status=%s temp=%s current=%s force=%s limit=%s)\n\n", dev,
           eok ? "ok" : "FAIL", sok ? "ok" : "FAIL", tok ? "ok" : "FAIL", cok ? "ok" : "FAIL",
           faok ? "ok" : "FAIL", fsok ? "ok" : "FAIL");
    printf("%-8s %6s %8s %6s %9s %8s %7s   %s\n",
           "finger", "err", "status", "temp", "current", "force", "limit", "meaning");
    bool anyGated = false;
    for (int i = 0; i < 6; i++)
    {
        int16_t c  = cok  ? (int16_t)(cur[2 * i]  | (cur[2 * i + 1] << 8))  : 0;
        int16_t fa = faok ? (int16_t)(fact[2 * i] | (fact[2 * i + 1] << 8)) : 0;
        int16_t fs = fsok ? (int16_t)(fset[2 * i] | (fset[2 * i + 1] << 8)) : 0;
        // THE check that was missing: a finger already at/over its force limit is gated off
        // by the driver and will read 0 current with no error — identical to a dead motor.
        bool gated = faok && fsok && fs > 0 && (fa >= fs || -fa >= fs);
        if (gated) anyGated = true;
        std::string mean = eok ? errStr(err[i]) : "?";
        if (gated) mean = "FORCE-GATED (not dead: force >= limit)";
        printf("%-8s   0x%02X %8d %5d\xC2\xB0 %8d %7dg %6dg   %s\n",
               FING[i], eok ? err[i] : 0, sok ? sta[i] : -1, tok ? tmp[i] : -1, c,
               fa, fs, mean.c_str());
    }
    if (anyGated)
        printf("\n>> One or more fingers is FORCE-GATED: its measured force already meets its\n"
               ">> limit, so the driver refuses to energise it (0 current, no error). This is a\n"
               ">> drifted force-sensor zero, NOT a dead actuator. Confirm with:\n"
               ">>     %s %s move 1000\n"
               ">> Fix by re-zeroing the sensor with the hand unloaded, or raise the limit.\n",
               argv[0], dev);
    return 0;
}