/**
 * @file greet_common.hpp
 * @brief Shared arm-action + Inspire-hand greeting routines (handshake / thumbup).
 *
 * Arm uses Unitree's built-in actions (G1ArmActionClient, balance-aware).
 * Hand uses the Inspire DDS bridge -> REQUIRES ./inspire_g1 running for fingers.
 */
#pragma once

#include "unitree/robot/g1/arm/g1_arm_action_client.hpp"
#include "unitree/robot/g1/arm/g1_arm_action_error.hpp"
#include <unitree/idl/go2/MotorCmds_.hpp>
#include <unitree/robot/channel/channel_publisher.hpp>

#include <eigen3/Eigen/Dense>
#include <iostream>
#include <memory>
#include <string>
#include <unistd.h>

using Vec6 = Eigen::Matrix<float, 6, 1>;

// Inspire hand command publisher (open-loop; needs inspire_g1 running).
class Hand
{
public:
    Hand()
    {
        pub_ = std::make_shared<unitree::robot::ChannelPublisher<unitree_go::msg::dds_::MotorCmds_>>("rt/inspire/cmd");
        pub_->InitChannel();
        cmd_.cmds().resize(12);
    }
    void set(const Vec6 &right, const Vec6 &left)
    {
        for (size_t i = 0; i < 6; i++) { cmd_.cmds()[i].q() = right(i); cmd_.cmds()[i + 6].q() = left(i); }
        pub_->Write(cmd_);
    }
private:
    unitree_go::msg::dds_::MotorCmds_ cmd_;
    unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::MotorCmds_> pub_;
};

inline Vec6 gv(float p, float r, float m, float i, float tb, float tr) { Vec6 v; v << p, r, m, i, tb, tr; return v; }
inline void ghold(int ms) { usleep(ms * 1000); }

// q in [0,1], 0=closed 1=open; order pinky,ring,middle,index,thumb_bend,thumb_rot
static const Vec6 G_OPEN        = gv(1, 1, 1, 1, 1, 1);
static const Vec6 G_THUMBUP     = gv(0, 0, 0, 0, 1, 1);
static const Vec6 G_SHAKE_READY = gv(0.85, 0.85, 0.85, 0.85, 0.85, 0.6);
static const Vec6 G_SHAKE_GRIP  = gv(0.2, 0.2, 0.2, 0.2, 0.25, 0.1);

// Execute a built-in arm action; print a helpful message on failure.
inline bool arm_do(unitree::robot::g1::G1ArmActionClient &arm, int id, const char *name)
{
    using namespace unitree::robot::g1;
    std::cout << "arm: " << name << " (" << id << ") ..." << std::endl;
    int32_t ret = arm.ExecuteAction(id);
    if (ret != 0) {
        std::cerr << "  arm action '" << name << "' failed, code " << ret;
        if (ret == UT_ROBOT_ARM_ACTION_ERR_ARMSDK)              std::cerr << " (rt/arm_sdk occupied)";
        else if (ret == UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID) std::cerr << " (need fsm {500,501,801})";
        else if (ret == UT_ROBOT_ARM_ACTION_ERR_HOLDING)        std::cerr << " (arm holding; send release 99)";
        std::cerr << std::endl;
        return false;
    }
    return true;
}

// arm reaches out -> hand grips -> thumbs-up -> arm releases
inline void runHandshake(unitree::robot::g1::G1ArmActionClient &arm, Hand &hand)
{
    hand.set(G_SHAKE_READY, G_OPEN);                 // present an open hand
    if (!arm_do(arm, 27, "shake hand")) { hand.set(G_OPEN, G_OPEN); return; }
    ghold(300);
    std::cout << "hand: grip" << std::endl;
    hand.set(G_SHAKE_GRIP, G_OPEN); ghold(1500);
    std::cout << "hand: thumbs-up" << std::endl;
    hand.set(G_THUMBUP, G_OPEN);    ghold(1200);
    hand.set(G_OPEN, G_OPEN);       ghold(400);
    arm_do(arm, 99, "release arm");
}

// raise right hand -> hand thumbs-up -> arm releases
inline void runThumbup(unitree::robot::g1::G1ArmActionClient &arm, Hand &hand)
{
    if (!arm_do(arm, 23, "right hand up")) { hand.set(G_OPEN, G_OPEN); return; }
    ghold(300);
    std::cout << "hand: thumbs-up" << std::endl;
    hand.set(G_THUMBUP, G_OPEN); ghold(2500);
    hand.set(G_OPEN, G_OPEN);    ghold(400);
    arm_do(arm, 99, "release arm");
}

// Shared setup: init DDS + arm client + hand, then return the arm client.
inline std::shared_ptr<unitree::robot::g1::G1ArmActionClient> greet_init(const std::string &iface)
{
    unitree::robot::ChannelFactory::Instance()->Init(0, iface);
    auto arm = std::make_shared<unitree::robot::g1::G1ArmActionClient>();
    arm->Init();
    arm->SetTimeout(10.f);
    return arm;
}