#include "flight_robot/linker_hand/linker_hand_adapter.h"
#include "flight_robot/realman/realman_arm_adapter.h"

#include "rm_control/common/robot_types.h"

#include <chrono>
#include <cstdlib>
#include <iostream>
#include <string>
#include <thread>

namespace {

constexpr rm_control::interfaces::O6JointPose kOpenPose{
    255U, 255U, 0U, 255U, 255U, 255U};
constexpr rm_control::interfaces::O6JointPose kGripPose{
    140U, 100U, 0U, 55U, 55U, 55U};
constexpr rm_control::interfaces::O6JointPose kSpeedCommands{
    20U, 20U, 20U, 20U, 20U, 20U};
constexpr rm_control::interfaces::O6JointPose kTorqueCommands{
    20U, 20U, 20U, 20U, 20U, 20U};
constexpr std::uint16_t kPositionTolerance = 5U;

void PrintPose(const char *label,
               const rm_control::interfaces::O6JointPose &pose) {
    std::cout << label << "=[";
    for (std::size_t index = 0U; index < pose.size(); ++index) {
        std::cout << (index == 0U ? "" : ",") << pose[index];
    }
    std::cout << "]\n";
}

bool PoseReached(const rm_control::interfaces::O6JointPose &actual,
                 const rm_control::interfaces::O6JointPose &target) {
    for (std::size_t index = 0U; index < actual.size(); ++index) {
        const int delta = std::abs(static_cast<int>(actual[index]) -
                                   static_cast<int>(target[index]));
        if (delta > static_cast<int>(kPositionTolerance)) {
            return false;
        }
    }
    return true;
}

rm_control::common::Result<void> InitializeToolBus(
    flight_robot::realman::RealmanArmAdapter &robot) {
    rm_control::interfaces::ToolModbusBusConfiguration configuration;
    configuration.power_cycle_tool = false;
    configuration.power_off_delay_ms = 500;
    configuration.startup_delay_ms = 2000;
    const auto configured = robot.Configure(configuration);
    return configured.ok
        ? rm_control::common::Result<void>::success()
        : rm_control::common::Result<void>::failure(configured.error_message);
}

rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>
WaitForGrip(flight_robot::linker_hand::LinkerHandAdapter &hand) {
    const auto deadline = std::chrono::steady_clock::now() +
        std::chrono::seconds(20);
    while (std::chrono::steady_clock::now() < deadline) {
        const auto readiness = hand.ReadReadiness();
        if (!readiness.ok || PoseReached(readiness.value.joint_positions, kGripPose)) {
            return readiness;
        }
        PrintPose("O6 moving", readiness.value.joint_positions);
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }
    return rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>::failure(
            "O6 did not reach the conservative grip pose within 20 seconds");
}

rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>
WaitForOpen(flight_robot::linker_hand::LinkerHandAdapter &hand) {
    const auto deadline = std::chrono::steady_clock::now() +
        std::chrono::seconds(20);
    while (std::chrono::steady_clock::now() < deadline) {
        const auto readiness = hand.ReadReadiness();
        if (!readiness.ok || PoseReached(readiness.value.joint_positions, kOpenPose)) {
            return readiness;
        }
        PrintPose("O6 opening", readiness.value.joint_positions);
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }
    return rm_control::common::Result<
        rm_control::interfaces::DexterousHandReadiness>::failure(
            "O6 did not reach the open byte-order probe within 20 seconds");
}

rm_control::common::Result<void> CommandPose(
    flight_robot::linker_hand::LinkerHandAdapter &hand,
    const rm_control::interfaces::O6JointPose &pose) {
    return hand.CommandMotion(
        rm_control::interfaces::O6MotionCommand{
            pose, kSpeedCommands, kTorqueCommands});
}

} // namespace

int main(int argc, char **argv) {
    if (argc != 2 || std::string(argv[1]) != "--live-grip") {
        std::cerr << "Usage: " << (argc > 0 ? argv[0] : "rm_o6_grip_demo")
                  << " --live-grip\n";
        return 64;
    }
    flight_robot::realman::RealmanArmAdapter robot;
    const auto connected = robot.connect(rm_control::common::RobotConnectionConfig{});
    const auto initialized = connected.ok
        ? InitializeToolBus(robot)
        : rm_control::common::Result<void>::failure(connected.error_message);
    if (!initialized.ok) {
        std::cerr << "O6 grip demo initialization failed: "
                  << initialized.error_message << '\n';
        return 1;
    }
    flight_robot::linker_hand::LinkerHandAdapter hand(
        robot, rm_control::interfaces::DexterousHandSide::Right);
    const auto before = hand.ReadReadiness();
    if (!before.ok) {
        std::cerr << "O6 pre-grip readiness failed: " << before.error_message << '\n';
        return 2;
    }
    PrintPose("O6 before grip", before.value.joint_positions);
    const auto opened_command = CommandPose(hand, kOpenPose);
    const auto opened = opened_command.ok
        ? WaitForOpen(hand)
        : rm_control::common::Result<
              rm_control::interfaces::DexterousHandReadiness>::failure(
              opened_command.error_message);
    if (!opened.ok) {
        std::cerr << "O6 safe open probe failed: " << opened.error_message << '\n';
        return 3;
    }
    PrintPose("O6 open reached", opened.value.joint_positions);
    std::this_thread::sleep_for(std::chrono::seconds(1));
    const auto commanded = CommandPose(hand, kGripPose);
    if (!commanded.ok) {
        std::cerr << "O6 grip command failed: " << commanded.error_message << '\n';
        return 4;
    }
    const auto after = WaitForGrip(hand);
    if (!after.ok) {
        std::cerr << "O6 grip verification failed: " << after.error_message << '\n';
        return 5;
    }
    PrintPose("O6 grip reached", after.value.joint_positions);
    return 0;
}
