// ============================================================================
// radio_scheduler.cpp
// ============================================================================

#include "radio_scheduler.h"

#include <cstring>

#ifdef ARDUINO
#include <Arduino.h>
#include <nvs_flash.h>
#include <nvs.h>
#include "mesh_publish.h"   // for mesh_get_radio() / mesh_start_receive()
#include "config.h"
#endif

#ifndef ARDUINO
// --- Native test stubs ---
#include <cstdio>
static uint32_t s_virt_ms = 0;
static uint32_t millis_native() { return s_virt_ms; }
extern "C" void radio_sched_test_advance_ms(uint32_t ms) { s_virt_ms += ms; }
#define MILLIS() millis_native()
#else
#define MILLIS() millis()
#endif

static const char* TAG_SCHED = "RadioSched";

// --- Persistent state ---
static uint8_t   s_pdl_pct   = RADIO_SCHED_DEFAULT_PDL_PCT;
static uint8_t   s_mesh_pct  = RADIO_SCHED_DEFAULT_MESH_PCT;
static uint8_t   s_rnode_pct = RADIO_SCHED_DEFAULT_RNODE_PCT;

static RadioMode s_current_mode = RADIO_MODE_PDL;
static uint32_t  s_current_dwell_ms = RADIO_SCHED_PERIOD_MS;
static uint32_t  s_window_started_at_ms = 0;

static bool      s_lease_active = false;
static RadioMode s_lease_mode   = RADIO_MODE_PDL;
static uint32_t  s_lease_until_ms = 0;

static RadioSchedStats s_stats;
static uint32_t  s_last_tick_ms = 0;
static bool      s_initialised = false;

// ---------------------------------------------------------------------------
// NVS load (Arduino only)
// ---------------------------------------------------------------------------
static void load_weights_from_nvs() {
#ifdef ARDUINO
    nvs_handle_t h;
    if (nvs_open(RADIO_SCHED_NVS_NS, NVS_READONLY, &h) != ESP_OK) {
        return;
    }
    uint8_t v = 0;
    if (nvs_get_u8(h, RADIO_SCHED_NVS_PDL,  &v) == ESP_OK) s_pdl_pct   = v;
    if (nvs_get_u8(h, RADIO_SCHED_NVS_MESH, &v) == ESP_OK) s_mesh_pct  = v;
    if (nvs_get_u8(h, RADIO_SCHED_NVS_RN,   &v) == ESP_OK) s_rnode_pct = v;
    nvs_close(h);
#endif
}

static void validate_weights() {
    int sum = (int)s_pdl_pct + (int)s_mesh_pct + (int)s_rnode_pct;
    if (sum != 100) {
#ifdef ARDUINO
        Serial.printf("[%s] weights sum=%d != 100, falling back to 100/0/0\n",
                      TAG_SCHED, sum);
#endif
        s_pdl_pct = 100;
        s_mesh_pct = 0;
        s_rnode_pct = 0;
    }
}

// ---------------------------------------------------------------------------
// Compute dwell-ms for a given mode (under current weights)
// ---------------------------------------------------------------------------
static uint32_t dwell_for(RadioMode m) {
    uint32_t period = RADIO_SCHED_PERIOD_MS;
    switch (m) {
        case RADIO_MODE_PDL:        return (period * s_pdl_pct)   / 100;
        case RADIO_MODE_MESHTASTIC: return (period * s_mesh_pct)  / 100;
        case RADIO_MODE_RNODE:      return (period * s_rnode_pct) / 100;
        default:                    return 0;
    }
}

// ---------------------------------------------------------------------------
// Pick next mode in round-robin order, skipping any with 0% weight
// ---------------------------------------------------------------------------
static RadioMode next_mode_after(RadioMode m) {
    for (int i = 1; i <= 3; i++) {
        RadioMode candidate = (RadioMode)(((int)m + i) % 3);
        if (dwell_for(candidate) > 0) return candidate;
    }
    return RADIO_MODE_PDL;
}

// ---------------------------------------------------------------------------
// Drain TX path then reconfigure the radio for the new mode
// ---------------------------------------------------------------------------
static void apply_mode_change(RadioMode new_mode) {
    if (new_mode == s_current_mode) return;

#ifdef ARDUINO
    // Drain by polling RadioLib's TX-busy state via standby() — simplest
    // correct drain on SX1262. A real TX queue would loop here; for the v1
    // we trust startTransmit() callers to await completion.
    SX1262& radio = mesh_get_radio();
    int st = radio.standby();
    if (st != RADIOLIB_ERR_NONE) {
        s_stats.reconfig_failures++;
        Serial.printf("[%s] standby() failed code=%d\n", TAG_SCHED, st);
    }
#endif

    int rc = 0;
    switch (new_mode) {
        case RADIO_MODE_PDL:        rc = radio_pdl_set_params();   break;
        case RADIO_MODE_MESHTASTIC: rc = radio_mesh_set_params();  break;
        case RADIO_MODE_RNODE:      rc = radio_rnode_set_params(); break;
        case RADIO_MODE_OFF:        rc = radio_off_set_params();   break;
        default: break;
    }
    if (rc != 0) {
        s_stats.reconfig_failures++;
#ifdef ARDUINO
        Serial.printf("[%s] reconfigure to mode %d failed rc=%d\n",
                      TAG_SCHED, (int)new_mode, rc);
#endif
    }

#ifdef ARDUINO
    // Re-arm RX so the new protocol can listen during its window
    if (new_mode != RADIO_MODE_OFF) {
        mesh_start_receive();
    }
#endif

    s_stats.mode_switches++;
    s_current_mode = new_mode;
    s_current_dwell_ms = dwell_for(new_mode);
    if (s_current_dwell_ms == 0) s_current_dwell_ms = 1; // avoid div-by-0
    s_window_started_at_ms = MILLIS();
}

// ---------------------------------------------------------------------------
// Public API
// ---------------------------------------------------------------------------
void radio_sched_init() {
    memset(&s_stats, 0, sizeof(s_stats));
    s_pdl_pct   = RADIO_SCHED_DEFAULT_PDL_PCT;
    s_mesh_pct  = RADIO_SCHED_DEFAULT_MESH_PCT;
    s_rnode_pct = RADIO_SCHED_DEFAULT_RNODE_PCT;
    load_weights_from_nvs();
    validate_weights();

    s_current_mode = RADIO_MODE_PDL;
    s_current_dwell_ms = dwell_for(RADIO_MODE_PDL);
    s_window_started_at_ms = MILLIS();
    s_last_tick_ms = s_window_started_at_ms;
    s_lease_active = false;
    s_initialised = true;

#ifdef ARDUINO
    Serial.printf("[%s] init weights pdl=%u mesh=%u rnode=%u\n",
                  TAG_SCHED, s_pdl_pct, s_mesh_pct, s_rnode_pct);
#endif
}

void radio_sched_reload_weights() {
    s_pdl_pct   = RADIO_SCHED_DEFAULT_PDL_PCT;
    s_mesh_pct  = RADIO_SCHED_DEFAULT_MESH_PCT;
    s_rnode_pct = RADIO_SCHED_DEFAULT_RNODE_PCT;
    load_weights_from_nvs();
    validate_weights();
}

void radio_sched_tick() {
    if (!s_initialised) return;

    uint32_t now = MILLIS();

    // Track time-in-mode
    uint32_t elapsed = now - s_last_tick_ms;
    if (s_current_mode < RADIO_MODE_COUNT) {
        s_stats.mode_ms[s_current_mode] += elapsed;
    }
    s_last_tick_ms = now;

    // 1) Active lease takes precedence
    if (s_lease_active) {
        if ((int32_t)(now - s_lease_until_ms) >= 0) {
            // lease expired — return to round-robin starting at PDL
            s_lease_active = false;
            apply_mode_change(RADIO_MODE_PDL);
        } else if (s_current_mode != s_lease_mode) {
            apply_mode_change(s_lease_mode);
        }
        return;
    }

    // 2) Round-robin window expiry
    uint32_t window_age = now - s_window_started_at_ms;
    if (window_age >= s_current_dwell_ms) {
        RadioMode nxt = next_mode_after(s_current_mode);
        apply_mode_change(nxt);
    }
}

bool radio_sched_request_mode(RadioMode mode, uint32_t duration_ms) {
    if (!s_initialised) return false;
    if (mode >= RADIO_MODE_COUNT) return false;
    if (s_lease_active) return false;

    s_lease_mode = mode;
    s_lease_until_ms = MILLIS() + duration_ms;
    s_lease_active = true;
    apply_mode_change(mode);
    return true;
}

RadioMode radio_sched_current_mode() {
    return s_current_mode;
}

bool radio_sched_can_tx(RadioMode mode) {
    return s_initialised && (s_current_mode == mode);
}

void radio_sched_get_stats(RadioSchedStats* out) {
    if (!out) return;
    *out = s_stats;
}

// ---------------------------------------------------------------------------
// PDL params (default: matches mesh_publish.cpp begin() values)
// ---------------------------------------------------------------------------
int radio_pdl_set_params() {
#ifdef ARDUINO
    SX1262& r = mesh_get_radio();
    int st;
    st = r.setFrequency(LORA_FREQ_MHZ);                 if (st) return st;
    st = r.setBandwidth(LORA_BANDWIDTH_KHZ);            if (st) return st;
    st = r.setSpreadingFactor(LORA_SPREAD_FACTOR);      if (st) return st;
    st = r.setCodingRate(LORA_CODING_RATE);             if (st) return st;
    st = r.setSyncWord(LORA_SYNC_WORD);                 if (st) return st;
    st = r.setPreambleLength(LORA_PREAMBLE_LEN);        if (st) return st;
    return 0;
#else
    return 0;
#endif
}

// Weak defaults — overridden by meshtastic_emit.cpp / rnode_bridge.cpp
// when the interop modules are compiled in.  Skipped in unit-test builds
// since test_interop pulls all four .cpp files into a single TU and the
// protocol-side strong definitions take their place.
#ifndef UNIT_TEST
__attribute__((weak)) int radio_mesh_set_params() {
#ifdef ARDUINO
    Serial.printf("[%s] mesh_set_params stub (interop not compiled in)\n",
                  TAG_SCHED);
#endif
    return 0;
}
__attribute__((weak)) int radio_rnode_set_params() {
#ifdef ARDUINO
    Serial.printf("[%s] rnode_set_params stub (interop not compiled in)\n",
                  TAG_SCHED);
#endif
    return 0;
}
#endif

int radio_off_set_params() {
#ifdef ARDUINO
    SX1262& r = mesh_get_radio();
    return r.sleep();
#else
    return 0;
#endif
}

// ---------------------------------------------------------------------------
// Native test helpers
// ---------------------------------------------------------------------------
#ifdef UNIT_TEST
extern "C" void radio_sched_test_reset() {
    s_virt_ms = 0;
    s_initialised = false;
    s_lease_active = false;
    memset(&s_stats, 0, sizeof(s_stats));
}
extern "C" void radio_sched_test_set_weights(uint8_t pdl, uint8_t mesh, uint8_t rnode) {
    s_pdl_pct = pdl;
    s_mesh_pct = mesh;
    s_rnode_pct = rnode;
    validate_weights();
    s_current_dwell_ms = dwell_for(s_current_mode);
}
#endif
