#include "flight_robot/realman/modbus_register_codec.h"

#include <cstddef>
#include <string>

namespace flight_robot::realman {
namespace {

constexpr int kMinimumSignedRmApiByte = -128;
constexpr int kMaximumUnsignedRmApiByte = 255;
constexpr int kUnsignedByteValueCount = 256;
constexpr std::size_t kBytesPerModbusRegister = 2U;

bool IsValidRmApiByte(int value) {
    return value >= kMinimumSignedRmApiByte &&
           value <= kMaximumUnsignedRmApiByte;
}

std::uint16_t NormalizeRmApiByte(int value) {
    const int normalized = value < 0 ? value + kUnsignedByteValueCount : value;
    return static_cast<std::uint16_t>(normalized);
}

} // namespace

rm_control::common::Result<std::vector<int>>
EncodeModbusRegistersAsRmApiHighLowBytes(
    const std::vector<std::uint16_t> &register_values) {
    if (register_values.empty()) {
        return rm_control::common::Result<std::vector<int>>::failure(
            "cannot encode an empty Modbus register sequence");
    }
    std::vector<int> encoded_bytes;
    if (register_values.size() >
        encoded_bytes.max_size() / kBytesPerModbusRegister) {
        return rm_control::common::Result<std::vector<int>>::failure(
            "Modbus register sequence is too large to encode as num * 2 bytes");
    }
    encoded_bytes.reserve(register_values.size() * kBytesPerModbusRegister);
    for (const std::uint16_t value : register_values) {
        encoded_bytes.push_back(static_cast<int>((value >> 8U) & 0xFFU));
        encoded_bytes.push_back(static_cast<int>(value & 0xFFU));
    }
    return rm_control::common::Result<std::vector<int>>::success(encoded_bytes);
}

rm_control::common::Result<std::vector<std::uint16_t>>
DecodeModbusRegistersFromRmApiHighLowBytes(
    const std::vector<int> &rm_api_high_low_bytes) {
    if (rm_api_high_low_bytes.empty() ||
        rm_api_high_low_bytes.size() % kBytesPerModbusRegister != 0U) {
        return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
            "RM_API2 register byte sequence must contain a nonzero even byte count");
    }
    std::vector<std::uint16_t> register_values;
    register_values.reserve(rm_api_high_low_bytes.size() /
                            kBytesPerModbusRegister);
    for (std::size_t byte_index = 0;
         byte_index < rm_api_high_low_bytes.size();
         byte_index += kBytesPerModbusRegister) {
        const int high = rm_api_high_low_bytes[byte_index];
        const int low = rm_api_high_low_bytes[byte_index + 1U];
        if (!IsValidRmApiByte(high) || !IsValidRmApiByte(low)) {
            const std::size_t invalid_index =
                !IsValidRmApiByte(high) ? byte_index : byte_index + 1U;
            return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
                "invalid RM_API2 byte at index " + std::to_string(invalid_index) +
                ": " + std::to_string(rm_api_high_low_bytes[invalid_index]));
        }
        register_values.push_back(static_cast<std::uint16_t>(
            (NormalizeRmApiByte(high) << 8U) | NormalizeRmApiByte(low)));
    }
    return rm_control::common::Result<std::vector<std::uint16_t>>::success(
        register_values);
}

} // namespace flight_robot::realman
