/**
 * @file hand_setid.cpp
 * @brief Give a hand a distinct RS-485 id, permanently, so left/right can never swap again.
 *
 * WHY THIS EXISTS
 *   The RH56 has no handedness register — it cannot tell you whether it is a left or a right
 *   unit. Both USB-RS485 dongles are CH340 1a86:7523 reporting the SAME USB serial. So the
 *   only thing distinguishing the two hands was the ttyUSB number, which the kernel hands out
 *   in enumeration order — and that changes across replugs, reboots and hub hiccups. When it
 *   flips, left and right silently trade places: nothing errors, and you drive the wrong hand.
 *   (Observed on this robot 2026-07-30: software "L.*" was in fact the robot's RIGHT hand.)
 *
 *   Giving the two hands DIFFERENT ids fixes that at the root. inspire_g1 scans ids 1..4 on
 *   each port and, when the ids differ, uses them as identity: id 1 = RIGHT, id 2 = LEFT.
 *   That is wiring-independent and survives any replug.
 *
 * USAGE — run ONCE per robot, with the OTHER hand's dongle unplugged or on its own port:
 *   g++ -std=c++17 -Iinclude example/hand_setid.cpp -o build/hand_setid
 *   ./build/hand_setid /dev/ttyUSBx            # just report which id answers
 *   ./build/hand_setid /dev/ttyUSBx 1 2        # change id 1 -> 2 and SAVE it
 *
 * Convention: leave the RIGHT hand at id 1, give the LEFT hand id 2.
 * Needs the port free — stop the container first.
 *
 * Registers: HAND_ID 1000 (0x03E8), SAVE 1005 (0x03ED). The id change only survives a
 * power cycle if SAVE is written afterwards, which this does.
 */
#include "inspire.h"
#include <cstdio>
#include <cstdlib>
#include <string>
#include <memory>
#include <unistd.h>

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

// Write the SAVE register (1005) so the new id survives a power cycle.
static void saveParams(SerialPort &sp, uint8_t id)
{
    uint8_t c[9] = {0xEB, 0x90, id, 0x04, 0x12, 0xED, 0x03, 0x01, 0x00};
    c[8] = cks(c, 9);
    sp.send(c, 9);
    usleep(300000);
    uint8_t b[32];
    sp.recv(b, sizeof(b));
}

// Which id answers on this port? -1 if none.
static int findId(SerialPort::SharedPtr sp)
{
    for (int id = 1; id <= 4; id++)
    {
        inspire::InspireHand h(sp, (uint8_t)id);
        Eigen::Matrix<double, 6, 1> q;
        for (int t = 0; t < 3; t++)
            if (h.GetPosition(q) == 0) return id;
    }
    return -1;
}

int main(int argc, char **argv)
{
    if (argc < 2)
    {
        printf("usage: %s <port> [from_id to_id]\n"
               "  %s /dev/ttyUSB0          report the id that answers\n"
               "  %s /dev/ttyUSB0 1 2      change id 1 -> 2 and save\n",
               argv[0], argv[0], argv[0]);
        return 1;
    }
    const char *dev = argv[1];
    auto sp = std::make_shared<SerialPort>(dev, B115200, 8);

    int cur = findId(sp);
    if (cur < 0)
    {
        printf("%s: no hand answered on ids 1-4.\n"
               "  Is the container still holding the port? Is the hand powered?\n", dev);
        return 2;
    }
    printf("%s: hand answers at rs485 id %d\n", dev, cur);

    if (argc < 4) return 0;

    int from = atoi(argv[2]), to = atoi(argv[3]);
    if (from != cur)
    {
        printf("refusing: you said from=%d but this hand is at id %d.\n", from, cur);
        return 3;
    }
    if (to < 1 || to > 4)
    {
        printf("refusing: to_id must be 1-4.\n");
        return 3;
    }

    printf("changing id %d -> %d ...\n", from, to);
    inspire::InspireHand h(sp, (uint8_t)from);
    h.ChangeID((uint8_t)from, (uint8_t)to);
    usleep(300000);
    saveParams(*sp, (uint8_t)to);
    usleep(300000);

    int now = findId(sp);
    if (now == to)
        printf("OK — this hand is now id %d and it is saved.\n"
               "  Convention: RIGHT hand stays id 1, LEFT hand becomes id 2.\n"
               "  inspire_g1 will now identify the hands by id, and left/right can no longer\n"
               "  swap however the USB ports enumerate.\n", now);
    else
        printf("FAILED — hand now answers at id %d (wanted %d). Nothing saved reliably;\n"
               "  power-cycle the hand and re-check with: %s %s\n", now, to, argv[0], dev);
    return now == to ? 0 : 4;
}