"""Neural network models for HPI: CriticNN, ActorNN, and cost function.

Replaces the polynomial basis functions (symbolic_engine.py) with
data-adaptive NN representations that avoid multicollinearity issues.
"""

import torch
import torch.nn as nn


class CriticNN(nn.Module):
    """Value function approximator V(x) >= 0 with structural V(0)=0.

    Architecture:
        Linear(2, 32, bias=False) -> Tanh
        Linear(32, 32, bias=False) -> Tanh
        Linear(32, 2, bias=False)          # two raw outputs z1, z2
        V(x) = z1(x)^2 + z2(x)^2           # guaranteed V>=0 and V(0)=0

    bias=False everywhere ensures V(0) = 0 structurally.
    Two output heads allow the Hessian at the origin to be rank-2 (strict PD possible).
    """

    def __init__(self):
        super().__init__()
        self.fc1 = nn.Linear(2, 32, bias=False)
        self.fc2 = nn.Linear(32, 32, bias=False)
        self.fc3 = nn.Linear(32, 2, bias=False)

    def forward(self, x):
        z = torch.tanh(self.fc1(x))
        z = torch.tanh(self.fc2(z))
        z = self.fc3(z)  # (..., 2): [z1, z2]
        return (z[:, 0:1] ** 2 + z[:, 1:2] ** 2).squeeze(-1)  # (...,) scalar

    def get_raw_z(self, x):
        """Return the two raw outputs z1, z2 before squaring."""
        z = torch.tanh(self.fc1(x))
        z = torch.tanh(self.fc2(z))
        return self.fc3(z)  # (..., 2)


class ActorNN(nn.Module):
    """Policy network u(x): R^2 -> R.

    Architecture:
        Linear(2, 32, bias=True) -> Tanh
        Linear(32, 32, bias=True) -> Tanh
        Linear(32, 1, bias=True)            # signed control output
    """

    def __init__(self):
        super().__init__()
        self.fc1 = nn.Linear(2, 32, bias=True)
        self.fc2 = nn.Linear(32, 32, bias=True)
        self.fc3 = nn.Linear(32, 1, bias=True)

    def forward(self, x):
        h = torch.tanh(self.fc1(x))
        h = torch.tanh(self.fc2(h))
        return self.fc3(h).squeeze(-1)  # (...,) scalar


def compute_q(x):
    """Cost function Q(x) = x1^2 + x2^2.

    Args:
        x: torch.Tensor of shape (..., 2).

    Returns:
        torch.Tensor of shape (...,).
    """
    return x[..., 0] ** 2 + x[..., 1] ** 2


def check_positive_definite(critic_nn, X_traj=None):
    """Check if the critic NN corresponds to a positive-definite value function.

    Two checks:
    1. Hessian at origin is strictly positive definite (necessary condition).
    2. V(x) > 0 on trajectory data (sanity check, structural guarantee makes
       this always true, but verifies the NN hasn't degenerated).

    Args:
        critic_nn: CriticNN instance.
        X_traj: Optional trajectory data (M, 2) numpy array for additional check.

    Returns:
        bool: True if all checks pass.
    """
    # 1. Hessian at origin
    x0 = torch.zeros(1, 2, requires_grad=True)
    V0 = critic_nn(x0)
    grad_V = torch.autograd.grad(V0.sum(), x0, create_graph=True)[0]  # (1, 2)

    h11 = torch.autograd.grad(grad_V[0, 0], x0, retain_graph=True)[0][0, 0].item()
    h22 = torch.autograd.grad(grad_V[0, 1], x0, retain_graph=True)[0][0, 1].item()
    h12 = torch.autograd.grad(grad_V[0, 0], x0, retain_graph=True)[0][0, 1].item()

    hessian_pd = bool(h11 > 1e-6 and h22 > 1e-6 and h11 * h22 - h12 ** 2 > 1e-12)

    if not hessian_pd:
        return False

    # 2. Optional: verify V(x) is well-behaved on trajectory data
    if X_traj is not None:
        x_t = torch.tensor(X_traj, dtype=torch.float32)
        with torch.no_grad():
            V_traj = critic_nn(x_t)
        if torch.any(torch.isnan(V_traj)) or torch.any(torch.isinf(V_traj)):
            return False
        # V should be finite and non-negative everywhere on trajectory
        if torch.any(V_traj < -1e-4):
            return False

    return True


def check_lyapunov_decrease(critic_nn, X, f_X):
    """Check if V satisfies the Lyapunov decrease condition on trajectory data.

    Verifies: dV/dx · f(x) < 0 for states away from origin.
    This is a necessary condition for V to be a valid Lyapunov function.

    Args:
        critic_nn: CriticNN instance.
        X: State trajectory (M, 2) numpy array.
        f_X: Drift dynamics f(X) evaluated on trajectory, (M, 2) numpy array.

    Returns:
        float: Fraction of points (away from origin) where V_dot < 0.
    """
    x_t = torch.tensor(X, dtype=torch.float32, requires_grad=True)
    f_t = torch.tensor(f_X, dtype=torch.float32)
    V = critic_nn(x_t)
    grad_V = torch.autograd.grad(V.sum(), x_t, create_graph=False)[0]
    V_dot = (grad_V * f_t).sum(dim=1)  # (M,)

    # Only check points away from origin (||x|| > 0.05)
    norms = torch.norm(x_t, dim=1)
    mask = norms > 0.05
    if mask.sum() == 0:
        return 1.0

    V_dot_filtered = V_dot[mask]
    fraction_negative = (V_dot_filtered < 0).float().mean().item()
    return fraction_negative