"""Automatic trajectory optimizer — "name a target, get the safest corridor".

Given a deep target and a set of critical structures (vessels, eloquent cortex,
deep nuclei, white-matter tracts — each a binary mask), this searches candidate
entry points and ranks the straight-line trajectories by a single safety
objective: maximise the minimum clearance to any critical structure, subject to
hard constraints (a per-structure minimum margin, plus optional maximum approach
angle and depth). It returns the recommended corridor and ranked alternatives.

This is the engine behind the automated-planning claim: instead of a surgeon
hand-checking one line at a time, Sulcora scores thousands of corridors against
every structure at once — optionally against the *predicted deformed* anatomy,
so the corridor is safe where the brain will be, not only where it was.

All scoring/ranking here is pure NumPy and unit-tested; clearance uses the same
KD-tree helper as the vessel module.

IMPORTANT: Research software only. Not a medical device. See docs/INTENDED_USE.md.
"""
from __future__ import annotations
from dataclasses import dataclass, field
import numpy as np
from .volume import Volume
from .planning import Trajectory, risk_band
from .vessels import _mask_world_points


@dataclass
class RiskStructure:
    """A critical structure the trajectory must avoid.

    mask             : binary Volume of the structure(s).
    min_clearance_mm : hard constraint — a trajectory closer than this is infeasible.
    weight           : soft importance (higher => the optimiser values clearance
                       to this structure more when ranking).
    """
    name: str
    mask: Volume
    min_clearance_mm: float = 1.0
    weight: float = 1.0


@dataclass
class TrajectoryPlan:
    entry: np.ndarray
    target: np.ndarray
    length_mm: float
    angle_deg: float
    clearances: dict           # structure name -> clearance (mm)
    min_clearance_mm: float
    nearest: str
    feasible: bool
    score: float               # higher is better
    band: str

    @property
    def trajectory(self) -> Trajectory:
        return Trajectory(self.entry, self.target)


def _unit(v):
    v = np.asarray(v, float)
    n = np.linalg.norm(v)
    return v / n if n > 0 else v


def candidate_entries(target, radius, axis=(0, 0, 1), max_angle_deg=60.0,
                      n_theta=6, n_phi=16) -> np.ndarray:
    """Entry points on a spherical cap of `radius` around `target`, within
    `max_angle_deg` of `axis` (the preferred approach direction, default +Z)."""
    target = np.asarray(target, float)
    a = _unit(axis)
    ref = np.array([1.0, 0, 0]) if abs(a[0]) < 0.9 else np.array([0, 1.0, 0])
    e1 = _unit(np.cross(a, ref)); e2 = np.cross(a, e1)
    out = []
    for th in np.linspace(0.0, np.radians(max_angle_deg), n_theta):
        phis = [0.0] if th == 0 else np.linspace(0, 2 * np.pi, n_phi, endpoint=False)
        for ph in phis:
            d = np.cos(th) * a + np.sin(th) * (np.cos(ph) * e1 + np.sin(ph) * e2)
            out.append(target + radius * d)
    return np.array(out)


def _tree(points):
    from scipy.spatial import cKDTree
    return cKDTree(points) if len(points) else None


def _clearance(tree, traj, n):
    if tree is None:
        return float("inf")
    d, _ = tree.query(traj.sample(n))
    return float(np.min(d))


def score_trajectory(entry, target, structures, *, axis=(0, 0, 1),
                     max_angle_deg=None, max_depth_mm=None,
                     length_penalty=0.0, angle_penalty=0.0,
                     deformation_factory=None, n=200,
                     _trees=None) -> TrajectoryPlan:
    """Score a single entry→target trajectory against all structures."""
    entry = np.asarray(entry, float); target = np.asarray(target, float)
    traj = Trajectory(entry, target)
    length = traj.length
    angle = traj.angle_from_axis(axis)

    clearances = {}
    for s in structures:
        if deformation_factory is not None:
            pts = _mask_world_points(s.mask, deformation_factory(traj))
            cl = _clearance(_tree(pts), traj, n)
        else:
            cl = _clearance(_trees[s.name], traj, n)
        clearances[s.name] = cl

    if clearances:
        nearest = min(clearances, key=clearances.get)
        min_cl = clearances[nearest]
        eff = min(clearances[s.name] / max(s.weight, 1e-9) for s in structures)
    else:
        nearest, min_cl, eff = None, float("inf"), float("inf")

    feasible = all(clearances[s.name] >= s.min_clearance_mm for s in structures)
    if max_angle_deg is not None and angle > max_angle_deg:
        feasible = False
    if max_depth_mm is not None and length > max_depth_mm:
        feasible = False

    score = eff - length_penalty * length - angle_penalty * angle
    return TrajectoryPlan(entry=entry, target=target, length_mm=round(length, 2),
                          angle_deg=round(angle, 1), clearances={k: round(v, 2) for k, v in clearances.items()},
                          min_clearance_mm=round(min_cl, 2) if np.isfinite(min_cl) else float("inf"),
                          nearest=nearest, feasible=feasible, score=score,
                          band=risk_band(min_cl))


def optimize_trajectory(target, entries, structures, *, axis=(0, 0, 1),
                        max_angle_deg=None, max_depth_mm=None,
                        length_penalty=0.0, angle_penalty=0.0,
                        deformation_factory=None, n=200) -> list:
    """Rank candidate corridors best-first.

    Feasible plans (satisfying every hard constraint) sort ahead of infeasible
    ones; within each group, higher score first. `deformation_factory(traj)`, if
    given, returns a deformation model per candidate so ranking is
    deformation-aware (each probe induces its own tissue shift).
    """
    trees = None
    if deformation_factory is None:
        trees = {s.name: _tree(_mask_world_points(s.mask)) for s in structures}
    plans = [score_trajectory(e, target, structures, axis=axis,
                              max_angle_deg=max_angle_deg, max_depth_mm=max_depth_mm,
                              length_penalty=length_penalty, angle_penalty=angle_penalty,
                              deformation_factory=deformation_factory, n=n, _trees=trees)
             for e in np.atleast_2d(np.asarray(entries, float))]
    plans.sort(key=lambda p: (p.feasible, p.score), reverse=True)
    return plans


def plan_safest(target, structures, *, radius, axis=(0, 0, 1), max_angle_deg=60.0,
                n_theta=6, n_phi=16, max_depth_mm=None, length_penalty=0.0,
                angle_penalty=0.0, deformation_factory=None, n=200) -> list:
    """Convenience: generate a spherical-cap of candidate entries and optimise."""
    entries = candidate_entries(target, radius, axis=axis, max_angle_deg=max_angle_deg,
                                n_theta=n_theta, n_phi=n_phi)
    return optimize_trajectory(target, entries, structures, axis=axis,
                               max_angle_deg=max_angle_deg, max_depth_mm=max_depth_mm,
                               length_penalty=length_penalty, angle_penalty=angle_penalty,
                               deformation_factory=deformation_factory, n=n)
