/**
 * @file hand_wrtest.cpp
 * @brief Reproduce inspire_g1's write-then-read cycle standalone (no DDS) to
 *        debug why GetPosition reads 0 inside the container but works natively.
 *
 *   g++ -std=c++17 -Iinclude example/hand_wrtest.cpp -o /tmp/hand_wrtest
 *   /tmp/hand_wrtest [rightPort=/dev/ttyUSB1] [leftPort=/dev/ttyUSB0]
 *
 * Needs the ports free (stop inspire_g1 / the container first).
 */
#include "inspire.h"
#include <cstdio>
#include <memory>
#include <unistd.h>

using namespace inspire;

int main(int argc, char **argv)
{
    const char *pr = argc > 1 ? argv[1] : "/dev/ttyUSB1"; // right
    const char *pl = argc > 2 ? argv[2] : "/dev/ttyUSB0"; // left

    auto s1 = std::make_shared<SerialPort>(pr, B115200, 6);
    auto s2 = std::make_shared<SerialPort>(pl, B115200, 6);
    InspireHand right(s1, 1), left(s2, 1);

    for (auto *h : {&right, &left})
    {
        h->ClearError();
        h->SetVelocity(1000, 1000, 1000, 1000, 1000, 1000);
        h->SetForce(500, 500, 500, 500, 500, 500);
    }

    // TIGHT loop, no inter-cycle delay, alternating open/close — the worst case
    // that failed 300/300 with the old one-shot GetPosition. Verify the re-query
    // GetPosition holds up.
    Eigen::Matrix<double, 6, 1> qcmd, qr, ql;
    int rfail = 0, lfail = 0, N = 300;
    for (int cyc = 0; cyc < N; cyc++)
    {
        double v = ((cyc / 30) % 2 == 0) ? 1.0 : 0.0; // flip every 30 cycles
        for (int i = 0; i < 6; i++) qcmd(i) = v;
        right.SetPosition(qcmd);
        left.SetPosition(qcmd);
        usleep(3000);
        int rr = right.GetPosition(qr);
        usleep(2000);
        int rl = left.GetPosition(ql);
        if (rr != 0) rfail++;
        if (rl != 0) lfail++;
        if (cyc % 30 == 0)
            printf("cyc%3d v=%.0f  R(ret%d): %.2f %.2f %.2f   L(ret%d): %.2f %.2f %.2f\n",
                   cyc, v, rr, qr(0), qr(1), qr(2), rl, ql(0), ql(1), ql(2));
    }
    printf("=== %d tight cycles: RIGHT read-fails=%d  LEFT read-fails=%d ===\n", N, rfail, lfail);
    return 0;
}