// =============================================================================
// PDL CONSULTING LLC — TRADE SECRET
// Copyright (c) 2026 PDL Consulting LLC. All rights reserved.
//
// src/cognitive_rf.cpp
// Cognitive RF Awareness — ESP32-S3 local EWMA + CUSUM baseline learner
//
// Ported from pdl-engine/pdl_platform/pdl_quantum/integrations/cognitive_rf.py
// (SpectrumBaselineLearner EWMA core + change-point detection).
//
// v1 scope: EWMA baselines, 60-sample rings, CUSUM detector, publish packet.
// v2 deferred: SVD/BSS, EmitterKnowledgeGraph, TemporalPatternMiner.
//
// Zero malloc.  All state in static arrays.  No Arduino headers required.
// =============================================================================

#include "cognitive_rf.h"
#ifdef ARDUINO
#  include "mesh_publish.h"   // mesh_publish_raw()
#else
   extern bool mesh_publish_raw(const uint8_t* data, size_t len);
#endif

#include <cstring>
#include <cstdint>
#include <cstddef>
#include <cmath>            // fabsf only

// ---------------------------------------------------------------------------
// State — all static, zero-initialised at boot
// ---------------------------------------------------------------------------

// EWMA mean per channel (dBm, float)
static float  s_ewma[COGRF_NUM_CHANNELS];

// EWMA variance per channel (variance of RSSI in dBm²)
static float  s_ewma_var[COGRF_NUM_CHANNELS];

// Per-channel observation count (saturates at 0xFFFF)
static uint16_t s_obs_count[COGRF_NUM_CHANNELS];

// Ring buffer of recent RSSI per channel (int16, dBm)
// s_ring[ch][head] cycles with s_ring_head[ch]
static int16_t  s_ring[COGRF_NUM_CHANNELS][COGRF_RING_DEPTH];
static uint8_t  s_ring_head[COGRF_NUM_CHANNELS];

// CUSUM positive accumulator per channel (tracks upward excursions only;
// per-drone C-UAS use case: sudden signal injection is the threat).
static float    s_cusum[COGRF_NUM_CHANNELS];

// Per-channel anomaly trip flag (cleared on each cogrf_init or manual reset)
static uint8_t  s_anomaly_flag[COGRF_NUM_CHANNELS];

// ---------------------------------------------------------------------------
// CRC-16/CCITT helper (same poly as wfx_features.cpp for consistency)
// ---------------------------------------------------------------------------
static uint16_t crc16_ccitt(const uint8_t* data, size_t len) {
    uint16_t crc = 0xFFFFu;
    for (size_t i = 0; i < len; i++) {
        crc ^= (uint16_t)((uint16_t)data[i] << 8u);
        for (int b = 0; b < 8; b++) {
            crc = (crc & 0x8000u)
                    ? (uint16_t)((crc << 1u) ^ 0x1021u)
                    : (uint16_t)(crc << 1u);
        }
    }
    return crc;
}

// ---------------------------------------------------------------------------
// cogrf_init
// ---------------------------------------------------------------------------
void cogrf_init() {
    for (uint8_t ch = 0u; ch < COGRF_NUM_CHANNELS; ch++) {
        s_ewma[ch]        = -80.0f;   // neutral starting RSSI (dBm)
        s_ewma_var[ch]    = 25.0f;    // initial variance (5 dBm std-dev)
        s_obs_count[ch]   = 0u;
        s_ring_head[ch]   = 0u;
        s_cusum[ch]       = 0.0f;
        s_anomaly_flag[ch] = 0u;
        for (uint8_t i = 0u; i < COGRF_RING_DEPTH; i++) {
            s_ring[ch][i] = (int16_t)-80;
        }
    }
}

// ---------------------------------------------------------------------------
// cogrf_observe
// ---------------------------------------------------------------------------
void cogrf_observe(uint8_t channel, int16_t rssi) {
    // Channel is 1-indexed on the wire; convert to 0-indexed array position.
    if (channel < 1u || channel > COGRF_NUM_CHANNELS) return;
    uint8_t idx = channel - 1u;

    float x = (float)rssi;

    // --- EWMA mean update (SpectrumBaselineLearner._TUNING["ewma_decay"]) ---
    float alpha    = COGRF_EWMA_ALPHA;
    float old_mean = s_ewma[idx];
    s_ewma[idx]    = alpha * x + (1.0f - alpha) * old_mean;

    // --- EWMA variance update (slightly lower decay for variance stability) ---
    float diff_sq     = (x - old_mean) * (x - old_mean);
    s_ewma_var[idx]   = 0.028f * diff_sq + (1.0f - 0.028f) * s_ewma_var[idx];

    // --- Ring buffer update ---
    s_ring[idx][s_ring_head[idx]] = rssi;
    s_ring_head[idx] = (uint8_t)((s_ring_head[idx] + 1u) % COGRF_RING_DEPTH);

    // --- Saturating observation count ---
    if (s_obs_count[idx] < 0xFFFFu) {
        s_obs_count[idx]++;
    }

    // --- CUSUM update (only after warm-up) ---
    if (s_obs_count[idx] >= COGRF_MIN_OBS) {
        // CUSUM: S_t = max(0, S_{t-1} + (x - mu - k))
        // Tracks positive excursions (signal-injection threat vector).
        float excess   = x - s_ewma[idx] - COGRF_CUSUM_K;
        s_cusum[idx]   = s_cusum[idx] + excess;
        if (s_cusum[idx] < 0.0f) s_cusum[idx] = 0.0f;

        if (s_cusum[idx] >= COGRF_CUSUM_THRESHOLD) {
            s_anomaly_flag[idx] = 1u;
            // Reset CUSUM to allow future detection after the event subsides
            s_cusum[idx] = 0.0f;
        }
    }
}

// ---------------------------------------------------------------------------
// cogrf_anomalous_emitter
// ---------------------------------------------------------------------------
bool cogrf_anomalous_emitter(uint8_t* channel_out, float* score_out) {
    uint8_t  best_ch    = 0u;
    float    best_score = 0.0f;
    bool     found      = false;

    for (uint8_t i = 0u; i < COGRF_NUM_CHANNELS; i++) {
        if (s_anomaly_flag[i] != 0u) {
            // Score = CUSUM headroom normalised by threshold; use obs_count as
            // a proxy for reliability (more observations → more credible alert).
            float base_score = (float)s_obs_count[i] / (float)COGRF_MIN_OBS;
            // Clamp at 10.0 to avoid dominating downstream logic
            if (base_score > 10.0f) base_score = 10.0f;

            if (!found || base_score > best_score) {
                best_score = base_score;
                best_ch    = (uint8_t)(i + 1u);   // back to 1-indexed wire channel
                found      = true;
            }
        }
    }

    if (found) {
        if (channel_out != nullptr) *channel_out = best_ch;
        if (score_out   != nullptr) *score_out   = best_score;
        // Clear all trip flags after reporting (edge-triggered semantics)
        memset(s_anomaly_flag, 0u, sizeof(s_anomaly_flag));
    }
    return found;
}

// ---------------------------------------------------------------------------
// cogrf_get_baseline
// ---------------------------------------------------------------------------
void cogrf_get_baseline(float baselines[COGRF_NUM_CHANNELS]) {
    for (uint8_t i = 0u; i < COGRF_NUM_CHANNELS; i++) {
        baselines[i] = s_ewma[i];
    }
}

// ---------------------------------------------------------------------------
// cogrf_publish
// ---------------------------------------------------------------------------
bool cogrf_publish(uint32_t sensor_id) {
    // Packet layout (COGRF_PACKET_SIZE = 76 bytes):
    //   [0]       msg_type = 0x1C
    //   [1..4]    sensor_id (big-endian)
    //   [5..8]    timestamp_ms (big-endian) — populated by millis() on target
    //   [9..60]   baselines[0..12] as float32 big-endian (13 × 4 = 52 bytes)
    //   [61..73]  anomaly_flags[0..12] (13 × uint8)
    //   [74..75]  CRC16-CCITT over bytes [0..73]

    static uint8_t pkt[COGRF_PACKET_SIZE];
    memset(pkt, 0, sizeof(pkt));

    pkt[0] = 0x1Cu;   // MSG_TYPE_COGNITIVE_RF

    pkt[1] = (uint8_t)(sensor_id >> 24u);
    pkt[2] = (uint8_t)(sensor_id >> 16u);
    pkt[3] = (uint8_t)(sensor_id >>  8u);
    pkt[4] = (uint8_t)(sensor_id);

    // Timestamp: millis() on embedded target; 0 in unit tests
#ifndef UNIT_TEST
    extern unsigned long millis();
    uint32_t ts = (uint32_t)millis();
#else
    uint32_t ts = 0u;
#endif
    pkt[5] = (uint8_t)(ts >> 24u);
    pkt[6] = (uint8_t)(ts >> 16u);
    pkt[7] = (uint8_t)(ts >>  8u);
    pkt[8] = (uint8_t)(ts);

    // Baselines: 13 × float32 big-endian
    for (uint8_t i = 0u; i < COGRF_NUM_CHANNELS; i++) {
        float  f   = s_ewma[i];
        uint32_t bits;
        // Type-pun float to uint32 without UB
        memcpy(&bits, &f, sizeof(bits));
        uint8_t* dst = pkt + 9u + (i * 4u);
        dst[0] = (uint8_t)(bits >> 24u);
        dst[1] = (uint8_t)(bits >> 16u);
        dst[2] = (uint8_t)(bits >>  8u);
        dst[3] = (uint8_t)(bits);
    }

    // Anomaly flags
    for (uint8_t i = 0u; i < COGRF_NUM_CHANNELS; i++) {
        pkt[61u + i] = s_anomaly_flag[i];
    }

    // CRC16 covers bytes [0..73]
    uint16_t crc = crc16_ccitt(pkt, COGRF_PACKET_SIZE - 2u);
    pkt[COGRF_PACKET_SIZE - 2u] = (uint8_t)(crc >> 8u);
    pkt[COGRF_PACKET_SIZE - 1u] = (uint8_t)(crc);

    return mesh_publish_raw(pkt, COGRF_PACKET_SIZE);
}
