import numpy as np


class HurwitzLatentObserver:
    """Euler observer: z_dot = f(z) + G(y - C z)."""
    def __init__(self, f, C, G, dt, U_K=None):
        self.f = f
        self.C = np.asarray(C, dtype=float)
        self.G = np.asarray(G, dtype=float)
        self.dt = float(dt)
        if U_K is None:
            _, s, vh = np.linalg.svd(self.C, full_matrices=True)
            rank = int(np.sum(s > 1e-10))
            U_K = vh[rank:].T
        self.U_K = np.asarray(U_K, dtype=float)

    def step(self, z, y):
        z = np.asarray(z, dtype=float)
        y = np.asarray(y, dtype=float)
        return z + self.dt * (self.f(z) + self.G @ (y - self.C @ z))

    def projected_jacobian(self, x, jacobian):
        J = np.asarray(jacobian(x), dtype=float)
        return self.U_K.T @ (J - self.G @ self.C) @ self.U_K

    def lyapunov_penalty(self, x, jacobian, P=None, alpha=0.1):
        H = self.projected_jacobian(x, jacobian)
        if H.size == 0:
            return 0.0
        if P is None:
            P = np.eye(H.shape[0])
        P = np.asarray(P, dtype=float)
        S = H.T @ P + P @ H + 2.0 * alpha * P
        return float(max(0.0, np.max(np.linalg.eigvalsh((S + S.T) / 2.0))) )