/**
 * @file arm_action.cpp
 * @brief Trigger Unitree's built-in G1 upper-body actions (shake hand, etc.).
 *
 * Uses unitree_sdk2 G1ArmActionClient. The action server drives rt/arm_sdk for
 * you (balance-aware, Unitree-designed motions) — do NOT run arm_raise at the
 * same time (error 7400 = rt/arm_sdk occupied).
 *
 *   ./arm_action list                 # list actions
 *   ./arm_action "shake hand"         # by name
 *   ./arm_action 27 [iface]           # by id  (default iface: eth0)
 *   ./arm_action "release arm"        # = 99, returns the arm to normal
 *
 * After most actions the arm HOLDS until you send "release arm" (99).
 * Actions require the robot FSM id in {500,501,801} (check rt/sportmodestate).
 */

#include "unitree/robot/g1/arm/g1_arm_action_client.hpp"
#include "unitree/robot/g1/arm/g1_arm_action_error.hpp"

#include <iostream>
#include <memory>
#include <string>

using namespace unitree::robot::g1;

int main(int argc, char **argv)
{
    if (argc < 2) {
        std::cout << "Usage: ./arm_action <list | id | \"name\"> [networkInterface]\n";
        return 1;
    }
    std::string action = argv[1];
    std::string iface  = argc > 2 ? argv[2] : "eth0";

    unitree::robot::ChannelFactory::Instance()->Init(0, iface);
    auto client = std::make_shared<G1ArmActionClient>();
    client->Init();
    client->SetTimeout(10.f);

    if (action == "list") {
        std::cout << "Built-in actions (id  name):\n";
        for (const auto &kv : client->action_map)
            std::cout << "  " << kv.second << "\t" << kv.first << "\n";
        std::string data;
        if (client->GetActionList(data) == 0)
            std::cout << "\nServer-reported list:\n" << data << std::endl;
        return 0;
    }

    // Resolve action id from a number or a name.
    int32_t id = 0;
    try {
        id = std::stoi(action);
    } catch (const std::exception &) {
        auto it = client->action_map.find(action);
        if (it == client->action_map.end()) {
            std::cerr << "Unknown action '" << action << "'. Try: ./arm_action list\n";
            return 1;
        }
        id = it->second;
    }

    std::cout << "Executing action id " << id << " ..." << std::endl;
    int32_t ret = client->ExecuteAction(id);
    if (ret == 0) {
        std::cout << "OK. (Arm holds until you send \"release arm\" / 99.)" << std::endl;
        return 0;
    }

    switch (ret) {
    case UT_ROBOT_ARM_ACTION_ERR_ARMSDK:
        std::cerr << "Error 7400: " << UT_ROBOT_ARM_ACTION_ERR_ARMSDK_DESC
                  << " (something else holds rt/arm_sdk — e.g. arm_raise)." << std::endl;
        break;
    case UT_ROBOT_ARM_ACTION_ERR_HOLDING:
        std::cerr << "Error 7401: " << UT_ROBOT_ARM_ACTION_ERR_HOLDING_DESC << std::endl;
        break;
    case UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID:
        std::cerr << "Error 7402: " << UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID_DESC << std::endl;
        break;
    case UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID:
        std::cerr << "Error 7404: invalid FSM id. Actions need fsm id {500,501,801} "
                     "(check rt/sportmodestate; in 801 only fsm mode {0,3})." << std::endl;
        break;
    default:
        std::cerr << "Execute action failed, error code: " << ret << std::endl;
        break;
    }
    return ret;
}