"""YOLO26 detection with a validity-aware depth/intensity input adapter."""

from __future__ import annotations

from pathlib import Path
import re

import torch
from torch import nn
from torch.nn import functional as F
from ultralytics.cfg import get_cfg
from ultralytics.nn.modules import C2f, Conv
from ultralytics.nn.tasks import DetectionModel, load_checkpoint


def valid_depth_edges(depth: torch.Tensor, valid: torch.Tensor) -> torch.Tensor:
    """Return depth differences only between pairs of valid neighboring pixels."""
    if depth.shape != valid.shape or depth.ndim != 4 or depth.shape[1] != 1:
        raise ValueError("depth and valid must each have shape [B, 1, H, W]")
    horizontal = (depth[..., 1:] - depth[..., :-1]).abs() * (valid[..., 1:] * valid[..., :-1])
    vertical = (depth[..., 1:, :] - depth[..., :-1, :]).abs() * (
        valid[..., 1:, :] * valid[..., :-1, :]
    )
    return torch.maximum(F.pad(horizontal, (0, 1)), F.pad(vertical, (0, 0, 0, 1)))


def _masked_modalities(image: torch.Tensor, use_depth: bool, use_intensity: bool):
    if image.ndim != 4 or image.shape[1] != 4 or not image.is_floating_point():
        raise ValueError("Expected floating image [B, 4, H, W]: depth, intensity, validD, validI")
    depth, intensity, valid_d, valid_i = image.split(1, dim=1)
    valid_d = (
        ((valid_d > 0.5) & torch.isfinite(depth)).to(image.dtype)
        if use_depth else torch.zeros_like(valid_d)
    )
    valid_i = (
        ((valid_i > 0.5) & torch.isfinite(intensity)).to(image.dtype)
        if use_intensity else torch.zeros_like(valid_i)
    )
    depth = torch.where(torch.isfinite(depth), depth, 0.0) * valid_d
    intensity = torch.where(torch.isfinite(intensity), intensity, 0.0) * valid_i
    return depth, intensity, valid_d, valid_i


class ShallowFusion(nn.Module):
    """Map masked depth and intensity to the three channels of a pretrained backbone."""

    def __init__(self, use_depth: bool = True, use_intensity: bool = True):
        super().__init__()
        if not (use_depth or use_intensity):
            raise ValueError("At least one modality must be enabled")
        self.use_depth = use_depth
        self.use_intensity = use_intensity
        self.refine = nn.Sequential(
            nn.Conv2d(4, 16, 3, padding=1, bias=False),
            nn.SiLU(),
            nn.Conv2d(16, 3, 1, bias=False),
        )
        nn.init.zeros_(self.refine[-1].weight)

    def forward(self, image: torch.Tensor) -> torch.Tensor:
        depth, intensity, valid_d, valid_i = _masked_modalities(image, self.use_depth, self.use_intensity)
        base = torch.cat((depth, intensity, (depth + intensity) * 0.5), dim=1)
        return base + self.refine(torch.cat((depth, intensity, valid_d, valid_i), dim=1)) * (
            (valid_d + valid_i) > 0
        ).to(image.dtype)


class P2ModalityFusion(nn.Module):
    """Fuse independent stride-four modality stems using learned, validity-aware gates."""

    def __init__(self, channels: int, use_depth: bool, use_intensity: bool):
        super().__init__()
        self.use_depth = use_depth
        self.use_intensity = use_intensity

        def stem():
            return nn.Sequential(
                nn.Conv2d(2, 32, 3, stride=2, padding=1, bias=False),
                nn.SiLU(),
                nn.Conv2d(32, channels, 3, stride=2, padding=1, bias=False),
                nn.SiLU(),
            )

        self.depth_stem = stem() if use_depth else None
        self.intensity_stem = stem() if use_intensity else None
        self.gate = nn.Conv2d(channels * 2 + 2, 2, 1)
        nn.init.zeros_(self.gate.weight)
        nn.init.zeros_(self.gate.bias)
        self.projection = nn.Conv2d(channels, channels, 1, bias=False)
        nn.init.zeros_(self.projection.weight)

    def forward(self, image: torch.Tensor) -> torch.Tensor:
        depth, intensity, valid_d, valid_i = _masked_modalities(image, self.use_depth, self.use_intensity)
        d_feat = self.depth_stem(torch.cat((depth, valid_d), dim=1)) if self.depth_stem is not None else None
        i_feat = self.intensity_stem(torch.cat((intensity, valid_i), dim=1)) if self.intensity_stem is not None else None
        if d_feat is None:
            d_feat = torch.zeros_like(i_feat)
        if i_feat is None:
            i_feat = torch.zeros_like(d_feat)

        coverage_d = F.avg_pool2d(valid_d, kernel_size=4, stride=4)
        coverage_i = F.avg_pool2d(valid_i, kernel_size=4, stride=4)
        d_feat = d_feat * coverage_d
        i_feat = i_feat * coverage_i
        logits = self.gate(torch.cat((d_feat, i_feat, coverage_d, coverage_i), dim=1))
        logits = logits + torch.cat((coverage_d, coverage_i), dim=1).clamp_min(1e-6).log()
        weights = logits.softmax(dim=1)
        return self.projection(d_feat * weights[:, :1] + i_feat * weights[:, 1:])


class CSPNeXtReplacement(nn.Module):
    """Approximate the paper's CSPNeXt C2f replacement with a split large-kernel block."""

    def __init__(self, c1: int, c2: int, repeats: int):
        super().__init__()
        hidden = max(16, c2 // 2)
        self.bypass = Conv(c1, hidden, 1)
        self.path = Conv(c1, hidden, 1)
        self.blocks = nn.Sequential(*[
            nn.Sequential(Conv(hidden, hidden, 5, g=hidden), Conv(hidden, hidden, 1))
            for _ in range(repeats)
        ])
        self.project = Conv(hidden * 2, c2, 1)

    def forward(self, x):
        return self.project(torch.cat((self.bypass(x), self.blocks(self.path(x))), dim=1))


class PalletDetector(nn.Module):
    """Four-channel detector with native Ultralytics predictions and loss.

    Input channels are normalized depth, normalized intensity, depth validity and
    intensity validity. The final two channels are thresholded at 0.5. P2 models
    fuse independent depth/intensity stems at P2 and inject valid-pair depth
    edges into their stride-four detection feature.
    """

    def __init__(
        self,
        nc: int,
        *,
        p2: bool = True,
        pretrained: str | Path | None = None,
        use_depth: bool = True,
        use_intensity: bool = True,
        use_fusion: bool = True,
        use_edge: bool = True,
        family: str = "yolo26",
    ):
        super().__init__()
        if nc < 1:
            raise ValueError("nc must be positive")
        if family not in {"yolo26", "yolov8", "yolov8-cspnext"}:
            raise ValueError(f"unsupported YOLO family: {family}")
        if family != "yolo26" and p2:
            raise ValueError("YOLOv8 baselines use their original P3-P5 detector")
        self.family = family
        self.architecture_label = ("YOLOv8s + approximate CSPNeXt replacement"
                                   if family == "yolov8-cspnext" else family)
        self.p2 = p2
        self.use_fusion = use_fusion and p2
        self.use_edge = use_edge and use_depth and p2
        self.fusion = ShallowFusion(use_depth, use_intensity)
        architecture = ("yolo26s-p2.yaml" if p2 else "yolo26s.yaml") if family == "yolo26" else "yolov8s.yaml"
        self.yolo = DetectionModel(architecture, ch=3, nc=nc, verbose=False)
        if family == "yolov8-cspnext":
            for index, old in enumerate(self.yolo.model):
                if isinstance(old, C2f):
                    replacement = CSPNeXtReplacement(old.cv1.conv.in_channels,
                                                     old.cv2.conv.out_channels, len(old.m))
                    replacement.i, replacement.f = old.i, old.f
                    self.yolo.model[index] = replacement
        self.yolo.args = get_cfg()
        self.names = self.yolo.names
        self.p2_fusion = (
            P2ModalityFusion(
                self.yolo.model[2].cv2.conv.out_channels,
                use_depth,
                use_intensity,
            )
            if self.use_fusion
            else None
        )
        if self.use_edge:
            p2_channels = self.yolo.model[19].cv2.conv.out_channels
            self.edge_projection = nn.Conv2d(1, p2_channels, 1, bias=False)
            nn.init.zeros_(self.edge_projection.weight)
        else:
            self.edge_projection = None
        if pretrained is not None:
            self.load_pretrained(pretrained)

    @property
    def stride(self) -> torch.Tensor:
        return self.yolo.stride

    def load_pretrained(self, weights: str | Path | nn.Module) -> int:
        """Transfer weights by graph role; retain a detailed coverage audit."""
        if isinstance(weights, (str, Path)):
            source, _ = load_checkpoint(str(weights))
        else:
            source = weights
        source_state = source.state_dict()
        target_state = self.yolo.state_dict()
        # Original P3-P5 neck blocks at 17/19/20/22 reappear at 23/25/26/28.
        # P2's 19/20/22 are new blocks with different inputs and must stay new.
        neck = {17: 23, 19: 25, 20: 26, 22: 28} if self.p2 and self.family == "yolo26" else {}
        matching, entries = {}, []
        target_to_source = {}
        source_model_keys = [k for k in source_state if re.match(r"^model\.\d+\.", k)]
        for source_key in source_model_keys:
            parts = source_key.split(".")
            index = int(parts[1])
            target_index = neck.get(index, index if not self.p2 or index <= 16 else None)
            if self.p2 and self.family == "yolo26" and index == 23:
                # Class branches retain the same input channels at each P3-P5
                # level. Only their first depthwise conv/BN is width-invariant.
                head = re.match(r"^model\.23\.(cv3|one2one_cv3)\.([0-2])\.0\.0\.(conv|bn)\.", source_key)
                if head:
                    target_index = 29
                    parts[3] = str(int(parts[3]) + 1)
            target_key = None if target_index is None else ".".join((parts[0], str(target_index), *parts[2:]))
            reason = "transferred"
            if target_key is None:
                reason = "new_p2_graph_or_unmapped_source"
            elif target_key not in target_state:
                reason = "missing_target_tensor"
            elif target_state[target_key].shape != source_state[source_key].shape:
                reason = "shape_mismatch"
            elif target_key in target_to_source:
                reason = "duplicate_target"
            else:
                matching[target_key] = source_state[source_key]
                target_to_source[target_key] = source_key
            entries.append({"source": source_key, "target": target_key, "reason": reason,
                            "numel": source_state[source_key].numel()})
        if not matching:
            raise ValueError("No matching YOLO26s pretrained tensors found")
        self.yolo.load_state_dict(matching, strict=False)
        source_numel = sum(source_state[k].numel() for k in source_model_keys)
        target_numel = sum(v.numel() for k, v in target_state.items())
        transferred_numel = sum(target_state[k].numel() for k in matching)
        self.transfer_report = {
            "source_tensors": len(source_model_keys), "source_numel": source_numel,
            "target_tensors": len(target_state), "target_numel": target_numel,
            "transferred_tensors": len(matching), "transferred_numel": transferred_numel,
            "source_numel_coverage": transferred_numel / source_numel if source_numel else 0.0,
            "target_numel_coverage": transferred_numel / target_numel if target_numel else 0.0,
            "skipped": [entry for entry in entries if entry["reason"] != "transferred"],
            "new_target": [k for k in target_state if k not in matching],
            "transferred": [entry for entry in entries if entry["reason"] == "transferred"],
        }
        return len(matching)

    def forward(self, image: torch.Tensor):
        fused = self.fusion(image)
        if self.edge_projection is None and self.p2_fusion is None:
            return self.yolo(fused)

        if self.edge_projection is not None:
            depth, _, valid, _ = _masked_modalities(image, self.fusion.use_depth, self.fusion.use_intensity)
            edge = valid_depth_edges(depth, valid)
        outputs = []
        x = fused
        for layer in self.yolo.model:
            if layer.f != -1:
                x = outputs[layer.f] if isinstance(layer.f, int) else [
                    x if j == -1 else outputs[j] for j in layer.f
                ]
            x = layer(x)
            if layer.i == 2 and self.p2_fusion is not None:
                x = x + self.p2_fusion(image)
            if layer.i == 19 and self.edge_projection is not None:
                local_edge = F.avg_pool2d(edge, kernel_size=4, stride=4)
                x = x + self.edge_projection(local_edge)
            outputs.append(x if layer.i in self.yolo.save else None)
        return x

    def loss(self, batch: dict):
        """Compute Ultralytics detection loss from a four-channel image batch."""
        return self.yolo.loss(batch, preds=self.forward(batch["img"]))
