"""Adapters around pinned official point-cloud architectures (benchmark v3).

AST loading avoids unrelated training-only extension imports. The architecture
class bodies remain official; FPS/KNN/ball query use explicit PyTorch operators.
No segmentation/classification output is interpreted as a pallet pose.
"""
from pathlib import Path
from types import SimpleNamespace, ModuleType
import ast
import importlib.util
import logging
import sys
from typing import List, Type

import numpy as np
import torch
from torch import nn
from torch.nn import functional as F
from timm.layers import DropPath, trunc_normal_


ROOT = Path('outputs/pallet_pose_benchmark_v3')
UPSTREAM = ROOT / 'upstream'
PRETRAINED = ('utonia', 'sonata', 'concerto', 'litept', 'pointmae')
FAMILIES = PRETRAINED + ('dgcnn', 'pointnext', 'ptv3')


def package(name, path):
    if name not in sys.modules:
        module = ModuleType(name)
        module.__path__ = [str(path)]
        sys.modules[name] = module


def module_file(name, path):
    spec = importlib.util.spec_from_file_location(name, path)
    module = importlib.util.module_from_spec(spec)
    sys.modules[name] = module
    spec.loader.exec_module(module)
    return module


def official_definitions(path, namespace, names=None):
    tree = ast.parse(path.read_text(), filename=str(path))
    body = []
    for node in tree.body:
        if isinstance(node, (ast.FunctionDef, ast.ClassDef)) and (names is None or node.name in names):
            if isinstance(node, ast.ClassDef):
                node.decorator_list = []  # registry only; not used by inference
            body.append(node)
    exec(compile(ast.Module(body=body, type_ignores=[]), str(path), 'exec'), namespace)
    return namespace


@torch.no_grad()
def fps_indices(x, count):
    b, n, _ = x.shape
    result = torch.empty(b, count, dtype=torch.long, device=x.device)
    distance = torch.full((b, n), float('inf'), device=x.device)
    farthest = torch.zeros(b, dtype=torch.long, device=x.device)
    batch = torch.arange(b, device=x.device)
    for i in range(count):
        result[:, i] = farthest
        d = ((x - x[batch, farthest, None]) ** 2).sum(-1)
        distance = torch.minimum(distance, d)
        farthest = distance.max(-1).indices
    return result


def fps(x, count):
    return x.gather(1, fps_indices(x, count)[..., None].expand(-1, -1, 3))


class KNN(nn.Module):
    def __init__(self, k, transpose_mode=True):
        super().__init__()
        self.k = k

    def forward(self, support, query):
        return torch.cdist(query, support).topk(self.k, largest=False)


class BallGroup(nn.Module):
    def __init__(self, config):
        super().__init__()
        self.radius = config.get('radius', .1)
        self.nsample = config.get('nsample', 32)
        self.normalize_dp = config.get('normalize_dp', False)

    def forward(self, query, support, features):
        b, n, _ = support.shape
        if self.radius is None:
            return support.transpose(1, 2).unsqueeze(2), features.unsqueeze(2)
        else:
            valid = torch.cdist(query, support) < self.radius
            index = torch.arange(n, device=support.device).expand_as(valid)
            ordered = index.masked_fill(~valid, n).sort(-1).values
            first = ordered[..., :1].clamp_max(n - 1)
            indices = ordered[..., :min(self.nsample, n)]
            indices = torch.where(indices == n, first, indices)
            if indices.shape[-1] < self.nsample:
                indices = torch.cat((indices, first.expand(*first.shape[:-1], self.nsample - n)), -1)
        batch = torch.arange(b, device=support.device)[:, None, None]
        dp = (support[batch, indices] - query[:, :, None]).permute(0, 3, 1, 2)
        if self.normalize_dp:
            dp = dp / self.radius
        grouped = features.transpose(1, 2)[batch, indices].permute(0, 3, 1, 2)
        return dp, grouped


def pointnext_encoder():
    from addict import Dict
    root = UPSTREAM / 'pointnext/openpoints/models'
    package('_pose_next', root)
    package('_pose_next.layers', root / 'layers')
    conv = module_file('_pose_next.layers.conv', root / 'layers/conv.py')
    act = sys.modules['_pose_next.layers.activation'].create_act
    ns = dict(torch=torch, nn=nn, logging=logging, List=List, Type=Type,
              create_convblock1d=conv.create_convblock1d, create_convblock2d=conv.create_convblock2d,
              create_act=act, CHANNEL_MAP={'dp_fj': lambda c: c + 3},
              create_grouper=lambda c: BallGroup(dict(c)), furthest_point_sample=fps_indices,
              random_sample=lambda p, n: torch.rand(p.shape[:2], device=p.device).argsort(-1)[:, :n],
              get_aggregation_feautres=lambda p, dp, f, fj, feature_type='dp_fj': torch.cat((dp, fj), 1))
    official_definitions(root / 'backbone/pointnext.py', ns)
    return ns['PointNextEncoder'](in_channels=7, width=32, blocks=[1, 1, 1, 1, 1, 1],
                strides=[1, 2, 2, 2, 2, 1], nsample=32, radius=.15,
                radius_scaling=1.5, sa_layers=2, sa_use_res=True, expansion=4,
                group_args=Dict(NAME='ballquery', normalize_dp=True),
                norm_args={'norm': 'bn'}, act_args={'act': 'relu'}, conv_args={})


def pointmae_encoder():
    ns = dict(torch=torch, nn=nn, F=F, np=np, DropPath=DropPath,
              trunc_normal_=trunc_normal_, misc=SimpleNamespace(fps=fps), KNN=KNN)
    official_definitions(UPSTREAM / 'pointmae/models/Point_MAE.py', ns,
                        {'Encoder', 'Group', 'Mlp', 'Attention', 'Block', 'TransformerEncoder', 'PointTransformer'})
    config = SimpleNamespace(trans_dim=384, depth=12, drop_path_rate=.1,
                             cls_dim=40, num_heads=6, group_size=32, num_group=64, encoder_dims=384)
    model = ns['PointTransformer'](config)
    state = torch.load(ROOT / 'weights/pointmae.pth', map_location='cpu', weights_only=False)['base_model']
    mapped = {}
    for k, v in state.items():
        k = k.removeprefix('module.').removeprefix('MAE_encoder.').removeprefix('base_model.')
        if k in model.state_dict():
            mapped[k] = v
    missing, unexpected = model.load_state_dict(mapped, strict=False)
    allowed = ('cls_token', 'cls_pos', 'cls_head_finetune.')
    if unexpected or any(not k.startswith(allowed) for k in missing):
        raise ValueError(('Point-MAE encoder weight mismatch', missing, unexpected))
    model.cls_head_finetune = nn.Identity()
    return model


class Backbone(nn.Module):
    def __init__(self, family):
        super().__init__()
        self.family = family
        self.pretrained = family in PRETRAINED
        if family in ('utonia', 'sonata', 'concerto'):
            package(family, UPSTREAM / family / family)
            module = module_file(family + '.model', UPSTREAM / family / family / 'model.py')
            filename = {'utonia': 'utonia_small.pth', 'sonata': 'sonata.pth', 'concerto': 'concerto_small.pth'}[family]
            self.model = module.load(str(ROOT / 'weights' / filename), custom_config=dict(enable_flash=True, shuffle_orders=False))
            self.dim = {'utonia': 432, 'sonata': 512, 'concerto': 512}[family] * 2
        elif family == 'ptv3':
            package('_pose_ptv3', UPSTREAM / 'ptv3')
            module = module_file('_pose_ptv3.model', UPSTREAM / 'ptv3/model.py')
            self.model = module.PointTransformerV3(in_channels=7, cls_mode=True, enable_flash=True, shuffle_orders=False)
            self.dim = 1024
        elif family == 'litept':
            package('libs', UPSTREAM / 'litept/libs')
            package('litept', UPSTREAM / 'litept/litept')
            module = module_file('litept.model', UPSTREAM / 'litept/litept/model.py')
            self.model = module.LitePT(in_channels=6, enc_mode=True, shuffle_orders=False)
            weights = torch.load(ROOT / 'weights/litept_scan.pth', map_location='cpu', weights_only=False)['state_dict']
            weights = {k.removeprefix('module.backbone.'): v for k, v in weights.items() if k.startswith('module.backbone.')}
            weights = {k: v for k, v in weights.items() if not k.startswith('dec.')}
            self.model.load_state_dict(weights, strict=True)
            self.dim = 1008
        elif family == 'pointmae':
            self.model = pointmae_encoder()
            self.dim = 768
        elif family == 'pointnext':
            self.model = pointnext_encoder()
            self.dim = self.model.out_channels
        elif family == 'dgcnn':
            module = module_file('_pose_dgcnn', UPSTREAM / 'dgcnn/pytorch/model.py')
            self.model = module.DGCNN(SimpleNamespace(k=20, emb_dims=1024, dropout=.5), output_channels=4)
            self.model.linear3 = nn.Identity()
            self.dim = 256
        else:
            raise ValueError(family)
        # Official pooling layers have their own shuffle flag, independent of
        # the top-level flag. Disable both for repeatable inference.
        for module in self.model.modules():
            if hasattr(module, 'shuffle_orders'):
                module.shuffle_orders = False

    def forward(self, features):
        xyz = features[..., :3].contiguous()
        if self.family in ('pointmae',):
            return self.model(xyz)
        if self.family == 'dgcnn':
            return self.model(xyz.transpose(1, 2).contiguous())
        if self.family == 'pointnext':
            return self.model.forward_cls_feat({'pos': xyz, 'x': features.transpose(1, 2).contiguous()})
        b, n, _ = features.shape
        # Dimensionless coordinates: no native-to-metric conversion is invented.
        grid = ((xyz - xyz.amin(1, keepdim=True)) / .02).floor().int()
        if self.family in ('utonia', 'sonata', 'concerto'):
            feat = torch.cat((xyz, torch.zeros_like(xyz), torch.zeros_like(xyz)), -1)
        elif self.family == 'litept':
            feat = torch.cat((torch.zeros_like(xyz), torch.zeros_like(xyz)), -1)
        else:
            feat = features
        # spconv requires unique (batch, voxel) coordinates. Like upstream
        # GridSample, retain one measured point per voxel; choose the first
        # deterministically rather than random train-time voxel sampling.
        chosen, counts = [], []
        for i in range(b):
            _, inverse = torch.unique(grid[i], dim=0, return_inverse=True)
            count = int(inverse.max()) + 1
            first = torch.full((count,), n, device=xyz.device, dtype=torch.long)
            first.scatter_reduce_(0, inverse, torch.arange(n, device=xyz.device), reduce='amin')
            chosen.append(first + i * n)
            counts.append(count)
        chosen = torch.cat(chosen)
        data = dict(coord=xyz.reshape(-1, 3)[chosen], grid_coord=grid.reshape(-1, 3)[chosen],
                    feat=feat.reshape(b * n, -1)[chosen],
                    offset=torch.tensor(counts, device=xyz.device).cumsum(0), grid_size=.02)
        encoded = self.model(data)
        return torch.stack([torch.cat((encoded.feat[encoded.batch == i].mean(0),
                                     encoded.feat[encoded.batch == i].amax(0))) for i in range(b)])


class PoseHead(nn.Module):
    def __init__(self, dim, mean, std):
        super().__init__()
        self.register_buffer('mean', torch.as_tensor(mean, dtype=torch.float32))
        self.register_buffer('std', torch.as_tensor(std, dtype=torch.float32).clamp_min(.001))
        self.layers = nn.Sequential(nn.Linear(dim, 128), nn.SiLU(), nn.Dropout(.15), nn.Linear(128, 4))
        nn.init.zeros_(self.layers[-1].weight)
        nn.init.zeros_(self.layers[-1].bias)

    def forward(self, x):
        value = self.layers(((x - self.mean) / self.std).clamp(-10, 10))
        return torch.cat((value[:, :2] * .1, F.normalize(value[:, 2:] + value.new_tensor([1., 0.]), dim=-1)), -1)
