#include "mocks/mock_dexterous_hand.h"
#include "mocks/mock_tool_modbus_bus.h"

#include "flight_robot/linker_hand/linker_hand_adapter.h"

#include <cstdint>
#include <initializer_list>
#include <iostream>
#include <string>
#include <vector>

namespace {

using rm_control::common::Result;
using rm_control::interfaces::DexterousHandSide;
using rm_control::interfaces::O6JointPose;
using rm_control::interfaces::O6MotionCommand;
using rm_control::tests::MockToolModbusBus;

constexpr std::uint16_t kPositionStartAddress = 0U;
constexpr std::uint16_t kTorqueStartAddress = 6U;
constexpr std::uint16_t kSpeedStartAddress = 12U;
constexpr std::uint16_t kTemperatureStartAddress = 18U;
constexpr std::uint16_t kFaultStartAddress = 24U;
constexpr std::uint16_t kIdentityStartAddress = 30U;

struct TestContext {
    int failures = 0;

    void Expect(bool condition, const std::string &message) {
        if (!condition) {
            ++failures;
            std::cerr << "[FAIL] " << message << '\n';
        }
    }
};

Result<std::vector<std::uint16_t>> RegisterValues(
    std::initializer_list<std::uint16_t> values) {
    return Result<std::vector<std::uint16_t>>::success(
        std::vector<std::uint16_t>(values));
}

O6MotionCommand ValidMotionCommand() {
    return O6MotionCommand{
        O6JointPose{0U, 1U, 64U, 128U, 192U, 255U},
        O6JointPose{10U, 11U, 12U, 13U, 14U, 15U},
        O6JointPose{20U, 21U, 22U, 23U, 24U, 25U}};
}

void ConfigureValidRightHand(MockToolModbusBus *tool_bus) {
    tool_bus->input_register_results[kIdentityStartAddress] =
        RegisterValues({6U, 101U, 1U, 2U, 3U, static_cast<std::uint16_t>('R')});
    tool_bus->input_register_results[kPositionStartAddress] =
        RegisterValues({0U, 1U, 64U, 128U, 192U, 255U});
    tool_bus->input_register_results[kTorqueStartAddress] =
        RegisterValues({30U, 31U, 32U, 33U, 34U, 35U});
    tool_bus->input_register_results[kSpeedStartAddress] =
        RegisterValues({40U, 41U, 42U, 43U, 44U, 45U});
    tool_bus->input_register_results[kTemperatureStartAddress] =
        RegisterValues({20U, 21U, 22U, 23U, 24U, 25U});
    tool_bus->input_register_results[kFaultStartAddress] =
        RegisterValues({0U, 0U, 0U, 0U, 0U, 0U});
}

void TestValidRightHandReadiness(TestContext *context) {
    MockToolModbusBus tool_bus;
    ConfigureValidRightHand(&tool_bus);
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Right);

    const auto result = adapter.ReadReadiness();

    context->Expect(result.ok, "valid right-hand readiness should succeed");
    if (!result.ok) {
        return;
    }
    context->Expect(result.value.ready, "successful readiness must set ready=true");
    context->Expect(result.value.device_id == 0x27U,
                    "right-hand device ID must be 0x27");
    context->Expect(result.value.freedom == 6U && result.value.direction == 'R',
                    "identity must preserve freedom and right direction");
    context->Expect(result.value.joint_positions[5] == 255U,
                    "position register order must be preserved");
    context->Expect(result.value.joint_torques[0] == 30U &&
                        result.value.joint_torques[5] == 35U,
                    "torque input register order must be preserved");
    context->Expect(result.value.joint_speeds[0] == 40U &&
                        result.value.joint_speeds[5] == 45U,
                    "speed input register order must be preserved");
    context->Expect(tool_bus.status_calls == 1 &&
                        tool_bus.single_read_calls.size() == 36U &&
                        tool_bus.read_calls.empty(),
                    "readiness must query status and 36 single FC04 registers");
    for (const auto &call : tool_bus.single_read_calls) {
        context->Expect(call.device_id == 0x27U && call.register_count == 1U,
                        "each right-hand read must use ID 0x27 and one register");
    }
}

void TestDirectionMismatchRejected(TestContext *context) {
    MockToolModbusBus tool_bus;
    ConfigureValidRightHand(&tool_bus);
    tool_bus.input_register_results[kIdentityStartAddress] =
        RegisterValues({6U, 101U, 1U, 2U, 3U, static_cast<std::uint16_t>('L')});
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Right);

    const auto result = adapter.ReadReadiness();

    context->Expect(!result.ok, "right-hand adapter must reject identity direction L");
    context->Expect(result.error_message.find("direction") != std::string::npos,
                    "direction rejection should include field context");
    context->Expect(tool_bus.single_read_calls.size() == 6U,
                    "identity mismatch must stop without extra Modbus reads");
}

void TestValidLeftHandUsesLeftIdentityAndDeviceId(TestContext *context) {
    MockToolModbusBus tool_bus;
    ConfigureValidRightHand(&tool_bus);
    tool_bus.input_register_results[kIdentityStartAddress] =
        RegisterValues({6U, 101U, 1U, 2U, 3U, static_cast<std::uint16_t>('L')});
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Left);

    const auto result = adapter.ReadReadiness();

    context->Expect(result.ok && result.value.device_id == 0x28U,
                    "valid left hand must use Modbus device ID 0x28");
    context->Expect(result.ok && result.value.direction == 'L',
                    "valid left-hand identity must preserve direction L");
    for (const auto &call : tool_bus.single_read_calls) {
        context->Expect(call.device_id == 0x28U,
                        "each left-hand read must use device ID 0x28");
    }
}

void TestFaultRejected(TestContext *context) {
    MockToolModbusBus tool_bus;
    ConfigureValidRightHand(&tool_bus);
    tool_bus.input_register_results[kFaultStartAddress] =
        RegisterValues({0U, 0U, 7U, 0U, 0U, 0U});
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Right);

    const auto result = adapter.ReadReadiness();

    context->Expect(!result.ok, "nonzero O6 fault register must reject readiness");
    context->Expect(result.error_message.find("code=7") != std::string::npos,
                    "fault rejection should include the device fault code");
}

void TestTemperatureRejected(TestContext *context) {
    MockToolModbusBus tool_bus;
    ConfigureValidRightHand(&tool_bus);
    tool_bus.input_register_results[kTemperatureStartAddress] =
        RegisterValues({20U, 21U, 22U, 71U, 24U, 25U});
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Right);

    const auto result = adapter.ReadReadiness();

    context->Expect(!result.ok, "temperature above 70 Celsius must reject readiness");
    context->Expect(result.error_message.find("temperature") != std::string::npos,
                    "temperature rejection should include field context");
}

void TestRegisterDimensionRejected(TestContext *context) {
    MockToolModbusBus tool_bus;
    ConfigureValidRightHand(&tool_bus);
    tool_bus.input_register_results[kPositionStartAddress] =
        RegisterValues({0U, 1U, 2U, 3U, 4U});
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Right);

    const auto result = adapter.ReadReadiness();

    context->Expect(!result.ok, "five position registers must reject O6 readiness");
    context->Expect(result.error_message.find("address 5") !=
                        std::string::npos,
                    "missing sixth register should include its exact address");
}

void TestInvalidToolBusStatusRejectedBeforeRead(TestContext *context) {
    MockToolModbusBus tool_bus;
    ConfigureValidRightHand(&tool_bus);
    tool_bus.status_result =
        Result<rm_control::interfaces::ToolModbusBusStatus>::success(
            rm_control::interfaces::ToolModbusBusStatus{
                true, 1, 1, 57600, 3, 10});
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Right);

    const auto result = adapter.ReadReadiness();

    context->Expect(!result.ok, "non-115200 tool bus must reject readiness");
    context->Expect(tool_bus.read_calls.empty() && tool_bus.single_read_calls.empty(),
                    "invalid tool bus must be rejected before Modbus reads");
}

bool WasHoldingRegisterAddressWritten(const MockToolModbusBus &tool_bus,
                                      std::uint16_t address) {
    for (const auto &call : tool_bus.write_calls) {
        if (call.start_address == address) {
            return true;
        }
    }
    return false;
}

void TestValidMotionUsesVerifiedSafetyConfigurationBeforePosition(
    TestContext *context) {
    MockToolModbusBus tool_bus;
    const O6MotionCommand command = ValidMotionCommand();
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Right);

    const auto result = adapter.CommandMotion(command);

    context->Expect(result.ok, "valid O6 motion command should succeed");
    context->Expect(
        tool_bus.operation_calls ==
            (std::vector<std::string>{"write_holding:6", "write_holding:12",
                                      "write_holding:0"}),
        "motion must write torque, speed, then position without false FC04 command readback");
    context->Expect(tool_bus.single_read_calls.empty(),
                    "command path must not confuse live FC04 state with FC16 command values");
    context->Expect(tool_bus.write_calls.size() == 3U,
                    "valid motion must write each register group exactly once");
    for (const auto &call : tool_bus.write_calls) {
        context->Expect(call.device_id == 0x27U,
                        "all right-hand writes must use Modbus device ID 0x27");
    }
    if (tool_bus.write_calls.size() == 3U) {
        context->Expect(
            tool_bus.write_calls[0].values == std::vector<std::uint16_t>(
                command.joint_torque_commands.begin(),
                command.joint_torque_commands.end()),
            "torque write must preserve all six values");
        context->Expect(
            tool_bus.write_calls[1].values == std::vector<std::uint16_t>(
                command.joint_speed_commands.begin(),
                command.joint_speed_commands.end()),
            "speed write must preserve all six values");
        context->Expect(
            tool_bus.write_calls[2].values == std::vector<std::uint16_t>(
                command.target_positions.begin(), command.target_positions.end()),
            "position write must preserve all six values");
    }
}

void TestOutOfRangeCommandRejectedWithoutWrite(TestContext *context) {
    MockToolModbusBus tool_bus;
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Left);
    O6MotionCommand command = ValidMotionCommand();
    command.joint_speed_commands[2] = 256U;

    const auto result = adapter.CommandMotion(command);

    context->Expect(!result.ok, "O6 command value 256 must be rejected");
    context->Expect(tool_bus.write_calls.empty(),
                    "out-of-range O6 command must not reach the bus");
    context->Expect(tool_bus.read_calls.empty() && tool_bus.single_read_calls.empty(),
                    "out-of-range O6 command must not read the bus");
}

void TestTorqueWriteFailureStopsWithoutRetryOrPosition(TestContext *context) {
    MockToolModbusBus tool_bus;
    const O6MotionCommand command = ValidMotionCommand();
    tool_bus.write_results_by_start_address[kTorqueStartAddress] =
        Result<void>::failure("mock torque write timeout");
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Right);

    const auto result = adapter.CommandMotion(command);

    context->Expect(!result.ok, "torque write failure must be returned to the caller");
    context->Expect(tool_bus.write_calls.size() == 1U,
                    "failed torque write must not be retried automatically");
    context->Expect(!WasHoldingRegisterAddressWritten(tool_bus, kPositionStartAddress),
                    "torque write failure must prevent position write");
    context->Expect(result.error_message.find("mock torque write timeout") !=
                        std::string::npos,
                    "torque write failure should preserve bus error context");
}

void TestSpeedWriteFailureStopsWithoutRetryOrPosition(TestContext *context) {
    MockToolModbusBus tool_bus;
    const O6MotionCommand command = ValidMotionCommand();
    tool_bus.write_results_by_start_address[kSpeedStartAddress] =
        Result<void>::failure("mock speed write timeout");
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Right);

    const auto result = adapter.CommandMotion(command);

    context->Expect(!result.ok, "speed write failure must be returned");
    context->Expect(tool_bus.operation_calls ==
                        (std::vector<std::string>{"write_holding:6",
                                                  "write_holding:12"}),
                    "speed write failure must stop immediately without retry");
    context->Expect(!WasHoldingRegisterAddressWritten(tool_bus, kPositionStartAddress),
                    "speed write failure must prevent position write");
}

void TestPositionWriteFailureIsNotRetried(TestContext *context) {
    MockToolModbusBus tool_bus;
    const O6MotionCommand command = ValidMotionCommand();
    tool_bus.write_results_by_start_address[kPositionStartAddress] =
        Result<void>::failure("mock position write timeout");
    flight_robot::linker_hand::LinkerHandAdapter adapter(
        tool_bus, DexterousHandSide::Right);

    const auto result = adapter.CommandMotion(command);

    context->Expect(!result.ok, "position write failure must be returned");
    context->Expect(tool_bus.write_calls.size() == 3U,
                    "failed final position write must not be retried");
    context->Expect(result.error_message.find("mock position write timeout") !=
                        std::string::npos,
                    "position write failure must preserve bus error context");
}

void TestMockDexterousHandRecordsCommand(TestContext *context) {
    rm_control::tests::MockDexterousHand hand;
    const O6MotionCommand command = ValidMotionCommand();

    const auto result = hand.CommandMotion(command);

    context->Expect(result.ok && hand.command_calls == 1,
                    "MockDexterousHand should report and count a configured command");
    context->Expect(
        hand.last_motion_command.target_positions == command.target_positions &&
            hand.last_motion_command.joint_speed_commands ==
                command.joint_speed_commands &&
            hand.last_motion_command.joint_torque_commands ==
                command.joint_torque_commands,
        "MockDexterousHand should preserve the complete last motion command");
}

} // namespace

int main() {
    TestContext context;
    TestValidRightHandReadiness(&context);
    TestDirectionMismatchRejected(&context);
    TestValidLeftHandUsesLeftIdentityAndDeviceId(&context);
    TestFaultRejected(&context);
    TestTemperatureRejected(&context);
    TestRegisterDimensionRejected(&context);
    TestInvalidToolBusStatusRejectedBeforeRead(&context);
    TestValidMotionUsesVerifiedSafetyConfigurationBeforePosition(&context);
    TestOutOfRangeCommandRejectedWithoutWrite(&context);
    TestTorqueWriteFailureStopsWithoutRetryOrPosition(&context);
    TestSpeedWriteFailureStopsWithoutRetryOrPosition(&context);
    TestPositionWriteFailureIsNotRetried(&context);
    TestMockDexterousHandRecordsCommand(&context);

    if (context.failures == 0) {
        std::cout << "[PASS] rm_linker_hand_adapter_tests\n";
        return 0;
    }
    std::cerr << context.failures << " test(s) failed\n";
    return 1;
}
