//! Coherence gating for environment stability (ADR-031).
//!
//! Phase coherence determines whether the wireless environment is sufficiently
//! stable for a model update. When multipath conditions change rapidly (e.g.
//! doors opening, people entering), phase becomes incoherent and fusion
//! quality degrades. The coherence gate prevents model updates during these
//! transient periods.
//!
//! The core computation is the complex mean of unit phasors:
//!
//! ```text
//! coherence = |mean(exp(j * delta_phi))|
//!           = sqrt((mean(cos(delta_phi)))^2 + (mean(sin(delta_phi)))^2)
//! ```
//!
//! A coherence value near 1.0 indicates consistent phase; near 0.0 indicates
//! random phase (incoherent environment).

// ---------------------------------------------------------------------------
// CoherenceState
// ---------------------------------------------------------------------------

/// Rolling coherence state tracking phase consistency over a sliding window.
///
/// Maintains a circular buffer of phase differences and incrementally updates
/// the coherence estimate as new measurements arrive.
#[derive(Debug, Clone)]
pub struct CoherenceState {
    /// Circular buffer of phase differences (radians).
    phase_diffs: Vec<f32>,
    /// Write position in the circular buffer.
    write_pos: usize,
    /// Number of valid entries in the buffer (may be less than capacity
    /// during warm-up).
    count: usize,
    /// Running sum of cos(phase_diff).
    sum_cos: f64,
    /// Running sum of sin(phase_diff).
    sum_sin: f64,
}

impl CoherenceState {
    /// Create a new coherence state with the given window size.
    ///
    /// # Arguments
    ///
    /// - `window_size`: number of phase measurements to retain. Larger windows
    ///   are more stable but respond more slowly to environment changes.
    ///   Must be at least 1.
    pub fn new(window_size: usize) -> Self {
        let size = window_size.max(1);
        CoherenceState {
            phase_diffs: vec![0.0; size],
            write_pos: 0,
            count: 0,
            sum_cos: 0.0,
            sum_sin: 0.0,
        }
    }

    /// Push a new phase difference measurement into the rolling window.
    ///
    /// If the buffer is full, the oldest measurement is evicted and its
    /// contribution is subtracted from the running sums.
    pub fn push(&mut self, phase_diff: f32) {
        let cap = self.phase_diffs.len();

        // If buffer is full, subtract the evicted entry.
        if self.count == cap {
            let old = self.phase_diffs[self.write_pos];
            self.sum_cos -= old.cos() as f64;
            self.sum_sin -= old.sin() as f64;
        } else {
            self.count += 1;
        }

        // Write new entry.
        self.phase_diffs[self.write_pos] = phase_diff;
        self.sum_cos += phase_diff.cos() as f64;
        self.sum_sin += phase_diff.sin() as f64;

        self.write_pos = (self.write_pos + 1) % cap;
    }

    /// Current coherence value in `[0, 1]`.
    ///
    /// Returns 0.0 if no measurements have been pushed yet.
    pub fn coherence(&self) -> f32 {
        if self.count == 0 {
            return 0.0;
        }
        let n = self.count as f64;
        let mean_cos = self.sum_cos / n;
        let mean_sin = self.sum_sin / n;
        (mean_cos * mean_cos + mean_sin * mean_sin).sqrt() as f32
    }

    /// Number of measurements currently in the buffer.
    pub fn len(&self) -> usize {
        self.count
    }

    /// Returns `true` if no measurements have been pushed.
    pub fn is_empty(&self) -> bool {
        self.count == 0
    }

    /// Window capacity.
    pub fn capacity(&self) -> usize {
        self.phase_diffs.len()
    }

    /// Reset the coherence state, clearing all measurements.
    pub fn reset(&mut self) {
        self.write_pos = 0;
        self.count = 0;
        self.sum_cos = 0.0;
        self.sum_sin = 0.0;
    }
}

// ---------------------------------------------------------------------------
// CoherenceGate
// ---------------------------------------------------------------------------

/// Coherence gate that controls model updates based on phase stability.
///
/// Only allows model updates when the coherence exceeds a configurable
/// threshold. Provides hysteresis to avoid rapid gate toggling near the
/// threshold boundary.
#[derive(Debug, Clone)]
pub struct CoherenceGate {
    /// Coherence threshold for opening the gate.
    pub threshold: f32,
    /// Hysteresis band: gate opens at `threshold` and closes at
    /// `threshold - hysteresis`.
    pub hysteresis: f32,
    /// Current gate state: `true` = open (updates allowed).
    gate_open: bool,
    /// Total number of gate evaluations.
    total_evaluations: u64,
    /// Number of times the gate was open.
    open_count: u64,
}

impl CoherenceGate {
    /// Create a new coherence gate with the given threshold.
    ///
    /// # Arguments
    ///
    /// - `threshold`: coherence level required for the gate to open (typically 0.7).
    /// - `hysteresis`: band below the threshold where the gate stays in its
    ///   current state (typically 0.05).
    pub fn new(threshold: f32, hysteresis: f32) -> Self {
        CoherenceGate {
            threshold: threshold.clamp(0.0, 1.0),
            hysteresis: hysteresis.clamp(0.0, threshold),
            gate_open: false,
            total_evaluations: 0,
            open_count: 0,
        }
    }

    /// Create a gate with default parameters (threshold=0.7, hysteresis=0.05).
    pub fn default_params() -> Self {
        Self::new(0.7, 0.05)
    }

    /// Evaluate the gate against the current coherence value.
    ///
    /// Returns `true` if the gate is open (model update allowed).
    pub fn evaluate(&mut self, coherence: f32) -> bool {
        self.total_evaluations += 1;

        if self.gate_open {
            // Gate is open: close if coherence drops below threshold - hysteresis.
            if coherence < self.threshold - self.hysteresis {
                self.gate_open = false;
            }
        } else {
            // Gate is closed: open if coherence exceeds threshold.
            if coherence >= self.threshold {
                self.gate_open = true;
            }
        }

        if self.gate_open {
            self.open_count += 1;
        }

        self.gate_open
    }

    /// Whether the gate is currently open.
    pub fn is_open(&self) -> bool {
        self.gate_open
    }

    /// Fraction of evaluations where the gate was open.
    pub fn duty_cycle(&self) -> f32 {
        if self.total_evaluations == 0 {
            return 0.0;
        }
        self.open_count as f32 / self.total_evaluations as f32
    }

    /// Reset the gate state and counters.
    pub fn reset(&mut self) {
        self.gate_open = false;
        self.total_evaluations = 0;
        self.open_count = 0;
    }
}

// ---------------------------------------------------------------------------
// ADR-138 — Clock-quality gate (coherence × clock dispersion/age)
// ---------------------------------------------------------------------------

/// Per-node clock-synchronisation quality (ADR-138 §2.2), derived from the
/// ADR-110 802.15.4 time-sync follower offset statistics.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ClockQualityScore {
    /// EMA-smoothed follower offset standard deviation (µs). ADR-110 measured
    /// ~104 µs against the ±100 µs target on COM9↔COM12.
    pub offset_stdev_us: f32,
    /// Age of the most recent sync packet (µs). The sensing server enforces a
    /// 9 s staleness ceiling.
    pub age_us: u64,
    /// Whether a valid sync has ever been observed for this node.
    pub valid: bool,
}

impl ClockQualityScore {
    /// Scalar clock quality in `[0, 1]` (1 = perfectly synced). Reaches 0 at
    /// `5 × max_offset_stdev_us`; used to bias directional attention weights.
    #[must_use]
    pub fn quality(&self, max_offset_stdev_us: f32) -> f32 {
        if !self.valid || max_offset_stdev_us <= 0.0 {
            return 0.0;
        }
        (1.0 - self.offset_stdev_us / (5.0 * max_offset_stdev_us)).clamp(0.0, 1.0)
    }
}

/// Why a node failed the clock-quality gate hard.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ClockRejectReason {
    /// Phase coherence below the gate threshold.
    Incoherent,
    /// Sync packet older than the staleness ceiling.
    ClockStale,
    /// Offset dispersion far beyond the floor (≥ 5× the monitor threshold).
    ClockDispersed,
    /// No valid sync ever observed for this node.
    ClockInvalid,
}

/// One node's gate decision for one sensing cycle (ADR-138 §2.2).
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum ClockGateDecision {
    /// Both terms pass: node admitted at full weight.
    Admit,
    /// Phase OK but clock degraded: evidence-only, NO environment/model update.
    MonitorOnly {
        /// Combined clock-quality score in [0, 1] (dispersion × age terms).
        clock_quality: f32,
    },
    /// Either term fails hard: node excluded this cycle.
    Reject {
        /// Which hard term failed (phase, dispersion, or age).
        reason: ClockRejectReason,
    },
}

/// Clock-quality gate: combines the phase [`CoherenceGate`] with clock
/// dispersion and age terms (ADR-138 §2.2).
#[derive(Debug, Clone)]
pub struct ClockQualityGate {
    /// Phase-coherence gate (threshold + hysteresis).
    pub coherence: CoherenceGate,
    /// Offset-stdev floor (µs): at or above ⇒ `MonitorOnly`. Default 200.0.
    pub max_offset_stdev_us: f32,
    /// Sync-age ceiling (µs): above ⇒ hard reject. Default 9_000_000.
    pub max_age_us: u64,
}

impl ClockQualityGate {
    /// Construct from a phase gate and the two clock thresholds.
    pub fn new(coherence: CoherenceGate, max_offset_stdev_us: f32, max_age_us: u64) -> Self {
        Self { coherence, max_offset_stdev_us, max_age_us }
    }

    /// Defaults: phase gate 0.7/0.05, 200 µs floor, 9 s staleness ceiling.
    pub fn default_params() -> Self {
        Self::new(CoherenceGate::default_params(), 200.0, 9_000_000)
    }

    /// Evaluate both terms for one node this cycle. `coherence_value` is the
    /// rolling phasor coherence ([`CoherenceState::coherence`]).
    pub fn evaluate(&mut self, coherence_value: f32, clock: &ClockQualityScore) -> ClockGateDecision {
        if !clock.valid {
            return ClockGateDecision::Reject { reason: ClockRejectReason::ClockInvalid };
        }
        if clock.age_us > self.max_age_us {
            return ClockGateDecision::Reject { reason: ClockRejectReason::ClockStale };
        }
        if clock.offset_stdev_us >= 5.0 * self.max_offset_stdev_us {
            return ClockGateDecision::Reject { reason: ClockRejectReason::ClockDispersed };
        }
        // Phase term (hysteretic). Clock-degraded but coherent ⇒ MonitorOnly.
        if !self.coherence.evaluate(coherence_value) {
            return ClockGateDecision::Reject { reason: ClockRejectReason::Incoherent };
        }
        if clock.offset_stdev_us >= self.max_offset_stdev_us {
            ClockGateDecision::MonitorOnly {
                clock_quality: clock.quality(self.max_offset_stdev_us),
            }
        } else {
            ClockGateDecision::Admit
        }
    }
}

/// Stateless coherence gate function matching the ADR-031 specification.
///
/// Computes the complex mean of unit phasors from the given phase differences
/// and returns `true` when coherence exceeds the threshold.
///
/// # Arguments
///
/// - `phase_diffs`: delta-phi over T recent frames (radians).
/// - `threshold`: coherence threshold (typically 0.7).
///
/// # Returns
///
/// `true` if the phase coherence exceeds the threshold.
pub fn coherence_gate(phase_diffs: &[f32], threshold: f32) -> bool {
    if phase_diffs.is_empty() {
        return false;
    }
    let (sum_cos, sum_sin) = phase_diffs.iter().fold((0.0_f32, 0.0_f32), |(c, s), &dp| {
        (c + dp.cos(), s + dp.sin())
    });
    let n = phase_diffs.len() as f32;
    let coherence = ((sum_cos / n).powi(2) + (sum_sin / n).powi(2)).sqrt();
    coherence > threshold
}

/// Compute the raw coherence value from phase differences.
///
/// Returns a value in `[0, 1]` where 1.0 = perfectly coherent phase.
pub fn compute_coherence(phase_diffs: &[f32]) -> f32 {
    if phase_diffs.is_empty() {
        return 0.0;
    }
    let (sum_cos, sum_sin) = phase_diffs.iter().fold((0.0_f32, 0.0_f32), |(c, s), &dp| {
        (c + dp.cos(), s + dp.sin())
    });
    let n = phase_diffs.len() as f32;
    ((sum_cos / n).powi(2) + (sum_sin / n).powi(2)).sqrt()
}

// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn coherent_phase_returns_high_value() {
        // All phase diffs are the same -> coherence ~ 1.0
        let phase_diffs = vec![0.5_f32; 100];
        let c = compute_coherence(&phase_diffs);
        assert!(
            c > 0.99,
            "identical phases should give coherence ~ 1.0, got {c}"
        );
    }

    #[test]
    fn random_phase_returns_low_value() {
        // Uniformly spaced phases around the circle -> coherence ~ 0.0
        let n = 1000;
        let phase_diffs: Vec<f32> = (0..n)
            .map(|i| 2.0 * std::f32::consts::PI * i as f32 / n as f32)
            .collect();
        let c = compute_coherence(&phase_diffs);
        assert!(
            c < 0.05,
            "uniformly spread phases should give coherence ~ 0.0, got {c}"
        );
    }

    #[test]
    fn coherence_gate_opens_above_threshold() {
        let coherent = vec![0.3_f32; 50]; // same phase -> high coherence
        assert!(coherence_gate(&coherent, 0.7));
    }

    #[test]
    fn coherence_gate_closed_below_threshold() {
        let n = 500;
        let incoherent: Vec<f32> = (0..n)
            .map(|i| 2.0 * std::f32::consts::PI * i as f32 / n as f32)
            .collect();
        assert!(!coherence_gate(&incoherent, 0.7));
    }

    #[test]
    fn coherence_gate_empty_returns_false() {
        assert!(!coherence_gate(&[], 0.5));
    }

    #[test]
    fn coherence_state_rolling_window() {
        let mut state = CoherenceState::new(10);
        // Push coherent measurements.
        for _ in 0..10 {
            state.push(1.0);
        }
        let c1 = state.coherence();
        assert!(c1 > 0.9, "coherent window should give high coherence");

        // Push incoherent measurements to replace the window.
        for i in 0..10 {
            state.push(i as f32 * 0.628);
        }
        let c2 = state.coherence();
        assert!(c2 < c1, "incoherent updates should reduce coherence");
    }

    #[test]
    fn coherence_state_empty_returns_zero() {
        let state = CoherenceState::new(10);
        assert_eq!(state.coherence(), 0.0);
        assert!(state.is_empty());
    }

    #[test]
    fn gate_hysteresis_prevents_toggling() {
        let mut gate = CoherenceGate::new(0.7, 0.1);
        // Open the gate.
        assert!(gate.evaluate(0.8));
        assert!(gate.is_open());

        // Coherence drops to 0.65 (below threshold but within hysteresis band).
        assert!(gate.evaluate(0.65));
        assert!(
            gate.is_open(),
            "gate should stay open within hysteresis band"
        );

        // Coherence drops below hysteresis boundary (0.7 - 0.1 = 0.6).
        assert!(!gate.evaluate(0.55));
        assert!(
            !gate.is_open(),
            "gate should close below hysteresis boundary"
        );
    }

    #[test]
    fn gate_duty_cycle_tracks_correctly() {
        let mut gate = CoherenceGate::new(0.5, 0.0);
        gate.evaluate(0.6); // open
        gate.evaluate(0.6); // open
        gate.evaluate(0.3); // close
        gate.evaluate(0.3); // close
        let duty = gate.duty_cycle();
        assert!(
            (duty - 0.5).abs() < 1e-5,
            "duty cycle should be 0.5, got {duty}"
        );
    }

    #[test]
    fn gate_reset_clears_state() {
        let mut gate = CoherenceGate::new(0.5, 0.0);
        gate.evaluate(0.6);
        assert!(gate.is_open());
        gate.reset();
        assert!(!gate.is_open());
        assert_eq!(gate.duty_cycle(), 0.0);
    }

    #[test]
    fn coherence_state_push_and_len() {
        let mut state = CoherenceState::new(5);
        assert_eq!(state.len(), 0);
        state.push(0.1);
        state.push(0.2);
        assert_eq!(state.len(), 2);
        // Fill past capacity.
        for i in 0..10 {
            state.push(i as f32 * 0.1);
        }
        assert_eq!(state.len(), 5, "count should be capped at window size");
    }

    // ===== ADR-138 clock-quality gate =====

    #[test]
    fn clock_gate_invalid_rejected() {
        let mut g = ClockQualityGate::default_params();
        let c = ClockQualityScore { offset_stdev_us: 10.0, age_us: 0, valid: false };
        assert_eq!(
            g.evaluate(0.9, &c),
            ClockGateDecision::Reject { reason: ClockRejectReason::ClockInvalid }
        );
    }

    #[test]
    fn clock_gate_dispersed_rejected() {
        let mut g = ClockQualityGate::default_params(); // floor 200 → 5× = 1000 µs
        let c = ClockQualityScore { offset_stdev_us: 1500.0, age_us: 0, valid: true };
        assert_eq!(
            g.evaluate(0.9, &c),
            ClockGateDecision::Reject { reason: ClockRejectReason::ClockDispersed }
        );
    }

    #[test]
    fn clock_gate_admit_and_monitor_and_quality() {
        let mut g = ClockQualityGate::default_params();
        let good = ClockQualityScore { offset_stdev_us: 50.0, age_us: 0, valid: true };
        assert_eq!(g.evaluate(0.9, &good), ClockGateDecision::Admit);
        // quality: 1 - 50/(5*200) = 0.95
        assert!((good.quality(200.0) - 0.95).abs() < 1e-4);

        let mut g2 = ClockQualityGate::default_params();
        let degraded = ClockQualityScore { offset_stdev_us: 250.0, age_us: 0, valid: true };
        assert!(matches!(
            g2.evaluate(0.9, &degraded),
            ClockGateDecision::MonitorOnly { .. }
        ));
    }
}