#include "flight_robot/realman/modbus_register_codec.h"

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

namespace {

struct TestContext {
    int failures = 0;

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

void TestEncodePreservesZeroByteAndUint16Boundaries(TestContext *context) {
    const std::vector<std::uint16_t> registers{
        0x0000U, 0x00FFU, 0x0100U, 0xFFFFU};

    const auto result =
        flight_robot::realman::EncodeModbusRegistersAsRmApiHighLowBytes(
            registers);

    context->Expect(result.ok, "boundary registers should encode successfully");
    context->Expect(
        result.ok && result.value ==
            (std::vector<int>{0x00, 0x00, 0x00, 0xFF,
                              0x01, 0x00, 0xFF, 0xFF}),
        "registers must encode as num * 2 high-low bytes without truncation");
}

void TestDecodeUnsignedBytesPreservesRegisterBoundaries(TestContext *context) {
    const std::vector<int> bytes{
        0x00, 0x00, 0x00, 0xFF, 0x01, 0x00, 0xFF, 0xFF};

    const auto result =
        flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes(
            bytes);

    context->Expect(result.ok, "unsigned RM API bytes should decode");
    context->Expect(
        result.ok && result.value ==
            (std::vector<std::uint16_t>{0x0000U, 0x00FFU, 0x0100U, 0xFFFFU}),
        "unsigned high-low byte pairs must decode to exact uint16 values");
}

void TestDecodeSignedInt8BytesPreservesRawBits(TestContext *context) {
    const std::vector<int> bytes{0, -1, -128, 0, -1, -1};

    const auto result =
        flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes(
            bytes);

    context->Expect(result.ok, "signed int8 RM API bytes should decode");
    context->Expect(
        result.ok && result.value ==
            (std::vector<std::uint16_t>{0x00FFU, 0x8000U, 0xFFFFU}),
        "signed int8 bytes must normalize without changing their raw bits");
}

void TestEmptyEncodeAndDecodeAreRejected(TestContext *context) {
    const auto encoded =
        flight_robot::realman::EncodeModbusRegistersAsRmApiHighLowBytes({});
    const auto decoded =
        flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes({});

    context->Expect(!encoded.ok, "empty register sequence must not encode");
    context->Expect(!decoded.ok, "empty byte sequence must not decode");
}

void TestOddByteCountIsRejected(TestContext *context) {
    const auto result =
        flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes(
            std::vector<int>{0, 1, 2});

    context->Expect(!result.ok, "odd RM API byte count must be rejected");
    context->Expect(result.error_message.find("even byte count") !=
                        std::string::npos,
                    "odd byte count failure must include dimension context");
}

void TestByteBelowSignedInt8RangeIsRejected(TestContext *context) {
    const auto result =
        flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes(
            std::vector<int>{-129, 0});

    context->Expect(!result.ok, "RM API byte -129 must be rejected");
    context->Expect(result.error_message.find("index 0") != std::string::npos,
                    "invalid low-bound byte must report its index");
}

void TestByteAboveUnsignedByteRangeIsRejected(TestContext *context) {
    const auto result =
        flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes(
            std::vector<int>{0, 256});

    context->Expect(!result.ok, "RM API byte 256 must be rejected");
    context->Expect(result.error_message.find("index 1") != std::string::npos,
                    "invalid high-bound byte must report its index");
}

} // namespace

int main() {
    TestContext context;
    TestEncodePreservesZeroByteAndUint16Boundaries(&context);
    TestDecodeUnsignedBytesPreservesRegisterBoundaries(&context);
    TestDecodeSignedInt8BytesPreservesRawBits(&context);
    TestEmptyEncodeAndDecodeAreRejected(&context);
    TestOddByteCountIsRejected(&context);
    TestByteBelowSignedInt8RangeIsRejected(&context);
    TestByteAboveUnsignedByteRangeIsRejected(&context);

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