/**
 * @file hand_forcetest.cpp
 * @brief Read BOTH the actual angle (0x0A) and the force sensor (0x2E) of the
 *        right hand while it holds open, to see whether pushing a finger by hand
 *        registers as a FORCE change even though the position can't move
 *        (non-backdrivable). Robust re-query read (flush + resend + scan).
 *
 *   g++ -std=c++17 -Iinclude example/hand_forcetest.cpp -o /tmp/hft
 *   /tmp/hft /dev/ttyUSB1   (right hand; needs the port free)
 */
#include "inspire.h"
#include <cstdio>
#include <cstdint>
#include <memory>
#include <vector>
#include <unistd.h>

// Robust read of a 6-value register (angleAct 0x0A or forceAct 0x2E).
static int robustRead(SerialPort &sp, uint8_t addr, uint8_t id,
                      Eigen::Matrix<double, 6, 1> &out, double scale, bool sign)
{
    std::vector<uint8_t> q = {0xEB, 0x90, id, 0x04, 0x11, addr, 0x06, 0x0C, 0x00};
    uint8_t s = 0;
    for (size_t i = 2; i < q.size() - 1; i++) s += q[i];
    q.back() = s;

    uint8_t buf[64];
    for (int att = 0; att < 8; att++)
    {
        sp.flush();
        sp.send(q.data(), q.size());
        usleep(8000);
        size_t n = sp.recv(buf, sizeof(buf));
        for (size_t off = 0; off + 20 <= n; off++)
        {
            if (!((buf[off] == 0x90 && buf[off + 1] == 0xEB) ||
                  (buf[off] == 0xEB && buf[off + 1] == 0x90))) continue;
            if (buf[off + 4] != 0x11 || buf[off + 5] != addr) continue;
            uint8_t cs = 0;
            for (int i = 2; i < 19; i++) cs += buf[off + i];
            if (buf[off + 19] != cs) continue;
            const uint8_t *b = buf + off;
            for (int i = 0; i < 6; i++)
            {
                uint16_t raw = b[7 + 2 * i] | (b[8 + 2 * i] << 8);
                out(i) = (sign ? (double)(int16_t)raw : (double)raw) * scale;
            }
            return 0;
        }
    }
    return 1;
}

int main(int argc, char **argv)
{
    const char *pr = argc > 1 ? argv[1] : "/dev/ttyUSB1";
    auto sp = std::make_shared<SerialPort>(pr, B115200, 6);
    inspire::InspireHand right(sp, 1);
    right.ClearError();
    right.SetVelocity(1000, 1000, 1000, 1000, 1000, 1000);
    right.SetForce(500, 500, 500, 500, 500, 500);

    Eigen::Matrix<double, 6, 1> qopen, pos, force;
    for (int i = 0; i < 6; i++) qopen(i) = 1.0;

    // DIAGNOSTIC: mimic inspire_g1's tight loop (SetPos, settle, GetPos, GetForce,
    // no inter-cycle sleep) and count how often each read fails + whether force
    // actually changes. This reproduces the "force frozen" symptom.
    if (argc > 2 && std::string(argv[2]) == "diag")
    {
        int posFail = 0, forceFail = 0, N = 200;
        double fmin[6], fmax[6];
        for (int i = 0; i < 6; i++) { fmin[i] = 1e9; fmax[i] = -1e9; }
        for (int k = 0; k < N; k++)
        {
            right.SetPosition(qopen);
            usleep(3000);
            if (robustRead(*sp, 0x0A, 1, pos, 1.0 / 1000.0, false) != 0) posFail++;
            if (robustRead(*sp, 0x2E, 1, force, 1.0, true) != 0) forceFail++;
            else for (int i = 0; i < 6; i++) { if (force(i) < fmin[i]) fmin[i] = force(i); if (force(i) > fmax[i]) fmax[i] = force(i); }
        }
        printf("tight loop N=%d:  GetPosition fails=%d   GetForce fails=%d\n", N, posFail, forceFail);
        printf("force range over run (g): ");
        for (int i = 0; i < 6; i++) printf("[%.0f..%.0f] ", fmin[i], fmax[i]);
        printf("\n");
        return 0;
    }

    const char *NM[6] = {"pinky", "ring", "mid", "index", "thumbB", "thumbR"};
    const double THRESH = 100.0; // grams of deviation from baseline = a deliberate push

    // Drive open + hold, and calibrate each finger's resting-force baseline (~2s).
    printf("Calibrating resting baseline — DON'T touch the fingers for 2s...\n");
    double base[6] = {0, 0, 0, 0, 0, 0};
    int nb = 0;
    for (int k = 0; k < 10; k++)
    {
        right.SetPosition(qopen);
        usleep(3000);
        if (robustRead(*sp, 0x2E, 1, force, 1.0, true) == 0)
        {
            for (int i = 0; i < 6; i++) base[i] += force(i);
            nb++;
        }
        usleep(150000);
    }
    for (int i = 0; i < 6; i++) base[i] = nb ? base[i] / nb : 0;
    printf("Baseline(g): ");
    for (int i = 0; i < 6; i++) printf("%s=%.0f ", NM[i], base[i]);
    printf("\n\nNow PUSH one right finger at a time — it flags which finger and how hard:\n\n");

    for (int k = 0; k < 120; k++)
    {
        right.SetPosition(qopen);
        usleep(3000);
        int rf = robustRead(*sp, 0x2E, 1, force, 1.0, true);
        if (rf == 0)
        {
            char line[256]; int p = 0; bool pushed = false;
            for (int i = 0; i < 6; i++)
            {
                double dev = force(i) - base[i];
                if (dev > THRESH || dev < -THRESH)
                {
                    pushed = true;
                    p += sprintf(line + p, "%s:%+.0fg  ", NM[i], dev);
                }
            }
            if (pushed) printf("k%3d  PUSH -> %s\n", k, line);
        }
        usleep(120000);
    }
    printf("(done)\n");
    return 0;
}