"""Evidence-based, per-collection sensor policies; never infer units per frame."""
from __future__ import annotations

from collections import defaultdict
from dataclasses import replace
import json
from pathlib import Path

import cv2
import numpy as np

from .manifest import _pcd_grid
from .pcd import read_pcd


def policy_key(record):
    grid = _pcd_grid(record.pcd) if record.pcd else None
    variant = "none_rectified" if "none_rectified" in record.variant else "primary"
    return f"{record.frame_id.split('/')[0]}|{grid}|{variant}"


def projection_evidence(points, shape):
    """Identify a forward coordinate only if pinhole ratios fit pixel rays."""
    h, w = shape
    indices = np.linspace(0, len(points) - 1, min(4096, len(points)), dtype=int)
    xyz = points[indices, :3].astype(np.float64)
    uv = np.column_stack(((indices % w) / max(w - 1, 1) * 2 - 1,
                          (indices // w) / max(h - 1, 1) * 2 - 1, np.ones(len(indices))))
    choices = []
    for axis in range(3):
        other = [i for i in range(3) if i != axis]
        valid = np.isfinite(xyz).all(axis=1) & (xyz[:, axis] > 1e-7)
        if valid.sum() < 64:
            continue
        values = xyz[valid][:, other] / xyz[valid, axis:axis + 1]
        finite = (np.abs(values) < 5).all(axis=1)
        x, values = uv[valid][finite], values[finite]
        if len(x) < 64:
            continue
        fit = np.linalg.lstsq(x, values, rcond=None)[0]
        residual = ((values - x @ fit) ** 2).sum(axis=0)
        total = ((values - values.mean(axis=0)) ** 2).sum(axis=0)
        score = float(np.min(1 - residual / np.maximum(total, 1e-12)))
        slopes = np.linalg.norm(fit[:2], axis=1)
        if score >= 0.995 and min(slopes) > 0.02 and np.linalg.cond(fit[:2]) < 20:
            choices.append({"axis": axis, "r2": score, "coefficients": fit.tolist(),
                            "fov_estimate": np.degrees(2 * np.arctan(slopes)).tolist()})
    return max(choices, key=lambda x: x["r2"]) if choices else None


def _matrix(path):
    fs = cv2.FileStorage(str(path), cv2.FILE_STORAGE_READ)
    try:
        return fs.getFirstTopLevelNode().mat() if fs.isOpened() else None
    finally:
        fs.release()


def metric_anchor(record, xyz, shape, axis):
    """Find a documented metric encoding and verify its numeric correspondence."""
    sources = []
    if "depth" in record.paths:
        depth = cv2.imread(str(record.paths["depth"]), cv2.IMREAD_UNCHANGED)
        if depth is not None and depth.shape == shape and depth.dtype == np.uint16:
            sources.append(("uint16_depth_mm", depth.ravel().astype(float), xyz[:, axis]))
    if "xyz" in record.paths:
        mat = _matrix(record.paths["xyz"])
        if mat is not None and mat.shape == (*shape, 3) and mat.dtype == np.int16 and np.max(np.abs(mat)) > 1:
            # Integer XYZ encoding is documented in the supplied data audit.
            sources.append(("int16_xyz_mm", mat.reshape(-1, 3).ravel().astype(float), xyz[:, :3].ravel()))
    for source, target, raw in sources:
        mask = np.isfinite(raw) & np.isfinite(target) & (np.abs(raw) > 1e-7) & (np.abs(target) > 1)
        if mask.sum() < 64:
            continue
        for scale in (1.0, 1000.0):
            error = np.abs(raw[mask] * scale - target[mask])
            relative = error / np.maximum(np.abs(target[mask]), 100)
            if float(np.quantile(relative, 0.95)) < 0.01:
                return {"scale_to_mm": scale, "source": source,
                        "matched_pixels": int(mask.sum()), "relative_p95": float(np.quantile(relative, 0.95)),
                        "frame_id": record.frame_id}
    return None


def build_registry(records, output, *, samples_per_policy=3):
    groups = defaultdict(list)
    for record in records:
        if record.pcd and _pcd_grid(record.pcd):
            groups[policy_key(record)].append(record)
    registry = {}
    for key, members in sorted(groups.items()):
        sample_ids = sorted(set(np.linspace(0, len(members)-1, min(samples_per_policy, len(members)), dtype=int)))
        evidence, anchors, errors = [], [], []
        for i in sample_ids:
            record = members[i]
            try:
                _, points = read_pcd(record.pcd)
                shape = _pcd_grid(record.pcd)
                if len(points) != shape[0] * shape[1]:
                    raise ValueError("PCD point count does not match restored grid")
                projection = projection_evidence(points, shape)
                if projection:
                    evidence.append({"frame_id": record.frame_id, **projection})
                    anchor = metric_anchor(record, points, shape, projection["axis"])
                    if anchor:
                        anchors.append(anchor)
            except Exception as exc:
                errors.append({"frame_id": record.frame_id, "error": str(exc)})
        grid = _pcd_grid(members[0].pcd)
        stable_axis = len(evidence) == len(sample_ids) and len({e['axis'] for e in evidence}) == 1
        stable_scale = bool(anchors) and len({e['scale_to_mm'] for e in anchors}) == 1
        fovs = np.array([e['fov_estimate'] for e in evidence])
        stable_projection = stable_axis and np.ptp(fovs, axis=0).max() < 2
        fov = fovs.mean(axis=0).tolist() if len(fovs) else None
        projection_tag = "unverified" if not stable_projection else "fov_" + "_".join(str(int(round(v/5)*5)) for v in fov)
        registry[key] = {
            "policy_id": key, "grid": list(grid),
            "domain_id": f"{grid[1]}x{grid[0]}_{projection_tag}",
            "depth_available": bool(stable_axis and stable_projection and stable_scale),
            "depth_source": "pcd", "depth_kind": "axial", "output_unit": "mm",
            "depth_axis": evidence[0]['axis'] if stable_axis else None,
            "scale_to_mm": anchors[0]['scale_to_mm'] if stable_scale else None,
            "projection_evidence": evidence, "unit_evidence": anchors,
            "errors": errors, "sample_count": len(sample_ids), "frames": len(members),
            "unavailable_reason": None if stable_axis and stable_projection and stable_scale
            else "No consistent sampled projection and paired metric-unit evidence",
            "source_note": "Projection fit identifies configurations, not factory calibration; metric anchors follow dataset report encoding."}
    output = Path(output)
    output.parent.mkdir(parents=True, exist_ok=True)
    output.write_text(json.dumps(registry, indent=2), encoding="utf-8")
    return registry


def attach_registry(records, registry):
    return [replace(record, sensor_config=registry.get(policy_key(record), {
        "domain_id": "unknown", "depth_available": False, "unavailable_reason": "No organized sensor grid"}))
        for record in records]
