// ============================================================================
// jamming_detector.cpp — Counter-RF / active jamming detection
// ============================================================================

#include "jamming_detector.h"

#ifdef ARDUINO
#include <Arduino.h>         // millis()
#include "mesh_publish.h"    // mesh_publish_raw()
#include "config.h"          // DEFAULT_SENSOR_ID
#else
// Native stub: replace millis() with a simple counter driven by observation
// calls so tests can advance simulated time.
#include <cstdint>
static uint32_t s_fake_millis = 0;
static inline uint32_t millis() { return s_fake_millis; }
// Expose for test harness only
extern "C" void jamming_test_advance_ms(uint32_t ms) { s_fake_millis += ms; }
static const uint32_t DEFAULT_SENSOR_ID = 0x00000001u;
#endif

// ============================================================================
// Internal state — all static, no heap allocations
// ============================================================================

// --- Channel A: packet-loss sliding window ----------------------------------
static uint8_t  s_loss_window[JAMMING_LOSS_WINDOW_SLOTS];  // 1=lost, 0=ok
static uint8_t  s_loss_head   = 0;
static uint16_t s_loss_count  = 0;   // total observations inserted
static uint8_t  s_loss_sum    = 0;   // rolling sum of lost packets in window

// --- Channel B: per-WiFi-channel RSSI history (last value per channel) ------
static int16_t  s_ch_rssi[JAMMING_WIFI_CHANNEL_COUNT + 1];  // index 0 unused
static bool     s_ch_seen[JAMMING_WIFI_CHANNEL_COUNT + 1];

// --- Channel C: noise-floor baseline ----------------------------------------
static int16_t  s_baseline_rssi     = -100;  // starts at a very low floor
static bool     s_baseline_seeded   = false;

// --- Alarm state ------------------------------------------------------------
static bool     s_alarmed           = false;
static uint8_t  s_alarm_reason      = JAMMING_REASON_OK;
static uint32_t s_last_alert_ms     = 0;

// ============================================================================
// Internal helpers — no dynamic memory
// ============================================================================

// Integer square-root (Babylonian), safe for values up to ~65535
static uint16_t isqrt(uint32_t n) {
    if (n == 0) return 0;
    uint32_t x = n;
    uint32_t y = (x + 1) / 2;
    while (y < x) {
        x = y;
        y = (x + n / x) / 2;
    }
    return (uint16_t)x;
}

// Compute cross-channel RSSI standard deviation (integer dBm units)
// Uses only channels that have been observed at least once.
static uint16_t compute_rssi_stddev() {
    int32_t sum   = 0;
    uint8_t count = 0;
    for (uint8_t ch = 1; ch <= JAMMING_WIFI_CHANNEL_COUNT; ch++) {
        if (s_ch_seen[ch]) {
            sum += (int32_t)s_ch_rssi[ch];
            count++;
        }
    }
    if (count < 2) return 0;

    int32_t mean = sum / (int32_t)count;

    uint32_t variance = 0;
    for (uint8_t ch = 1; ch <= JAMMING_WIFI_CHANNEL_COUNT; ch++) {
        if (s_ch_seen[ch]) {
            int32_t diff = (int32_t)s_ch_rssi[ch] - mean;
            variance += (uint32_t)(diff * diff);
        }
    }
    variance /= (uint32_t)count;
    return isqrt(variance);
}

// Transmit the jamming alert mesh packet (one-shot, rate-limited by caller)
static void broadcast_jamming_alert(uint8_t reason) {
#ifdef ARDUINO
    uint32_t now_s = millis() / 1000u;
    uint32_t elapsed_s = now_s - (s_last_alert_ms / 1000u);
    if (elapsed_s > 0xFFFFu) elapsed_s = 0xFFFFu;

    uint8_t pkt[JAMMING_ALERT_PACKET_SIZE];
    pkt[0] = JAMMING_ALERT_MSG_TYPE;
    // sensor_id little-endian bytes 1..4
    uint32_t sid = DEFAULT_SENSOR_ID;
    pkt[1] = (uint8_t)(sid & 0xFF);
    pkt[2] = (uint8_t)((sid >> 8)  & 0xFF);
    pkt[3] = (uint8_t)((sid >> 16) & 0xFF);
    pkt[4] = (uint8_t)((sid >> 24) & 0xFF);
    pkt[5] = reason;
    pkt[6] = (uint8_t)(elapsed_s & 0xFF);
    pkt[7] = (uint8_t)((elapsed_s >> 8) & 0xFF);

    mesh_publish_raw(pkt, JAMMING_ALERT_PACKET_SIZE);
#else
    (void)reason;
#endif
}

// ============================================================================
// Public API
// ============================================================================

void jamming_init() {
    for (uint8_t i = 0; i < JAMMING_LOSS_WINDOW_SLOTS; i++) s_loss_window[i] = 0;
    s_loss_head         = 0;
    s_loss_count        = 0;
    s_loss_sum          = 0;

    for (uint8_t ch = 0; ch <= JAMMING_WIFI_CHANNEL_COUNT; ch++) {
        s_ch_rssi[ch] = -100;
        s_ch_seen[ch] = false;
    }

    s_baseline_rssi   = -100;
    s_baseline_seeded = false;
    s_alarmed         = false;
    s_alarm_reason    = JAMMING_REASON_OK;
    s_last_alert_ms   = 0;
}

void jamming_observe(uint8_t channel, int16_t rssi, bool packet_received_ok) {
    // ---- Channel B: update per-WiFi RSSI ----
    if (channel >= 1 && channel <= JAMMING_WIFI_CHANNEL_COUNT) {
        s_ch_rssi[channel] = rssi;
        s_ch_seen[channel] = true;
    }

    // ---- Channel C: noise-floor tracking ----
    // Snapshot the old baseline before updating so the jump check uses the
    // pre-update reference (avoids the baseline chasing the new value).
    int16_t prev_baseline = s_baseline_rssi;
    if (!s_baseline_seeded) {
        s_baseline_rssi   = rssi;
        s_baseline_seeded = true;
    } else {
        // Integer EMA: new_baseline = 0.95*old + 0.05*new
        // Multiply by 100 to keep integer precision
        int32_t old_100 = (int32_t)s_baseline_rssi * 100;
        int32_t new_100 = old_100 * 95 / 100 + (int32_t)rssi * 5;
        s_baseline_rssi  = (int16_t)(new_100 / 100);
    }

    // ---- Channel A: packet-loss window ----
    // Remove the oldest slot from the rolling sum
    s_loss_sum -= s_loss_window[s_loss_head];
    // Insert new observation
    uint8_t lost = packet_received_ok ? 0u : 1u;
    s_loss_window[s_loss_head] = lost;
    s_loss_sum += lost;
    s_loss_head = (uint8_t)((s_loss_head + 1) % JAMMING_LOSS_WINDOW_SLOTS);
    if (s_loss_count < 0xFFFFu) s_loss_count++;

    // ============================================================
    // Evaluate alarm conditions (only after window is seeded)
    // ============================================================
    if (s_loss_count < JAMMING_LOSS_WINDOW_SLOTS) {
        return;  // not enough data yet
    }

    uint8_t new_reason = JAMMING_REASON_OK;

    // Channel A — loss rate (percentage, 0..100)
    uint8_t loss_pct = (uint8_t)((uint16_t)s_loss_sum * 100u / JAMMING_LOSS_WINDOW_SLOTS);
    if (loss_pct >= JAMMING_LOSS_RATE_THRESHOLD) {
        new_reason = JAMMING_REASON_HIGH_LOSS;
    }

    // Channel B — spectrum std-dev
    if (new_reason == JAMMING_REASON_OK) {
        uint16_t stddev = compute_rssi_stddev();
        if (stddev >= JAMMING_RSSI_STDDEV_THRESHOLD) {
            new_reason = JAMMING_REASON_SPECTRUM_ANOMALY;
        }
    }

    // Channel C — noise-floor jump
    // Compare current RSSI against the pre-update baseline snapshot so the
    // EMA update does not dilute the detected step.
    if (new_reason == JAMMING_REASON_OK) {
        int16_t jump = (int16_t)(rssi - prev_baseline);
        if (jump >= JAMMING_NOISE_FLOOR_JUMP_DBM) {
            new_reason = JAMMING_REASON_NOISE_FLOOR_JUMP;
        }
    }

    if (new_reason != JAMMING_REASON_OK) {
        // Latch alarm state and rate-limit the mesh broadcast.
        // The alarm remains active for JAMMING_ALERT_COOLDOWN_S after the
        // last triggering event so that a single outlier observation does
        // not toggle the alarm on and off within the same cycle.
        uint32_t now_ms = millis();
        uint32_t elapsed_s = (now_ms - s_last_alert_ms) / 1000u;
        bool first_alert = (s_last_alert_ms == 0);

        if (first_alert || elapsed_s >= JAMMING_ALERT_COOLDOWN_S) {
            s_last_alert_ms = now_ms;
            broadcast_jamming_alert(new_reason);
        }
        // Always update latch even if broadcast is suppressed
        s_alarmed      = true;
        s_alarm_reason = new_reason;
    } else {
        // Only clear the alarm if the cooldown period has fully elapsed.
        // This prevents a single "clean" observation from clearing an alarm
        // that fired moments ago.
        uint32_t now_ms = millis();
        uint32_t elapsed_s = (now_ms - s_last_alert_ms) / 1000u;
        if (!s_alarmed || elapsed_s >= JAMMING_ALERT_COOLDOWN_S) {
            s_alarmed      = false;
            s_alarm_reason = JAMMING_REASON_OK;
        }
        // else: remain alarmed until cooldown expires
    }
}

bool jamming_is_alarmed(uint8_t* reason_code) {
    if (reason_code) {
        *reason_code = s_alarmed ? s_alarm_reason : JAMMING_REASON_OK;
    }
    return s_alarmed;
}
