#include "protocol.h"
#include <algorithm>
#include <cstring>

// ─── SignalDef ──────────────────────────────────────────────────────

int SignalDef::element_byte_size() const {
    switch (type_code) {
        case TC_UINT8:  case TC_INT8:   return 1;
        case TC_UINT16: case TC_INT16:  return 2;
        case TC_UINT32: case TC_INT32:  return 4;
        case TC_UINT64: case TC_INT64:  return 8;
        case TC_FLOAT32:                return 4;
        case TC_FLOAT64:                return 8;
        default:                        return 1;
    }
}

// ─── Packet header ──────────────────────────────────────────────────

bool parse_header(const uint8_t* buf, size_t len, UDPSPacketHeader& hdr) {
    if (len < UDPS_HEADER_SIZE) return false;
    std::memcpy(&hdr, buf, UDPS_HEADER_SIZE);
    return hdr.magic == UDPS_MAGIC;
}

// ─── CONFIG payload parser ──────────────────────────────────────────

static double read_f64_le(const uint8_t* p) {
    uint64_t bits;
    std::memcpy(&bits, p, 8);
    double result;
    std::memcpy(&result, &bits, 8);
    return result;
}

static uint32_t read_u32_le(const uint8_t* p) {
    return uint32_t(p[0]) | (uint32_t(p[1]) << 8) |
           (uint32_t(p[2]) << 16) | (uint32_t(p[3]) << 24);
}

static double read_raw_element(const uint8_t* p, uint8_t tc) {
    switch (tc) {
        case TC_UINT8:   return double(p[0]);
        case TC_INT8:    return double(int8_t(p[0]));
        case TC_UINT16:  return double(uint16_t(p[0]) | (uint16_t(p[1]) << 8));
        case TC_INT16:   return double(int16_t(uint16_t(p[0]) | (uint16_t(p[1]) << 8)));
        case TC_UINT32:  return double(read_u32_le(p));
        case TC_INT32:   return double(int32_t(read_u32_le(p)));
        case TC_UINT64: {
            uint64_t v; std::memcpy(&v, p, 8); return double(v);
        }
        case TC_INT64: {
            int64_t v; std::memcpy(&v, p, 8); return double(v);
        }
        case TC_FLOAT32: {
            float v; std::memcpy(&v, p, 4); return double(v);
        }
        case TC_FLOAT64: {
            double v; std::memcpy(&v, p, 8); return v;
        }
        default: return 0.0;
    }
}

static double dequantise(uint8_t qt, uint16_t raw, double rmin, double rmax) {
    double span = rmax - rmin;
    switch (qt) {
        case QUANT_UINT8:  return rmin + (double(uint8_t(raw)) / 255.0)  * span;
        case QUANT_INT8:   return rmin + (double(int8_t(raw) + 127) / 254.0) * span;
        case QUANT_UINT16: return rmin + (double(raw) / 65535.0) * span;
        case QUANT_INT16:  return rmin + (double(int16_t(raw) + 32767) / 65534.0) * span;
        default: return 0.0;
    }
}

std::vector<SignalDef> parse_config(const uint8_t* payload, size_t len) {
    std::vector<SignalDef> sigs;
    if (len < 4) return sigs;
    uint32_t num_sigs = read_u32_le(payload);
    size_t off = 4;

    for (uint32_t i = 0; i < num_sigs; i++) {
        if (off + UDPS_SIGDESC_SIZE > len) break;
        const uint8_t* raw = payload + off;

        SignalDef sd;
        sd.name.assign(reinterpret_cast<const char*>(raw),
                       ::strnlen(reinterpret_cast<const char*>(raw), 64));
        sd.type_code       = raw[64];
        sd.quant_type      = raw[65];
        sd.num_rows        = read_u32_le(raw + 67);
        sd.num_cols        = read_u32_le(raw + 71);
        sd.num_elements    = std::max(sd.num_rows, 1u) * std::max(sd.num_cols, 1u);
        sd.range_min       = read_f64_le(raw + 75);
        sd.range_max       = read_f64_le(raw + 83);
        sd.time_mode       = raw[91];
        sd.sampling_rate   = read_f64_le(raw + 92);
        sd.time_signal_idx = read_u32_le(raw + 100);
        sd.unit.assign(reinterpret_cast<const char*>(raw + 104),
                       ::strnlen(reinterpret_cast<const char*>(raw + 104), 32));

        sigs.push_back(std::move(sd));
        off += UDPS_SIGDESC_SIZE;
    }
    return sigs;
}

// ─── DATA payload parser ────────────────────────────────────────────

DataPacket parse_data(const uint8_t* payload, size_t len,
                      const std::vector<SignalDef>& sig_defs) {
    DataPacket dp;
    if (len < 8) return dp;

    std::memcpy(&dp.hrt_timestamp, payload, 8);
    size_t off = 8;
    dp.values.resize(sig_defs.size());

    for (size_t si = 0; si < sig_defs.size(); si++) {
        const auto& sig = sig_defs[si];
        auto& vals = dp.values[si];
        vals.resize(sig.num_elements);

        int esz = 0; // element size in bytes
        if (sig.quant_type == QUANT_NONE) {
            esz = sig.element_byte_size();
            size_t needed = sig.num_elements * esz;
            if (off + needed > len) break;
            for (uint32_t i = 0; i < sig.num_elements; i++) {
                vals[i] = read_raw_element(payload + off + i * esz, sig.type_code);
            }
            off += needed;
        } else {
            esz = (sig.quant_type <= QUANT_INT8) ? 1 : 2;
            size_t needed = sig.num_elements * esz;
            if (off + needed > len) break;
            for (uint32_t i = 0; i < sig.num_elements; i++) {
                uint16_t raw;
                if (esz == 1) {
                    raw = payload[off + i];
                } else {
                    raw = uint16_t(payload[off + i*2]) |
                          (uint16_t(payload[off + i*2 + 1]) << 8);
                }
                vals[i] = dequantise(sig.quant_type, raw, sig.range_min, sig.range_max);
            }
            off += needed;
        }
    }
    return dp;
}

// ─── Control packets ────────────────────────────────────────────────

std::vector<uint8_t> build_connect() {
    std::vector<uint8_t> pkt(UDPS_HEADER_SIZE, 0);
    UDPSPacketHeader hdr{};
    hdr.magic           = UDPS_MAGIC;
    hdr.type            = UDPS_TYPE_CONNECT;
    hdr.total_fragments = 1;
    std::memcpy(pkt.data(), &hdr, UDPS_HEADER_SIZE);
    return pkt;
}

std::vector<uint8_t> build_disconnect() {
    std::vector<uint8_t> pkt(UDPS_HEADER_SIZE, 0);
    UDPSPacketHeader hdr{};
    hdr.magic           = UDPS_MAGIC;
    hdr.type            = UDPS_TYPE_DISCONNECT;
    hdr.total_fragments = 1;
    std::memcpy(pkt.data(), &hdr, UDPS_HEADER_SIZE);
    return pkt;
}
