/**
 * @file like.cpp
 * @brief Robot holds a thumbs-up "like" WHILE the built-in handshake arm action plays.
 *
 * A background thread republishes the 👍 continuously (so inspire_g1 always has a
 * fresh command and the fingers stay curled), while the main thread triggers the
 * handshake and holds it for a fixed window (ExecuteAction can return before the
 * arm actually finishes moving, so we don't gate on its return).
 *
 *   ./like [thumb_bend] [thumb_rot] [networkInterface]
 *     thumb_bend, thumb_rot in [0,1] (default 1 1). Tweak if 👍 isn't clean:
 *     e.g.  ./like 1 0   ./like 0.7 0.3
 *   default iface: eth0
 *
 * Needs exactly one ./inspire_g1 running for the fingers. The ARM moves —
 * clear space, E-stop ready.
 */
#include "greet_common.hpp"
#include <cstdlib>
#include <thread>
#include <atomic>

int main(int argc, char **argv)
{
    const float       tb    = argc > 1 ? std::atof(argv[1]) : 1.0f; // thumb_bend (1 = extended)
    const float       tr    = argc > 2 ? std::atof(argv[2]) : 1.0f; // thumb_rot
    const std::string iface = argc > 3 ? argv[3] : "eth0";

    auto arm = greet_init(iface);
    Hand hand;
    usleep(1500000); // 1.5s: let DDS match inspire_g1 + the arm service before commanding

    // On these hands the 4 fingers close at q=1 (q=0 = extended, inverted vs SDK doc).
    const Vec6 like = gv(1, 1, 1, 1, tb, tr); // 4 fingers curled into a fist, thumb up

    std::cout << "*** LIKE while handshake (arm + hand). Clear space, E-stop ready. ***\n"
              << "hand: thumbs-up (thumb_bend=" << tb << " thumb_rot=" << tr << ")" << std::endl;

    // Background thread: keep the 👍 published the whole time.
    std::atomic<bool> running{true};
    std::thread holder([&] { while (running) { hand.set(like, G_OPEN); ghold(40); } });

    ghold(1200); // let the 👍 form before the arm starts

    if (arm_do(*arm, 27, "shake hand")) {
        ghold(4500);                          // hold the 👍 while the handshake plays
        arm_do(*arm, 99, "release arm");
        ghold(500);
    }

    running = false;
    holder.join();
    hand.set(G_OPEN, G_OPEN);
    std::cout << "done." << std::endl;
    return 0;
}