#include "rm_control/core/o6_bus_scanner.h"

#include <utility>

namespace rm_control::core {
namespace {

bool MatchesO6IdentityShape(std::uint8_t device_id,
                            const std::vector<std::uint16_t> &registers) {
    if (registers.size() != 6U || registers[0] != 6U) {
        return false;
    }
    const char expected_direction = device_id == 0x27U ? 'R' : 'L';
    return registers[5] == static_cast<std::uint16_t>(expected_direction);
}

common::Result<void> ValidateScanConfig(const O6BusScanConfig &config) {
    const std::vector<std::uint8_t> expected_device_ids{0x27U, 0x28U};
    if (config.device_ids != expected_device_ids ||
        config.identity_start_address != 30U ||
        config.identity_register_count != 6U) {
        return common::Result<void>::failure(
            "O6 bus probe is fixed to device IDs 0x27/0x28 and FC04 addresses 30..35");
    }
    return common::Result<void>::success();
}

O6BusScanResponse ProbeOneDevice(interfaces::IToolModbusBus &tool_bus,
                                 const O6BusScanConfig &config,
                                 std::uint8_t device_id) {
    O6BusScanResponse response;
    response.device_id = device_id;
    const auto multiple = tool_bus.ReadInputRegisters(
        device_id, config.identity_start_address,
        config.identity_register_count);
    response.multiple_read_succeeded = multiple.ok;
    if (multiple.ok) {
        response.identity_registers = multiple.value;
        if (multiple.value.size() != config.identity_register_count) {
            response.multiple_read_succeeded = false;
            response.multiple_read_error =
                "multiple input read returned an unexpected register count";
        }
    } else {
        response.multiple_read_error = multiple.error_message;
    }
    const auto single = tool_bus.ReadInputRegister(
        device_id, config.identity_start_address);
    response.single_read_succeeded = single.ok;
    if (single.ok) {
        response.single_freedom_register = single.value;
    } else {
        response.single_read_error = single.error_message;
    }
    response.single_and_multiple_reads_match =
        response.multiple_read_succeeded && response.single_read_succeeded &&
        response.identity_registers.front() == response.single_freedom_register;
    response.matches_o6_identity_shape =
        response.single_and_multiple_reads_match &&
        MatchesO6IdentityShape(device_id, response.identity_registers);
    return response;
}

} // namespace

common::Result<O6BusScanReport> ScanO6IdentityRegisters(
    interfaces::IToolModbusBus &tool_bus,
    const O6BusScanConfig &config) {
    const auto valid = ValidateScanConfig(config);
    if (!valid.ok) {
        return common::Result<O6BusScanReport>::failure(valid.error_message);
    }
    O6BusScanReport report;
    for (const std::uint8_t device_id : config.device_ids) {
        ++report.attempted_device_count;
        auto response = ProbeOneDevice(tool_bus, config, device_id);
        if (response.matches_o6_identity_shape) {
            ++report.confirmed_o6_device_count;
        } else {
            ++report.failed_device_count;
        }
        report.responses.push_back(std::move(response));
    }
    return common::Result<O6BusScanReport>::success(report);
}

} // namespace rm_control::core
