"""Finite word-length effects: quantization, round-off noise, coefficient
sensitivity, and limit cycles in fixed-point digital filters.

The functions use a fractional fixed-point model: an ``n_bits`` quantizer
rounds to a uniform grid of step ``Delta = 2 * x_max / 2**n_bits`` over the
range ``[-x_max, x_max)``. This is the standard model for digital-filter
word-length analysis (Manolakis, Ingle and Kogon, *Applied Digital Signal
Processing*, Cambridge 2011, ch. 15).

Everything here is plain NumPy and importable; the topic page imports these
functions for its demos, and ``test_finite_wordlength.py`` exercises them.
"""

from __future__ import annotations

import numpy as np


def quantization_step(n_bits: int, x_max: float = 1.0) -> float:
    """Quantizer step ``Delta`` for an ``n_bits`` quantizer over ``[-x_max, x_max)``."""
    return 2.0 * x_max / 2 ** n_bits


def quantize(x, n_bits: int, x_max: float = 1.0):
    """Round ``x`` to an ``n_bits`` fixed-point grid over ``[-x_max, x_max)``.

    Uses round-to-nearest, then clips to the representable range. The rounding
    error is bounded by ``Delta / 2``.
    """
    step = quantization_step(n_bits, x_max)
    xq = np.round(np.asarray(x, dtype=float) / step) * step
    return np.clip(xq, -x_max, x_max - step)


def sqnr_db(x, x_q) -> float:
    """Signal-to-quantization-noise ratio in dB: ``10 log10(P_signal / P_error)``."""
    x = np.asarray(x, dtype=float)
    err = x - np.asarray(x_q, dtype=float)
    return 10.0 * np.log10(np.mean(x ** 2) / np.mean(err ** 2))


def theoretical_sqnr_db(n_bits: int) -> float:
    """Ideal SQNR of a full-scale sinusoid quantized to ``n_bits``: ``6.02 n + 1.76`` dB.

    Follows from a uniform error of variance ``Delta**2 / 12`` against a sine of
    power ``x_max**2 / 2`` that fills the converter range.
    """
    return 6.02 * n_bits + 1.76


def quantize_coeffs(coeffs, n_bits: int, c_max: float = 2.0):
    """Quantize filter coefficients to an ``n_bits`` grid spanning ``[-c_max, c_max)``.

    ``c_max`` must cover the coefficient range. Second-order-section coefficients
    fit in ``c_max = 2`` (since ``|a_1| < 2``); high-order direct-form denominators
    need a much larger ``c_max`` and a correspondingly coarser grid, which is the
    whole point of the sensitivity story.
    """
    return quantize(coeffs, n_bits, x_max=c_max)


def is_stable(a) -> bool:
    """True if every root of the denominator polynomial ``a`` is inside the unit circle."""
    return bool(np.all(np.abs(np.roots(a)) < 1.0))


def sos_is_stable(sos) -> bool:
    """True if every second-order section in an SOS matrix is stable."""
    return all(is_stable(section[3:6]) for section in np.atleast_2d(np.asarray(sos, dtype=float)))


def first_order_zero_input(a: float, y0: float, n_bits: int, n_samples: int,
                           x_max: float = 1.0):
    """Zero-input response of the rounding recursion ``y[n] = Q{ a * y[n-1] }``.

    With infinite precision this decays geometrically to zero. With a rounding
    quantizer in the loop it can instead lock into a sustained nonzero
    **zero-input limit cycle** (a dead band) when the pole ``a`` is close enough
    to the unit circle that rounding pulls ``a * y`` back to ``y``.
    """
    y = np.zeros(n_samples)
    y[0] = quantize(y0, n_bits, x_max)
    for n in range(1, n_samples):
        y[n] = quantize(a * y[n - 1], n_bits, x_max)
    return y


# ---------------------------------------------------------------------------
# Scaling by shifts: what an arithmetic right shift does that a divide does not
#
# These model C integer semantics, not Python's. Python's ``//`` floors (so it
# agrees with ``>>``) while C's ``/`` truncates toward zero, so a Python model
# of embedded C must spell the difference out rather than reach for ``//``.
# ---------------------------------------------------------------------------


def shift_scale(x, k: int, rounded: bool = False):
    """Scale integers down by ``2**k`` the way C's arithmetic right shift does.

    ``rounded=False`` is a bare ``x >> k``, which floors (rounds toward minus
    infinity). ``rounded=True`` adds half an output LSB first,
    ``(x + (1 << (k-1))) >> k``, which is round-half-up.

    Returns an integer array. ``k`` must be at least 1.
    """
    if k < 1:
        raise ValueError("k must be at least 1")
    xi = np.asarray(x, dtype=np.int64)
    if rounded:
        return (xi + (1 << (k - 1))) >> k
    return xi >> k


def c_div_pow2(x, k: int):
    """Scale integers down by ``2**k`` the way C's ``/`` does: truncate toward zero.

    Provided so the contrast with :func:`shift_scale` can be tested rather than
    asserted in prose. Not the same operation for negative operands.
    """
    if k < 1:
        raise ValueError("k must be at least 1")
    xi = np.asarray(x, dtype=np.int64)
    # Take the magnitude in uint64, never in int64. ``abs(INT64_MIN)`` is not
    # representable as a positive int64: it wraps back to INT64_MIN, silently,
    # and the sign of the result then flips. That is this section's own
    # "two's complement has no positive counterpart" trap, landing in the
    # function whose whole job is to be the faithful reference for C's ``/``.
    neg = xi < 0
    mag = np.where(neg, np.negative(xi.view(np.uint64)), xi.view(np.uint64))
    out = (mag >> np.uint64(k)).view(np.int64)
    return np.where(neg, -out, out)


def shift_scale_mean_error(k: int, rounded: bool = False) -> float:
    """Mean error of :func:`shift_scale`, in output LSBs, for input uniform mod ``2**k``.

    A bare shift discards the low ``k`` bits, so the error is uniform on
    ``{0, -1/2**k, ..., -(2**k - 1)/2**k}`` and its mean is

        ``-(2**k - 1) / 2**(k+1)``,

    which tends to ``-1/2`` LSB as ``k`` grows: a DC offset, not just noise.
    Adding the half-LSB bias makes ties round up instead, leaving

        ``+1 / 2**(k+1)``,

    which tends to zero. Neither is exactly unbiased; round-half-to-even is.
    """
    if k < 1:
        raise ValueError("k must be at least 1")
    if rounded:
        return 1.0 / (1 << (k + 1))
    return -((1 << k) - 1) / (1 << (k + 1))


def ema_shift(x, k: int, rounded: bool = False, y0: int = 0):
    """Integer exponential moving average using only a shift: ``y += (x - y) >> k``.

    The multiplier-free smoother that appears in almost every embedded codebase.
    It has a dead band: see :func:`ema_shift_final_value`.
    """
    xi = np.asarray(x, dtype=np.int64)
    y = int(y0)
    out = np.empty(xi.size, dtype=np.int64)
    half = (1 << (k - 1)) if rounded else 0
    for i, v in enumerate(xi):
        d = int(v) - y
        y += (d + half) >> k
        out[i] = y
    return out


def ema_shift_final_value(target: int, k: int, y0: int = 0,
                          rounded: bool = False) -> int:
    """Where :func:`ema_shift` settles for a constant input ``target``.

    The update stalls when ``(d + half) >> k`` is zero, where ``d = target - y``
    and ``half`` is ``2**(k-1)`` for the rounded form and 0 otherwise. That is
    zero exactly while ``-half <= d <= 2**k - 1 - half``, so the stall band on
    ``d`` is ``[lo, hi]`` and the state stops at the near edge of it.

    **The unrounded band is one-sided, and that is this page's own point turned
    on itself.** With ``half = 0`` the band is ``0 <= d <= 2**k - 1``, which
    contains no negative residual: an arithmetic shift floors, so ``d >> k`` is
    ``-1`` and never 0 for every ``d`` in ``[-2**k, -1]``. A shift-only EMA
    therefore stalls up to ``2**k - 1`` counts SHORT when climbing, and
    converges EXACTLY when falling. The rounding bias makes the band two-sided
    and the behaviour symmetric.
    """
    if k < 1:
        raise ValueError("k must be at least 1")
    half = (1 << (k - 1)) if rounded else 0
    lo, hi = -half, (1 << k) - 1 - half
    if target >= y0:
        return max(y0, target - hi)
    return min(y0, target - lo)


# ---------------------------------------------------------------------------
# Overflow oscillations, in state space
#
# Everything above treats the quantizer as a small additive error. Overflow is
# not small: the state leaves the representable range and the arithmetic folds
# it back, and the fold is what can sustain a large oscillation in a filter that
# is perfectly stable on paper. The state-space view is what makes this tractable
# -- the question becomes whether a quadratic form shrinks under the map
# ``x -> Q(A x)``, which depends on the REALIZATION (the matrix A) and not only
# on the transfer function (its eigenvalues).


def saturate(v, x_max: float = 1.0):
    """Saturating overflow: clamp to ``[-x_max, x_max]``.

    Satisfies ``|saturate(v)| <= |v|``, which is the only property the Lyapunov
    argument in :func:`lyapunov_margin` needs. It preserves sign too, but that is
    not what the argument uses -- :func:`wrap` does not, and the argument covers
    it just the same.
    """
    return np.clip(np.asarray(v, dtype=float), -x_max, x_max)


def wrap(v, x_max: float = 1.0):
    """Two's-complement overflow: wrap into ``[-x_max, x_max)``.

    What a fixed-point adder does if nothing stops it: a large positive sum
    reappears as a large negative state, which is how overflow turns a decaying
    filter into an oscillating one.

    It is worth being precise about WHY, because the obvious explanation is
    wrong. ``|wrap(v)| <= |v|`` is in fact TRUE everywhere -- the output is
    bounded by ``x_max`` and the map is the identity inside the range -- so
    wraparound satisfies the same componentwise bound as :func:`saturate` and
    :func:`lyapunov_margin` applies to it equally. That is why the state-space
    condition is usually stated for two's-complement arithmetic. What wraparound
    destroys is not the bound but CONTINUITY and sign: it sends a state just
    over the top of the range to the bottom of it, so a realization whose margin
    is not negative has a mechanism available that saturation does not give it.
    """
    return (np.asarray(v, dtype=float) + x_max) % (2 * x_max) - x_max


def companion_form(a):
    """State matrix of the direct-form realization of ``y[n] = sum a_k y[n-k]``.

    For order two and above the lower rows are a delay line, so ``A e_1`` is
    ``(a_1, 1, 0, ...)`` and therefore ``||A||_2 >= sqrt(1 + a_1**2) >= 1``,
    however deep inside the unit circle the poles sit. (Order 1 is the
    exception: ``A = [[a]]``, no delay row, ``||A||_2 = |a| < 1``.)

    **What that does and does not settle.** It means a direct form of order >= 2
    never satisfies :func:`lyapunov_margin` with ``D = I``. It does NOT mean no
    diagonal ``D`` works: ``a = [0.3, 0.3]`` has margin ``+0.098`` at ``D = I``
    and ``-0.320`` at ``D = diag(1, 0.5)``, and that filter indeed cannot
    oscillate. Certificates exist for direct forms with small enough
    coefficients and run out as the poles approach the unit circle; the pole
    pair at ``r = 0.95`` used on the page admits none (best margin over a
    6001-point diagonal search: ``+2.02``).
    """
    a = np.atleast_1d(np.asarray(a, dtype=float))
    n = a.size
    A = np.zeros((n, n))
    A[0, :] = a
    if n > 1:
        A[1:, :-1] = np.eye(n - 1)
    return A


def normal_form(r: float, theta: float):
    """The 2x2 normal (coupled) realization of a pole pair ``r * exp(+-j theta)``.

    ``A = r * R(theta)`` with ``R`` a rotation, so ``A.T @ A = r**2 * I`` and
    ``||A||_2 = r`` exactly -- the pole radius, for any theta. Compare
    :func:`companion_form`, whose norm grows without bound as the poles approach
    the real axis while describing the same filter.
    """
    c, s = np.cos(theta), np.sin(theta)
    return r * np.array([[c, -s], [s, c]])


def block_normal_form(poles):
    """A block-diagonal realization: 1x1 blocks for real poles, normal 2x2 blocks
    for conjugate pairs.

    ``||A||_2`` is then the largest pole radius, so the sufficient condition in
    :func:`lyapunov_margin` holds by construction rather than by luck. This is a
    parallel (block-diagonal) connection of minimum-norm sections, in the family
    of Barnes and Fam (1977) -- not a cascade, though the two are often named
    together.

    **Distinct poles only.** A repeated pole makes the companion form a Jordan
    block, and a block-diagonal construction cannot reproduce that: with poles
    ``(0.6, 0.6)`` this returns ``0.6 * I``, whose transfer function has a SIMPLE
    pole, so no choice of input and output vectors realizes ``1/(1 - 0.6 z^-1)**2``.
    The general claim survives -- scaling a Jordan block by ``diag(1, eps, ...)``
    gives ``||A||_2 <= |lambda| + eps < 1`` -- but by a different construction
    than this one.

    ``poles`` is a sequence of complex poles, conjugates included once each pair.
    """
    poles = list(np.asarray(poles, dtype=complex))
    blocks, used = [], [False] * len(poles)
    for i, p in enumerate(poles):
        if used[i]:
            continue
        if abs(p.imag) < 1e-12:
            used[i] = True
            blocks.append(np.array([[p.real]]))
            continue
        # pair it with its conjugate
        for j in range(i + 1, len(poles)):
            if not used[j] and abs(poles[j] - np.conj(p)) < 1e-9:
                used[i] = used[j] = True
                blocks.append(normal_form(abs(p), abs(np.angle(p))))
                break
        else:
            raise ValueError(f"pole {p} has no conjugate in the list")
    # Refuse rather than mislead: for a repeated pole the block-diagonal result
    # realizes a DIFFERENT filter (a simple pole where the original has a double
    # one), and it would pass an eigenvalue check while doing so.
    for i, p in enumerate(poles):
        for q in poles[i + 1:]:
            if abs(p - q) < 1e-9:
                raise ValueError(
                    f"repeated pole {p}: a block-diagonal realization cannot reproduce "
                    "the Jordan structure, so it would realize a different filter")
    out = np.zeros((sum(b.shape[0] for b in blocks),) * 2)
    k = 0
    for b in blocks:
        m = b.shape[0]
        out[k:k + m, k:k + m] = b
        k += m
    return out


def lyapunov_margin(A, d=None) -> float:
    """Largest eigenvalue of ``A.T @ D @ A - D`` for diagonal ``D`` (default ``I``).

    **Negative means no overflow oscillation is possible** under any arithmetic
    satisfying ``|Q(v)| <= |v|`` componentwise -- which is both :func:`saturate`
    and :func:`wrap`, and also magnitude truncation. The
    argument is one line: with ``V(x) = x.T @ D @ x`` and ``D`` diagonal and
    positive, ``V(Q(A x)) <= V(A x) <= V(x)``, strictly unless ``x`` is zero, so
    the state cannot circulate forever.

    ``D`` must be DIAGONAL. The overflow acts on each state component
    separately, so ``V`` has to be a sum of per-component terms for componentwise
    shrinkage to imply that ``V`` shrinks; a general positive-definite ``P``
    breaks the OVERFLOW step, and not marginally: with
    ``P = [[0.01, 0.99], [0.99, 100]]`` and ``v = (-1.5, 1.0)``, saturation
    shrinks both components and ``V`` still RISES, 97.05 to 98.03. Nor could a
    general ``P`` be useful if it did work -- every stable ``A`` solves
    ``A.T P A - P = -I`` for some positive definite ``P``, so the test would
    certify every stable filter and say nothing. Diagonality is what makes the
    condition stronger than ordinary stability rather than equivalent to it.

    The condition is SUFFICIENT, not necessary: a filter can fail it and still
    not oscillate. What it cannot do is hold while an oscillation exists.
    """
    A = np.asarray(A, dtype=float)
    D = np.eye(A.shape[0]) if d is None else np.diag(np.asarray(d, dtype=float))
    return float(np.max(np.linalg.eigvalsh(A.T @ D @ A - D)))


def zero_input_orbit(A, x0, quantizer=saturate, n_samples: int = 200,
                     n_settle: int = 0):
    """Iterate ``x -> Q(A x)`` from ``x0`` and return the state trajectory.

    Rows are time, columns are state components. ``n_settle`` steps are run and
    discarded first, which is what separates a transient from a sustained orbit.
    """
    x = np.asarray(x0, dtype=float).copy()
    for _ in range(n_settle):
        x = quantizer(A @ x)
    out = np.empty((n_samples, x.size))
    for i in range(n_samples):
        x = quantizer(A @ x)
        out[i] = x
    return out


def orbit_period(orbit, max_period: int = 32, atol: float = 1e-12):
    """Smallest exact period of a state trajectory, or 0 if it is not periodic.

    Exact repetition, not near-repetition: a decaying transient never satisfies
    it, which is the whole reason this exists.
    """
    orbit = np.asarray(orbit, dtype=float)
    n = len(orbit)
    for p in range(1, min(max_period, n // 2) + 1):
        if np.allclose(orbit[-1], orbit[-1 - p], atol=atol, rtol=0):
            tail = orbit[-(n // 2):]
            if np.allclose(tail[p:], tail[:-p], atol=atol, rtol=0):
                return p
    return 0


def sustains_oscillation(A, quantizer=saturate, threshold: float = 0.3,
                         n_tries: int = 400, n_settle: int = 5000,
                         n_watch: int = 200, seed: int = 0,
                         require_periodic: bool = True):
    """Search random initial states for a sustained zero-input orbit.

    Returns ``(found, orbit)``.

    **Amplitude alone is not evidence, which is why periodicity is checked.**
    An earlier version of this returned as soon as the state was still large
    after settling, and that reports a slow transient as an oscillation: at
    ``r = 0.999, theta = 0.3`` it found an "orbit" of amplitude 0.74 which,
    given 100000 further steps, decays to 1e-44. A stable filter's transient can
    outlast any window you choose, so the window can never settle the question.
    :func:`orbit_period` does, because an exactly repeating trajectory is not
    decaying at all. ``n_settle`` is generous for the same reason.

    A positive result is then a proof by exhibition. A negative result is only a
    failed search over ``n_tries`` starts and must never be reported as "cannot
    oscillate": for that you need :func:`lyapunov_margin` to be negative, which
    is an actual proof.
    """
    A = np.asarray(A, dtype=float)
    rng = np.random.default_rng(seed)
    for _ in range(n_tries):
        x0 = rng.uniform(-1, 1, A.shape[0])
        orbit = zero_input_orbit(A, x0, quantizer, n_watch, n_settle)
        if np.max(np.abs(orbit)) <= threshold:
            continue
        if require_periodic and orbit_period(orbit) == 0:
            continue
        return True, orbit
    return False, None
