"""Discrete time-domain passivity observer/controller at the master port."""

from __future__ import annotations

from dataclasses import dataclass

import numpy as np


@dataclass(frozen=True)
class POPCDiagnostics:
    tau_candidate: np.ndarray
    tau_applied: np.ndarray
    observer_before: float
    observer_preclip: float
    observer_after: float
    candidate_power: float
    applied_power: float
    damping_gain: float
    intervention_active: bool
    fail_safe_active: bool


class TimeDomainPOPC:
    """Causal PO/PC using dissipative velocity feedback.

    Positive ``tau @ qd`` is energy delivered by the device.  When the
    candidate would exhaust the observer balance, the controller injects
    ``-beta * qd``.  This is intentionally a separate baseline from the radial
    tank projection implemented in :mod:`core.haptic_render`.
    """

    def __init__(
        self,
        *,
        initial_energy: float = 0.0,
        minimum_energy: float = 0.0,
        maximum_energy: float = np.inf,
        velocity_epsilon: float = 1e-12,
    ):
        if not np.isfinite(initial_energy) or not np.isfinite(minimum_energy):
            raise ValueError("initial and minimum energy must be finite")
        if maximum_energy <= minimum_energy:
            raise ValueError("maximum_energy must exceed minimum_energy")
        if not minimum_energy <= initial_energy <= maximum_energy:
            raise ValueError("initial_energy must lie within observer bounds")
        if velocity_epsilon <= 0.0:
            raise ValueError("velocity_epsilon must be positive")
        self.minimum_energy = float(minimum_energy)
        self.maximum_energy = float(maximum_energy)
        self.velocity_epsilon = float(velocity_epsilon)
        self.energy = float(initial_energy)
        self.last_diagnostics: POPCDiagnostics | None = None

    def reset(self, energy: float | None = None) -> None:
        target = self.energy if energy is None else float(energy)
        if not self.minimum_energy <= target <= self.maximum_energy:
            raise ValueError("reset energy lies outside observer bounds")
        self.energy = target
        self.last_diagnostics = None

    def apply(
        self,
        tau_candidate: np.ndarray,
        qd_master: np.ndarray,
        dt: float,
    ) -> tuple[np.ndarray, POPCDiagnostics]:
        candidate = np.asarray(tau_candidate, dtype=float).reshape(-1)
        velocity = np.asarray(qd_master, dtype=float).reshape(-1)
        if candidate.shape != velocity.shape or candidate.size == 0:
            raise ValueError("candidate and velocity must have equal non-empty shapes")
        before = self.energy
        if (
            not np.isfinite(dt)
            or dt <= 0.0
            or not np.all(np.isfinite(candidate))
            or not np.all(np.isfinite(velocity))
        ):
            applied = np.zeros_like(candidate)
            diagnostics = POPCDiagnostics(
                tau_candidate=candidate.copy(),
                tau_applied=applied,
                observer_before=before,
                observer_preclip=before,
                observer_after=before,
                candidate_power=0.0,
                applied_power=0.0,
                damping_gain=0.0,
                intervention_active=True,
                fail_safe_active=True,
            )
            self.last_diagnostics = diagnostics
            return applied, diagnostics

        candidate_power = float(candidate @ velocity)
        available = max(0.0, before - self.minimum_energy)
        allowed_power = available / dt
        damping_gain = 0.0
        applied = candidate.copy()
        velocity_norm_sq = float(velocity @ velocity)
        if candidate_power > allowed_power:
            if velocity_norm_sq <= self.velocity_epsilon:
                # With an almost-zero velocity, nonzero power is numerical
                # contamination; zero output is the conservative response.
                applied.fill(0.0)
            else:
                damping_gain = (
                    candidate_power - allowed_power
                ) / velocity_norm_sq
                applied = candidate - damping_gain * velocity

        applied_power = float(applied @ velocity)
        preclip = before - applied_power * dt
        self.energy = float(
            np.clip(preclip, self.minimum_energy, self.maximum_energy)
        )
        diagnostics = POPCDiagnostics(
            tau_candidate=candidate.copy(),
            tau_applied=applied.copy(),
            observer_before=before,
            observer_preclip=preclip,
            observer_after=self.energy,
            candidate_power=candidate_power,
            applied_power=applied_power,
            damping_gain=damping_gain,
            intervention_active=bool(
                np.any(np.abs(applied - candidate) > 1e-12)
            ),
            fail_safe_active=False,
        )
        self.last_diagnostics = diagnostics
        return applied, diagnostics