/**
 * @file hand_readtest.cpp
 * @brief Low-level read diagnostic: send the Inspire "read angleAct" query on a
 *        serial port and dump the RAW bytes that come back. Isolates whether the
 *        hand answers position reads (writes are confirmed working).
 *
 *   g++ -std=c++17 -Iinclude example/hand_readtest.cpp -o /tmp/hand_readtest
 *   /tmp/hand_readtest /dev/ttyUSB0 [slaveid=1] [waitms=15]
 *
 * Needs the port free (stop inspire_g1 / the container first).
 */
#include "SerialPort.h"
#include <cstdio>
#include <cstdint>
#include <cstdlib>
#include <unistd.h>

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

int main(int argc, char **argv)
{
    const char *dev = argc > 1 ? argv[1] : "/dev/ttyUSB0";
    int slave = argc > 2 ? atoi(argv[2]) : 1;
    int waitms = argc > 3 ? atoi(argv[3]) : 15;

    SerialPort sp(dev, B115200, 40); // 40ms read timeout
    uint8_t q[9] = {0xEB, 0x90, (uint8_t)slave, 0x04, 0x11, 0x0A, 0x06, 0x0C, 0x00};
    q[8] = checksum(q, 9);

    printf("port=%s slave=%d wait=%dms  (query angleAct)\n", dev, slave, waitms);
    for (int t = 0; t < 8; t++)
    {
        sp.flush();
        sp.send(q, 9);
        usleep(waitms * 1000);
        uint8_t buf[128];
        size_t n = sp.recv(buf, sizeof(buf));
        printf("  try %d: %2zu bytes:", t, n);
        for (size_t i = 0; i < n; i++) printf(" %02X", buf[i]);
        printf("\n");
        usleep(150000);
    }
    return 0;
}