#include "rm_control/core/o6_bus_scanner.h"

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

namespace {

class FakeScanningToolBus final : public rm_control::interfaces::IToolModbusBus {
public:
    std::vector<std::uint8_t> requested_device_ids;
    std::vector<std::string> operations;

    rm_control::common::Result<rm_control::interfaces::ToolModbusBusStatus>
    Configure(const rm_control::interfaces::ToolModbusBusConfiguration &) override {
        return GetStatus();
    }

    rm_control::common::Result<rm_control::interfaces::ToolModbusBusStatus>
    GetStatus() override {
        return rm_control::common::Result<
            rm_control::interfaces::ToolModbusBusStatus>::success(
                {true, 1, 1, 115200, 3, 1});
    }

    rm_control::common::Result<std::uint16_t> ReadInputRegister(
        std::uint8_t device_id,
        std::uint16_t) override {
        operations.push_back("single:" + std::to_string(device_id));
        if (device_id == 0x27U) {
            return rm_control::common::Result<std::uint16_t>::success(6U);
        }
        return rm_control::common::Result<std::uint16_t>::failure(
            "mock single read timeout");
    }

    rm_control::common::Result<std::vector<std::uint16_t>> ReadInputRegisters(
        std::uint8_t device_id,
        std::uint16_t,
        std::size_t) override {
        requested_device_ids.push_back(device_id);
        operations.push_back("multiple:" + std::to_string(device_id));
        if (device_id == 0x27U) {
            return rm_control::common::Result<std::vector<std::uint16_t>>::success(
                {6U, 101U, 1U, 2U, 3U, static_cast<std::uint16_t>('R')});
        }
        return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
            "mock no response");
    }

    rm_control::common::Result<std::vector<std::uint16_t>> ReadHoldingRegisters(
        std::uint8_t, std::uint16_t, std::size_t) override {
        return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
            "not used by scanner");
    }

    rm_control::common::Result<void> WriteHoldingRegisters(
        std::uint8_t,
        std::uint16_t,
        const std::vector<std::uint16_t> &) override {
        return rm_control::common::Result<void>::failure(
            "scanner must never write");
    }
};

int TestScanCollectsResponsesAndMarksO6() {
    FakeScanningToolBus bus;
    rm_control::core::O6BusScanConfig config;
    const auto result = rm_control::core::ScanO6IdentityRegisters(bus, config);
    if (!result.ok || result.value.attempted_device_count != 2U ||
        result.value.failed_device_count != 1U ||
        result.value.confirmed_o6_device_count != 1U ||
        result.value.responses.size() != 2U ||
        !result.value.responses[0].matches_o6_identity_shape ||
        result.value.responses[1].matches_o6_identity_shape ||
        result.value.responses[0].device_id != 0x27U ||
        result.value.responses[1].device_id != 0x28U ||
        bus.operations !=
            (std::vector<std::string>{"multiple:39", "single:39",
                                      "multiple:40", "single:40"})) {
        std::cerr << "[FAIL] probe should use both FC04 paths once per O6 ID\n";
        return 1;
    }
    return 0;
}

int TestInvalidRangeIsRejectedWithoutRead() {
    FakeScanningToolBus bus;
    rm_control::core::O6BusScanConfig config;
    config.device_ids = {0x27U};
    const auto result = rm_control::core::ScanO6IdentityRegisters(bus, config);
    if (result.ok || !bus.requested_device_ids.empty()) {
        std::cerr << "[FAIL] invalid scan range must fail before bus access\n";
        return 1;
    }
    return 0;
}

} // namespace

int main() {
    const int failures = TestScanCollectsResponsesAndMarksO6() +
                         TestInvalidRangeIsRejectedWithoutRead();
    if (failures == 0) {
        std::cout << "[PASS] rm_o6_bus_scanner_tests\n";
        return 0;
    }
    return 1;
}
