#include "codec.hpp"
#include <cstring>
#include <endian.h>

// ------------------------------------------------------------
// helpers
// ------------------------------------------------------------

static inline std::uint16_t make_word(std::uint8_t hi, std::uint8_t lo)
{
    return static_cast<std::uint16_t>((hi << 8) | lo);
}

static inline void split_word(std::uint16_t w,
                              std::uint8_t& hi,
                              std::uint8_t& lo)
{
    hi = static_cast<std::uint8_t>((w >> 8) & 0xFF);
    lo = static_cast<std::uint8_t>(w & 0xFF);
}

// ------------------------------------------------------------
// core generic helpers (N words)
// ------------------------------------------------------------

template<std::size_t N>
static std::array<std::uint16_t, N>
encode_words(const std::uint8_t* bytes, const DataFormat& fmt)
{
    std::array<std::uint16_t, N> words{};

    // byte → word
    for (std::size_t i = 0; i < N; ++i) {
        std::uint8_t hi, lo;

        if (fmt.byte_order == ByteOrder::BigEndian) {
            hi = bytes[i * 2];
            lo = bytes[i * 2 + 1];
        } else {
            hi = bytes[i * 2 + 1];
            lo = bytes[i * 2];
        }

        words[i] = make_word(hi, lo);
    }

    // word order
    if (fmt.word_order == WordOrder::LowFirst && N >= 2) {
        for (std::size_t i = 0; i < N / 2; ++i) {
            std::swap(words[i], words[N - 1 - i]);
        }
    }

    return words;
}

template<std::size_t N>
static void
decode_words(const std::array<std::uint16_t, N>& words,
             std::uint8_t* bytes,
             const DataFormat& fmt)
{
    std::array<std::uint16_t, N> w = words;

    // word order
    if (fmt.word_order == WordOrder::LowFirst && N >= 2) {
        for (std::size_t i = 0; i < N / 2; ++i) {
            std::swap(w[i], w[N - 1 - i]);
        }
    }

    // word → byte
    for (std::size_t i = 0; i < N; ++i) {
        std::uint8_t hi, lo;
        split_word(w[i], hi, lo);

        if (fmt.byte_order == ByteOrder::BigEndian) {
            bytes[i * 2]     = hi;
            bytes[i * 2 + 1] = lo;
        } else {
            bytes[i * 2]     = lo;
            bytes[i * 2 + 1] = hi;
        }
    }
}

// ------------------------------------------------------------
// 32-bit signed / unsigned
// ------------------------------------------------------------

std::array<std::uint16_t, 2>
encode_int32(std::int32_t value, const DataFormat& fmt)
{
    std::uint8_t b[4];
    std::memcpy(b, &value, 4);
    return encode_words<2>(b, fmt);
}

std::int32_t
decode_int32(std::uint16_t r0,
             std::uint16_t r1,
             const DataFormat& fmt)
{
    std::uint8_t b[4];
    decode_words<2>({ r0, r1 }, b, fmt);

    std::int32_t value;
    std::memcpy(&value, b, 4);
    return value;
}

std::array<std::uint16_t, 2>
encode_uint32(std::uint32_t value, const DataFormat& fmt)
{
    std::uint8_t b[4];
    std::memcpy(b, &value, 4);
    return encode_words<2>(b, fmt);
}

std::uint32_t
decode_uint32(std::uint16_t r0,
              std::uint16_t r1,
              const DataFormat& fmt)
{
    std::uint8_t b[4];
    decode_words<2>({ r0, r1 }, b, fmt);

    std::uint32_t value;
    std::memcpy(&value, b, 4);
    return value;
}

// ------------------------------------------------------------
// float / double
// ------------------------------------------------------------

std::array<std::uint16_t, 2>
encode_float(float value, const DataFormat& fmt)
{
    std::uint32_t u;
    std::memcpy(&u, &value, 4);
    // Convert host byte order → big-endian bytes before encoding words.
    if (fmt.byte_order == ByteOrder::BigEndian)
        u = htobe32(u);
    std::uint8_t b[4];
    std::memcpy(b, &u, 4);
    return encode_words<2>(b, fmt);
}

float
decode_float(std::uint16_t r0,
             std::uint16_t r1,
             const DataFormat& fmt)
{
    std::uint8_t b[4];
    decode_words<2>({ r0, r1 }, b, fmt);

    std::uint32_t u;
    std::memcpy(&u, b, 4);
    // decode_words produces big-endian bytes; convert to host byte order.
    if (fmt.byte_order == ByteOrder::BigEndian)
        u = be32toh(u);
    float value;
    std::memcpy(&value, &u, 4);
    return value;
}

std::array<std::uint16_t, 4>
encode_double(double value, const DataFormat& fmt)
{
    std::uint8_t b[8];
    std::memcpy(b, &value, 8);
    return encode_words<4>(b, fmt);
}

double
decode_double(std::uint16_t r0,
              std::uint16_t r1,
              std::uint16_t r2,
              std::uint16_t r3,
              const DataFormat& fmt)
{
    std::uint8_t b[8];
    decode_words<4>({ r0, r1, r2, r3 }, b, fmt);

    double value;
    std::memcpy(&value, b, 8);
    return value;
}

// ------------------------------------------------------------
// 64-bit signed / unsigned
// ------------------------------------------------------------

std::array<std::uint16_t, 4>
encode_int64(std::int64_t value, const DataFormat& fmt)
{
    std::uint8_t b[8];
    std::memcpy(b, &value, 8);
    return encode_words<4>(b, fmt);
}

std::int64_t
decode_int64(std::uint16_t r0,
             std::uint16_t r1,
             std::uint16_t r2,
             std::uint16_t r3,
             const DataFormat& fmt)
{
    std::uint8_t b[8];
    decode_words<4>({ r0, r1, r2, r3 }, b, fmt);

    std::int64_t value;
    std::memcpy(&value, b, 8);
    return value;
}

std::array<std::uint16_t, 4>
encode_uint64(std::uint64_t value, const DataFormat& fmt)
{
    std::uint8_t b[8];
    std::memcpy(b, &value, 8);
    return encode_words<4>(b, fmt);
}

std::uint64_t
decode_uint64(std::uint16_t r0,
              std::uint16_t r1,
              std::uint16_t r2,
              std::uint16_t r3,
              const DataFormat& fmt)
{
    std::uint8_t b[8];
    decode_words<4>({ r0, r1, r2, r3 }, b, fmt);

    std::uint64_t value;
    std::memcpy(&value, b, 8);
    return value;
}