"""Stability analysis for AC simulation loop gain data (gain/phase margins)."""

import numpy as np


def _interp_crossing(x: np.ndarray, y: np.ndarray, threshold: float) -> list[float]:
    """Find all x values where y crosses threshold, using linear interpolation."""
    crossings = []
    for i in range(len(y) - 1):
        if (y[i] - threshold) * (y[i + 1] - threshold) < 0:
            dy = y[i + 1] - y[i]
            if abs(dy) < 1e-30:
                crossings.append(float(x[i]))
            else:
                frac = (threshold - y[i]) / dy
                crossings.append(float(x[i] + frac * (x[i + 1] - x[i])))
    return crossings


def _interp_y_at_crossing(
    x: np.ndarray, y: np.ndarray, ref: np.ndarray, threshold: float
) -> list[tuple[float, float]]:
    """Find interpolated (x, y) pairs where ref crosses threshold."""
    results = []
    for i in range(len(ref) - 1):
        if (ref[i] - threshold) * (ref[i + 1] - threshold) < 0:
            dref = ref[i + 1] - ref[i]
            if abs(dref) < 1e-30:
                results.append((float(x[i]), float(y[i])))
            else:
                frac = (threshold - ref[i]) / dref
                xi = float(x[i] + frac * (x[i + 1] - x[i]))
                yi = float(y[i] + frac * (y[i + 1] - y[i]))
                results.append((xi, yi))
    return results


def compute_gain_margin(frequency: np.ndarray, loop_gain_complex: np.ndarray) -> dict:
    """Compute gain margin from loop gain T(s).

    Args:
        frequency: Frequency array in Hz (real-valued)
        loop_gain_complex: Complex loop gain T(jw)

    Returns:
        Dict with gain_margin_db, phase_crossover_freq_hz, is_stable
    """
    if len(frequency) < 2 or len(loop_gain_complex) < 2:
        return {
            "gain_margin_db": None,
            "phase_crossover_freq_hz": None,
            "is_stable": None,
        }

    freq = np.real(frequency).astype(np.float64)
    mag_db = 20.0 * np.log10(np.maximum(np.abs(loop_gain_complex), 1e-30))
    phase_deg = np.degrees(np.unwrap(np.angle(loop_gain_complex)))

    # Phase crossover: where phase crosses -180 degrees
    # Find magnitude at that crossing via interpolation
    hits = _interp_y_at_crossing(freq, mag_db, phase_deg, -180.0)

    if not hits:
        # No phase crossover => infinite gain margin (stable for any gain)
        return {
            "gain_margin_db": float("inf"),
            "phase_crossover_freq_hz": None,
            "is_stable": True,
        }

    # Use the first phase crossover
    crossover_freq, mag_at_crossover = hits[0]
    gain_margin = -mag_at_crossover

    return {
        "gain_margin_db": float(gain_margin),
        "phase_crossover_freq_hz": float(crossover_freq),
        "is_stable": gain_margin > 0,
    }


def compute_phase_margin(frequency: np.ndarray, loop_gain_complex: np.ndarray) -> dict:
    """Compute phase margin from loop gain T(s).

    Args:
        frequency: Frequency array in Hz (real-valued)
        loop_gain_complex: Complex loop gain T(jw)

    Returns:
        Dict with phase_margin_deg, gain_crossover_freq_hz, is_stable
    """
    if len(frequency) < 2 or len(loop_gain_complex) < 2:
        return {
            "phase_margin_deg": None,
            "gain_crossover_freq_hz": None,
            "is_stable": None,
        }

    freq = np.real(frequency).astype(np.float64)
    mag_db = 20.0 * np.log10(np.maximum(np.abs(loop_gain_complex), 1e-30))
    phase_deg = np.degrees(np.unwrap(np.angle(loop_gain_complex)))

    # Gain crossover: where magnitude crosses 0 dB
    # Find phase at that crossing via interpolation
    hits = _interp_y_at_crossing(freq, phase_deg, mag_db, 0.0)

    if not hits:
        # No gain crossover. If gain is always below 0 dB, system is stable.
        is_stable = bool(np.all(mag_db < 0))
        return {
            "phase_margin_deg": float("inf") if is_stable else None,
            "gain_crossover_freq_hz": None,
            "is_stable": is_stable,
        }

    # Use the first gain crossover
    crossover_freq, phase_at_crossover = hits[0]
    phase_margin = 180.0 + phase_at_crossover

    return {
        "phase_margin_deg": float(phase_margin),
        "gain_crossover_freq_hz": float(crossover_freq),
        "is_stable": phase_margin > 0,
    }


def compute_stability_metrics(frequency: np.ndarray, loop_gain_complex: np.ndarray) -> dict:
    """Compute comprehensive stability metrics including Bode plot data.

    Args:
        frequency: Frequency array in Hz (real-valued)
        loop_gain_complex: Complex loop gain T(jw)

    Returns:
        Dict with gain_margin, phase_margin, bode data, crossover
        frequencies, and overall stability assessment
    """
    if len(frequency) < 2 or len(loop_gain_complex) < 2:
        return {
            "gain_margin": compute_gain_margin(frequency, loop_gain_complex),
            "phase_margin": compute_phase_margin(frequency, loop_gain_complex),
            "bode": {"frequency_hz": [], "magnitude_db": [], "phase_deg": []},
            "is_stable": None,
        }

    freq = np.real(frequency).astype(np.float64)
    mag_db = 20.0 * np.log10(np.maximum(np.abs(loop_gain_complex), 1e-30))
    phase_deg = np.degrees(np.unwrap(np.angle(loop_gain_complex)))

    gm = compute_gain_margin(frequency, loop_gain_complex)
    pm = compute_phase_margin(frequency, loop_gain_complex)

    # Overall stability: both margins must be positive (when defined)
    gm_ok = gm["is_stable"] is not False
    pm_ok = pm["is_stable"] is not False
    is_stable = gm_ok and pm_ok

    return {
        "gain_margin": gm,
        "phase_margin": pm,
        "bode": {
            "frequency_hz": freq.tolist(),
            "magnitude_db": mag_db.tolist(),
            "phase_deg": phase_deg.tolist(),
        },
        "is_stable": is_stable,
    }
