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

#include "rm_control/common/robot_types.h"
#include "rm_control/core/o6_bus_scanner.h"

#include <iomanip>
#include <iostream>

namespace {

void PrintProbeResponse(const rm_control::core::O6BusScanResponse &response) {
    std::cout << "O6 candidate 0x" << std::hex << std::uppercase
              << static_cast<int>(response.device_id) << std::dec << std::nouppercase
              << ": multi=" << (response.multiple_read_succeeded ? "ok" : "failed");
    if (response.multiple_read_succeeded) {
        std::cout << " values=[";
        for (std::size_t index = 0U; index < response.identity_registers.size(); ++index) {
            std::cout << (index == 0U ? "" : ",")
                      << response.identity_registers[index];
        }
        std::cout << ']';
    } else {
        std::cout << " error={" << response.multiple_read_error << '}';
    }
    std::cout << ", single=" << (response.single_read_succeeded ? "ok" : "failed");
    if (response.single_read_succeeded) {
        std::cout << " value=" << response.single_freedom_register;
    } else {
        std::cout << " error={" << response.single_read_error << '}';
    }
    std::cout << ", cross_match="
              << (response.single_and_multiple_reads_match ? "true" : "false")
              << ", confirmed_o6="
              << (response.matches_o6_identity_shape ? "true" : "false") << '\n';
}

void PrintRightHandReadiness(
    const rm_control::interfaces::DexterousHandReadiness &readiness) {
    std::cout << "O6 right-hand readiness confirmed: device=0x27, freedom="
              << readiness.freedom << ", direction=" << readiness.direction
              << ", version=" << readiness.hand_version << ", positions=[";
    for (std::size_t index = 0U; index < readiness.joint_positions.size(); ++index) {
        std::cout << (index == 0U ? "" : ",")
                  << readiness.joint_positions[index];
    }
    std::cout << "]\n";
}

} // namespace

int main() {
    flight_robot::realman::RealmanArmAdapter robot;
    const auto connected = robot.connect(rm_control::common::RobotConnectionConfig{});
    if (!connected.ok) {
        std::cerr << "RM75 connection failed: " << connected.error_message << '\n';
        return 1;
    }
    const auto information = robot.getRobotInfo();
    if (!information.ok || information.value.controller_version != 3) {
        std::cerr << "O6 probe requires a connected generation-3 RM75: "
                  << (information.ok ? "unexpected controller generation"
                                     : information.error_message) << '\n';
        return 1;
    }
    rm_control::interfaces::ToolModbusBusConfiguration configuration;
    configuration.power_cycle_tool = true;
    configuration.power_off_delay_ms = 500;
    configuration.startup_delay_ms = 2000;
    const auto configured = robot.Configure(configuration);
    if (!configured.ok) {
        std::cerr << "O6 tool bus initialization failed: "
                  << configured.error_message << '\n';
        return 1;
    }
    std::cout << "Tool bus ready: port=" << configured.value.port
              << ", mode=" << configured.value.mode
              << ", baudrate=" << configured.value.baudrate
              << ", voltage_type=" << configured.value.voltage_type
              << ", timeout_units=" << configured.value.timeout_units << '\n';
    const auto report = rm_control::core::ScanO6IdentityRegisters(
        robot, rm_control::core::O6BusScanConfig{});
    if (!report.ok) {
        std::cerr << "O6 targeted probe failed: " << report.error_message << '\n';
        return 1;
    }
    for (const auto &response : report.value.responses) {
        PrintProbeResponse(response);
    }
    flight_robot::linker_hand::LinkerHandAdapter hand(
        robot, rm_control::interfaces::DexterousHandSide::Right);
    const auto readiness = hand.ReadReadiness();
    if (readiness.ok) {
        PrintRightHandReadiness(readiness.value);
    } else {
        std::cerr << "O6 single-register readiness failed: "
                  << readiness.error_message << '\n';
    }
    std::cout << "O6 probe summary: attempted="
              << report.value.attempted_device_count
              << ", confirmed=" << report.value.confirmed_o6_device_count
              << ", unconfirmed=" << report.value.failed_device_count << '\n';
    return readiness.ok ? 0 : 2;
}
