// =============================================================================
// PDL CONSULTING LLC — TRADE SECRET
// Copyright (c) 2026 PDL Consulting LLC. All rights reserved.
//
// This file contains trade-secret information of PDL Consulting LLC,
// protected under the Defend Trade Secrets Act, 18 U.S.C. § 1836 et seq.,
// and the Virginia Uniform Trade Secrets Act, Va. Code § 59.1-336 et seq.
//
// UNAUTHORIZED DISCLOSURE, COPYING, REPRODUCTION, OR USE IS STRICTLY
// PROHIBITED AND MAY RESULT IN CIVIL AND CRIMINAL LIABILITY.
// =============================================================================
//
// fhss_hop.cpp — FHSS Hop Sequence Predictor
//
// C++ port of pdl-engine/pdl_platform/pdl_quantum/integrations/fhss_hop_predictor.py
//
// Static memory only.  No STL, no heap.
// Allowed headers: <cstdint>, <cstring>, <cmath>.
// =============================================================================

#include "fhss_hop.h"
#include <cstdint>
#include <cstring>
#include <cmath>

// ---------------------------------------------------------------------------
// Frequency bucket mapping
//   Covers 2.4 GHz band: 2400–2480 MHz (80 MHz / 5 = 16 buckets of 5 MHz)
//   and 5.8 GHz overlap by folding via modulo.  Anything outside falls into
//   the nearest boundary bucket.
//
//   bucket = clamp((freq_mhz - FHSS_BASE_MHZ) / FHSS_STEP_MHZ, 0, 15)
//
//   For 5.8 GHz OcuSync channels (5735–5835 MHz) the fold puts them into
//   distinct buckets via: ((freq_mhz >> 3) ^ (freq_mhz & 0xF)) & 0xF
//   — a simple but stable 4-bit hash that distributes the 5G channels.
// ---------------------------------------------------------------------------
static const uint16_t FHSS_BASE_MHZ = 2400u;
static const uint16_t FHSS_STEP_MHZ = 5u;   // 5 MHz per bucket

static inline uint8_t freq_to_bucket(uint16_t freq_mhz) {
    if (freq_mhz >= 2400u && freq_mhz < 2480u) {
        // 2.4 GHz band — linear bucket
        uint16_t offset = freq_mhz - 2400u;
        uint8_t  b = static_cast<uint8_t>(offset / FHSS_STEP_MHZ);
        if (b >= FHSS_FREQ_BUCKETS) b = FHSS_FREQ_BUCKETS - 1;
        return b;
    }
    // 5 GHz / other — fold to 4-bit hash
    return static_cast<uint8_t>(((freq_mhz >> 3) ^ (freq_mhz & 0xFu)) & 0xFu);
}

// Map bucket back to a representative frequency (centre of 2.4 GHz slot,
// or a synthetic 5.8 GHz marker for buckets that come from the fold)
static inline uint16_t bucket_to_freq(uint8_t bucket) {
    return static_cast<uint16_t>(FHSS_BASE_MHZ + bucket * FHSS_STEP_MHZ + FHSS_STEP_MHZ / 2u);
}

// ---------------------------------------------------------------------------
// Ring buffer of observations
// ---------------------------------------------------------------------------
struct FhssObs {
    uint16_t freq_mhz;
    uint32_t timestamp_us;
};

static FhssObs  s_ring[FHSS_RING_SIZE];
static int      s_ring_head;   // index of next write slot
static int      s_ring_count;  // valid entries (saturates at FHSS_RING_SIZE)

// Get observation at age 0 (newest) .. s_ring_count-1 (oldest)
static inline const FhssObs& ring_at(int age) {
    int idx = s_ring_head - 1 - age;
    if (idx < 0) idx += FHSS_RING_SIZE;
    return s_ring[idx];
}

// ---------------------------------------------------------------------------
// 3-gram count table  (LRU cache over FHSS_NGRAM_SLOTS entries)
//   key = (b0 << 8) | (b1 << 4) | b2  (three 4-bit buckets, packed into 12 bits)
//   value = counts[FHSS_FREQ_BUCKETS] for the next-bucket histogram
// ---------------------------------------------------------------------------
struct NgramSlot {
    uint16_t key;                         // packed 12-bit key; 0xFFFF = empty
    uint16_t counts[FHSS_FREQ_BUCKETS];  // next-hop histogram
    uint32_t last_used;                   // LRU timestamp (observation index)
};

static NgramSlot s_ngram[FHSS_NGRAM_SLOTS];
static uint32_t  s_ngram_tick;  // global LRU counter

static void ngram_clear(void) {
    for (int i = 0; i < FHSS_NGRAM_SLOTS; ++i) {
        s_ngram[i].key = 0xFFFFu;
        std::memset(s_ngram[i].counts, 0, sizeof(s_ngram[i].counts));
        s_ngram[i].last_used = 0;
    }
    s_ngram_tick = 0;
}

// Find slot index for key; returns -1 if not found
static int ngram_find(uint16_t key) {
    for (int i = 0; i < FHSS_NGRAM_SLOTS; ++i) {
        if (s_ngram[i].key == key) return i;
    }
    return -1;
}

// Find or create slot for key (LRU eviction on miss)
static int ngram_get_or_create(uint16_t key) {
    int idx = ngram_find(key);
    if (idx >= 0) {
        s_ngram[idx].last_used = ++s_ngram_tick;
        return idx;
    }
    // Evict LRU slot
    int lru = 0;
    uint32_t lru_ts = s_ngram[0].last_used;
    for (int i = 1; i < FHSS_NGRAM_SLOTS; ++i) {
        if (s_ngram[i].key == 0xFFFFu) { lru = i; break; }
        if (s_ngram[i].last_used < lru_ts) { lru = i; lru_ts = s_ngram[i].last_used; }
    }
    s_ngram[lru].key = key;
    std::memset(s_ngram[lru].counts, 0, sizeof(s_ngram[lru].counts));
    s_ngram[lru].last_used = ++s_ngram_tick;
    return lru;
}

// Pack three 4-bit buckets into a 12-bit key
static inline uint16_t ngram_key(uint8_t b0, uint8_t b1, uint8_t b2) {
    return static_cast<uint16_t>(((uint16_t)b0 << 8) | ((uint16_t)b1 << 4) | b2);
}

// ---------------------------------------------------------------------------
// Dwell time estimation — rolling window of inter-hop intervals
//   Stores up to FHSS_DWELL_WINDOW intervals (µs).  Estimated dwell = median.
// ---------------------------------------------------------------------------
static uint32_t s_dwell_buf[FHSS_DWELL_WINDOW];
static int      s_dwell_head;
static int      s_dwell_count;
static uint32_t s_est_dwell_us;   // 0 = not yet estimated

// Insertion sort into a scratch copy to find median — O(n^2) but n<=32
static uint32_t dwell_median(void) {
    if (s_dwell_count < 2) return 0u;
    uint32_t tmp[FHSS_DWELL_WINDOW];
    int n = s_dwell_count;
    std::memcpy(tmp, s_dwell_buf, (size_t)n * sizeof(uint32_t));
    // Simple insertion sort
    for (int i = 1; i < n; ++i) {
        uint32_t v = tmp[i];
        int j = i - 1;
        while (j >= 0 && tmp[j] > v) { tmp[j+1] = tmp[j]; --j; }
        tmp[j+1] = v;
    }
    return tmp[n / 2];
}

// ---------------------------------------------------------------------------
// Internal: update 3-gram table with the latest ring entry
// ---------------------------------------------------------------------------
static void update_ngram(void) {
    if (s_ring_count < 4) return;  // need at least 3 context + 1 next
    uint8_t b0 = freq_to_bucket(ring_at(3).freq_mhz);  // oldest of the four
    uint8_t b1 = freq_to_bucket(ring_at(2).freq_mhz);
    uint8_t b2 = freq_to_bucket(ring_at(1).freq_mhz);
    uint8_t bn = freq_to_bucket(ring_at(0).freq_mhz);  // newest = "next"

    uint16_t key = ngram_key(b0, b1, b2);
    int slot = ngram_get_or_create(key);
    if (s_ngram[slot].counts[bn] < 0xFFFFu) {
        ++s_ngram[slot].counts[bn];
    }
}

// ---------------------------------------------------------------------------
// Internal: update dwell time estimate
// ---------------------------------------------------------------------------
static void update_dwell(void) {
    if (s_ring_count < 2) return;
    uint32_t t1 = ring_at(0).timestamp_us;
    uint32_t t2 = ring_at(1).timestamp_us;
    uint32_t dt = (t1 >= t2) ? (t1 - t2) : (t2 - t1);  // handle wrap
    if (dt < FHSS_MIN_DWELL_US || dt > FHSS_MAX_DWELL_US) return;

    s_dwell_buf[s_dwell_head] = dt;
    s_dwell_head = (s_dwell_head + 1) % FHSS_DWELL_WINDOW;
    if (s_dwell_count < FHSS_DWELL_WINDOW) ++s_dwell_count;

    if (s_dwell_count >= 2) {
        s_est_dwell_us = dwell_median();
    }
}

// ---------------------------------------------------------------------------
// Internal: predict next bucket via 3-gram → 2-gram → 1-gram → unigram
// ---------------------------------------------------------------------------
static int predict_next_bucket(void) {
    if (s_ring_count < 1) return -1;

    // Try 3-gram
    if (s_ring_count >= 3) {
        uint8_t b0 = freq_to_bucket(ring_at(2).freq_mhz);
        uint8_t b1 = freq_to_bucket(ring_at(1).freq_mhz);
        uint8_t b2 = freq_to_bucket(ring_at(0).freq_mhz);
        uint16_t key = ngram_key(b0, b1, b2);
        int slot = ngram_find(key);
        if (slot >= 0) {
            int best = -1; uint16_t best_c = 0;
            for (int i = 0; i < FHSS_FREQ_BUCKETS; ++i) {
                if (s_ngram[slot].counts[i] > best_c) {
                    best_c = s_ngram[slot].counts[i]; best = i;
                }
            }
            if (best >= 0) return best;
        }
    }

    // Try 2-gram
    if (s_ring_count >= 2) {
        uint8_t b1 = freq_to_bucket(ring_at(1).freq_mhz);
        uint8_t b2 = freq_to_bucket(ring_at(0).freq_mhz);
        uint16_t key = ngram_key(0, b1, b2);
        int slot = ngram_find(key);
        if (slot >= 0) {
            int best = -1; uint16_t best_c = 0;
            for (int i = 0; i < FHSS_FREQ_BUCKETS; ++i) {
                if (s_ngram[slot].counts[i] > best_c) {
                    best_c = s_ngram[slot].counts[i]; best = i;
                }
            }
            if (best >= 0) return best;
        }
    }

    // Unigram fallback — most common bucket across all observed hops
    uint32_t bucket_counts[FHSS_FREQ_BUCKETS];
    std::memset(bucket_counts, 0, sizeof(bucket_counts));
    for (int age = 0; age < s_ring_count; ++age) {
        uint8_t b = freq_to_bucket(ring_at(age).freq_mhz);
        ++bucket_counts[b];
    }
    int best = 0;
    for (int i = 1; i < FHSS_FREQ_BUCKETS; ++i) {
        if (bucket_counts[i] > bucket_counts[best]) best = i;
    }
    return best;
}

// ---------------------------------------------------------------------------
// Public API
// ---------------------------------------------------------------------------

void fhss_init(void) {
    std::memset(s_ring, 0, sizeof(s_ring));
    s_ring_head  = 0;
    s_ring_count = 0;
    ngram_clear();
    std::memset(s_dwell_buf, 0, sizeof(s_dwell_buf));
    s_dwell_head  = 0;
    s_dwell_count = 0;
    s_est_dwell_us = 0;
}

void fhss_observe(uint16_t freq_mhz, uint32_t timestamp_us) {
    s_ring[s_ring_head].freq_mhz    = freq_mhz;
    s_ring[s_ring_head].timestamp_us = timestamp_us;
    s_ring_head = (s_ring_head + 1) % FHSS_RING_SIZE;
    if (s_ring_count < FHSS_RING_SIZE) ++s_ring_count;

    update_dwell();
    update_ngram();
}

bool fhss_predict_next(uint16_t* freq_mhz_out, uint32_t* dwell_us_out) {
    if (!freq_mhz_out || !dwell_us_out) return false;
    if (s_ring_count < FHSS_MIN_OBS)   return false;
    if (s_est_dwell_us == 0)            return false;

    int bucket = predict_next_bucket();
    if (bucket < 0) return false;

    *freq_mhz_out = bucket_to_freq(static_cast<uint8_t>(bucket));
    *dwell_us_out = s_est_dwell_us;
    return true;
}

void fhss_reset(void) {
    fhss_init();
}

int fhss_obs_count(void) {
    return s_ring_count;
}
