"""Small-data partial-pose models and label-free ROI geometry.

PointNet-inspired shared point MLP + symmetric pooling, not a reproduction of
the original PointNet/T-Net. No CAD model, 6D rotation or robot transform is
invented. Preprocessing consumes pixels/XYZI only, never pose targets.
"""
from __future__ import annotations

import math
from pathlib import Path

import numpy as np
import torch
from torch import nn
from torch.nn import functional as F

POINT_DIM = 7
GLOBAL_DIM = 16
TASKS = ('planar_xy_theta', 'reference_xyz_normal')


def angular_error_degrees(pred, target):
    return np.abs((np.asarray(pred)-np.asarray(target)+180.)%360.-180.)


def canonical_normal(normal):
    normal=np.asarray(normal,dtype=np.float64)
    norm=np.linalg.norm(normal)
    if not np.isfinite(normal).all() or norm<1e-8:
        raise ValueError('Invalid plane normal')
    normal=normal/norm
    return -normal if normal[0]<0 else normal


def fit_front_plane(xyz, config, seed=42):
    """RANSAC followed by SVD; x-facing is a declared sensor-domain prior."""
    xyz=np.asarray(xyz,dtype=np.float64)
    if len(xyz)<16 or not np.isfinite(xyz).all():
        raise ValueError('Too few finite XYZ points')
    rng=np.random.default_rng(seed)
    count=min(len(xyz),config['geometry_points'])
    data=xyz[rng.choice(len(xyz),count,replace=False)]
    triples=rng.integers(0,count,size=(config['ransac_iterations'],3))
    a,b,c=(data[triples[:,i]] for i in range(3))
    normals=np.cross(b-a,c-a)
    lengths=np.linalg.norm(normals,axis=1)
    valid=(lengths>1e-8)&(np.abs(normals[:,0])>=.25*lengths)
    normals=normals[valid]/lengths[valid,None]
    centers=a[valid]
    success=False
    normal=np.array([1.,0.,0.])
    mask=np.ones(len(data),dtype=bool)
    if len(normals):
        residual=np.abs(data@normals.T-np.sum(centers*normals,axis=1))
        scores=(residual<config['plane_threshold']).sum(axis=0)
        best=int(scores.argmax())
        normal=canonical_normal(normals[best])
        mask=residual[:,best]<config['plane_threshold']
        success=int(mask.sum())>=16
        if success:
            for _ in range(2):
                center=np.median(data[mask],axis=0)
                centered=data[mask]-center
                _,_,vt=np.linalg.svd(centered,full_matrices=False)
                candidate=canonical_normal(vt[-1])
                if candidate[0]<.25:break
                refined=np.abs((data-center)@candidate)<config['plane_threshold']
                if refined.sum()<16:break
                normal,mask=candidate,refined
    center=np.median(data[mask],axis=0) if success else np.median(data,axis=0)
    return center,normal,float(mask.mean()) if success else 0.,success


def roi_features(points,pixel_map,box,width,height,config,*,scale_to_m=None,seed=42):
    """Extract network inputs with no dependency on GT pose or target identity.

    Unverified units use raw numeric coordinates in the explicitly declared
    research-only mode. This is not a new metric calibration claim.
    """
    if config['unverified_unit_policy']!='assume_native_numeric_metres_research_only':
        raise ValueError('Unknown research unit policy')
    box=np.asarray(box,dtype=float)
    if box.shape!=(4,) or not np.isfinite(box).all() or min(box[2:])<=0:
        raise ValueError('Invalid ROI box')
    if pixel_map.shape!=(height,width) or points.ndim!=2 or points.shape[1]!=4:
        raise ValueError('Cloud/grid shape mismatch')
    if pixel_map.min()<0 or pixel_map.max()>=len(points):
        raise ValueError('Pixel map escapes source points')
    x,y,w,h=box
    x0,y0=max(0,int(np.floor(x))),max(0,int(np.floor(y)))
    x1,y1=min(width,int(np.ceil(x+w))),min(height,int(np.ceil(y+h)))
    if x0>=x1 or y0>=y1:raise ValueError('ROI does not intersect native image')
    yy,xx=np.mgrid[y0:y1,x0:x1]
    selected=points[pixel_map[y0:y1,x0:x1].ravel()].astype(np.float64)
    uv=np.stack(((xx.ravel()-x)/w,(yy.ravel()-y)/h),axis=1)
    good=np.isfinite(selected[:,:3]).all(axis=1)&(np.linalg.norm(np.nan_to_num(selected[:,:3]),axis=1)>1e-8)
    selected,uv=selected[good],uv[good]
    if len(selected)<16:raise ValueError('ROI has fewer than 16 measured XYZ points')
    scale=float(scale_to_m) if scale_to_m is not None else 1.
    if not math.isfinite(scale) or scale<=0:raise ValueError('Invalid point scale')
    xyz=selected[:,:3]*scale
    if scale_to_m is None and np.median(np.linalg.norm(xyz,axis=1))>20:
        raise ValueError('Unverified raw coordinate scale is outside the research domain; supply calibration')
    low,high=np.percentile(xyz,[1,99],axis=0)
    keep=((xyz>=low)&(xyz<=high)).all(axis=1)
    if keep.sum()>=16:
        selected,uv,xyz=selected[keep],uv[keep],xyz[keep]
    anchor,normal,inlier_fraction,plane_success=fit_front_plane(xyz,config,seed)
    q25,q75=np.percentile(xyz,[25,75],axis=0)
    spread=q75-q25
    local_scale=max(float(np.linalg.norm(spread)),.02)
    centered=np.clip((xyz-anchor)/local_scale,-5,5)
    intensity=selected[:,3]
    valid_i=np.isfinite(intensity)
    encoded_i=np.zeros(len(intensity))
    if valid_i.any():
        lo,hi=np.percentile(intensity[valid_i],[5,95])
        encoded_i[valid_i]=np.clip((intensity[valid_i]-lo)/max(float(hi-lo),1e-6),0,1)
    features=np.column_stack((centered,encoded_i,valid_i.astype(float),uv-.5))
    rng=np.random.default_rng(seed)
    picked=rng.choice(len(features),config['points'],replace=len(features)<config['points'])
    features=features[picked].astype(np.float32)
    global_features=np.r_[anchor,spread,normal,[x/width,y/height,w/width,h/height],
        inlier_fraction,np.log1p(len(xyz))/10.,float(scale_to_m is not None)].astype(np.float32)
    if global_features.shape!=(GLOBAL_DIM,) or features.shape!=(config['points'],POINT_DIM):
        raise AssertionError('Feature schema mismatch')
    if not np.isfinite(features).all() or not np.isfinite(global_features).all():
        raise ValueError('Nonfinite extracted features')
    return features,global_features,{'roi_valid_points':len(xyz),'plane_success':plane_success,
        'plane_inlier_fraction':inlier_fraction,'point_scale_to_m':scale_to_m,
        'unverified_units_research_assumption':scale_to_m is None}


def box_array(box):
    return np.array([box[k] for k in ('x','y','width','height')],dtype=float)


def target_array(row,task):
    if row['subset']!='overlap' or row['pose']['kind']!=task or row['pose']['issues']:
        raise ValueError('Ineligible supervised pose record')
    values=row['pose']['values']
    if task=='planar_xy_theta':
        fields=('x_gt','y_gt','theta_d_gt')
        if not all(row['pose']['supervision_mask'][k] for k in fields):
            raise ValueError('Missing planar supervision')
        angle=np.deg2rad(values['theta_d_gt'])
        result=[values['x_gt'],values['y_gt'],np.cos(angle),np.sin(angle)]
    elif task=='reference_xyz_normal':
        fields=('x_gt','y_gt','z_gt')
        if not all(row['pose']['supervision_mask'][k] for k in fields) or not row['pose']['supervision_mask']['normal_gt']:
            raise ValueError('Missing reference/normal supervision')
        result=[values[k] for k in fields]+list(row['pose']['normal_gt'])
    else:raise ValueError('Unknown partial-pose task')
    array=np.asarray(result,dtype=np.float32)
    if not np.isfinite(array).all():raise ValueError('Invalid target')
    return array


def base_prediction(global_features,task):
    g=np.asarray(global_features)
    if task=='planar_xy_theta':
        n=g[...,6:8]
        n=n/np.maximum(np.linalg.norm(n,axis=-1,keepdims=True),1e-8)
        return np.concatenate((g[...,:2],n),axis=-1)
    if task=='reference_xyz_normal':
        return np.concatenate((g[...,:3],g[...,6:9]),axis=-1)
    raise ValueError('Unknown task')


def normalize_prediction(pred,task):
    pred=np.asarray(pred,dtype=float).copy()
    start=2 if task=='planar_xy_theta' else 3
    direction=pred[...,start:]
    norm=np.linalg.norm(direction,axis=-1,keepdims=True)
    fallback=np.zeros_like(direction);fallback[...,0]=1
    pred[...,start:]=np.where(norm>1e-8,direction/np.maximum(norm,1e-8),fallback)
    return pred


class RidgeResidual:
    """Train-only standardized geometric features -> regularized residual."""
    def __init__(self,task='planar_xy_theta',alpha=1.):
        self.task,self.alpha=task,alpha

    def fit(self,features,targets):
        x=np.asarray(features,dtype=float);y=np.asarray(targets,dtype=float)
        self.mean=x.mean(axis=0);self.std=np.maximum(x.std(axis=0),1e-3)
        x=(x-self.mean)/self.std
        scales=np.array([.05,.05,1.,1.]) if self.task=='planar_xy_theta' else np.array([.05]*3+[1.]*3)
        self.scales=scales
        residual=(y-base_prediction(features,self.task))/scales
        self.intercept=residual.mean(axis=0)
        self.coef=np.linalg.solve(x.T@x+self.alpha*np.eye(x.shape[1]),x.T@(residual-self.intercept))
        return self

    def predict(self,features):
        x=(np.asarray(features)-self.mean)/self.std
        pred=base_prediction(features,self.task)+(x@self.coef+self.intercept)*self.scales
        return normalize_prediction(pred,self.task)

    def document(self):
        return {'task':self.task,'alpha':self.alpha,**{k:getattr(self,k).tolist() for k in ('mean','std','scales','intercept','coef')}}

    @classmethod
    def from_document(cls,value):
        obj=cls(value['task'],value['alpha'])
        for k in ('mean','std','scales','intercept','coef'):setattr(obj,k,np.asarray(value[k]))
        return obj


class PointNetResidual(nn.Module):
    def __init__(self,task,global_mean=None,global_std=None):
        super().__init__()
        if task not in TASKS:raise ValueError('Unknown task')
        self.task=task
        self.register_buffer('global_mean',torch.zeros(GLOBAL_DIM) if global_mean is None else torch.as_tensor(global_mean,dtype=torch.float32))
        self.register_buffer('global_std',torch.ones(GLOBAL_DIM) if global_std is None else torch.as_tensor(global_std,dtype=torch.float32).clamp_min(1e-3))
        blocks=[]
        for inc,out in ((POINT_DIM,32),(32,64),(64,128)):
            blocks.extend((nn.Conv1d(inc,out,1),nn.GroupNorm(8,out),nn.SiLU()))
        self.points=nn.Sequential(*blocks)
        self.head=nn.Sequential(nn.Linear(256+GLOBAL_DIM,128),nn.SiLU(),nn.Dropout(.1),
            nn.Linear(128,64),nn.SiLU(),nn.Linear(64,4 if task==TASKS[0] else 6))
        nn.init.zeros_(self.head[-1].weight);nn.init.zeros_(self.head[-1].bias)

    def forward(self,points,global_features):
        hidden=self.points(points.transpose(1,2))
        pooled=torch.cat((hidden.amax(dim=-1),hidden.mean(dim=-1)),dim=-1)
        g=(global_features-self.global_mean)/self.global_std
        residual=self.head(torch.cat((pooled,g.clamp(-10,10)),dim=-1))
        if self.task=='planar_xy_theta':
            direction=F.normalize(global_features[:,6:8],dim=-1)
            xy=global_features[:,:2]+.05*residual[:,:2]
            return torch.cat((xy,F.normalize(direction+residual[:,2:],dim=-1,eps=1e-6)),dim=-1)
        xyz=global_features[:,:3]+.05*residual[:,:3]
        normal=F.normalize(global_features[:,6:9]+residual[:,3:],dim=-1,eps=1e-6)
        return torch.cat((xyz,normal),dim=-1)


def partial_loss(prediction,target,task):
    dims=2 if task=='planar_xy_theta' else 3
    position=F.smooth_l1_loss((prediction[:,:dims]-target[:,:dims])/.05,
        torch.zeros_like(target[:,:dims]),beta=1.)
    angle=(1-(prediction[:,dims:]*F.normalize(target[:,dims:],dim=-1)).sum(dim=-1).clamp(-1,1)).mean()
    return position+10*angle


def pose_metrics(prediction,target,task):
    p,t=np.asarray(prediction,dtype=float),np.asarray(target,dtype=float)
    if not len(p):return {'samples':0}
    dims=2 if task=='planar_xy_theta' else 3
    distances=np.linalg.norm(p[:,:dims]-t[:,:dims],axis=1)
    if task=='planar_xy_theta':
        angles=angular_error_degrees(np.rad2deg(np.arctan2(p[:,3],p[:,2])),np.rad2deg(np.arctan2(t[:,3],t[:,2])))
    else:
        pn=p[:,3:]/np.maximum(np.linalg.norm(p[:,3:],axis=1,keepdims=True),1e-8)
        tn=t[:,3:]/np.maximum(np.linalg.norm(t[:,3:],axis=1,keepdims=True),1e-8)
        angles=np.rad2deg(np.arccos(np.clip((pn*tn).sum(axis=1),-1,1)))
    result={'samples':len(p),'position_mae_mm':float(distances.mean()*1000),
        'position_median_mm':float(np.median(distances)*1000),'position_rmse_mm':float(np.sqrt((distances**2).mean())*1000),
        'position_p90_mm':float(np.percentile(distances,90)*1000),
        'angle_mae_degrees':float(angles.mean()),'angle_median_degrees':float(np.median(angles)),
        'success_50mm_5deg':float(((distances<=.05)&(angles<=5.)).mean()),
        'selection_score':float(distances.mean()/.05+angles.mean()/5.)}
    for i,axis in enumerate('xyz'[:dims]):result[axis+'_mae_mm']=float(np.abs(p[:,i]-t[:,i]).mean()*1000)
    return result


def grouped_metrics(prediction,targets,rows,task):
    groups=sorted({r['group_id'] for r in rows})
    by_group={}
    for group in groups:
        indices=[i for i,r in enumerate(rows) if r['group_id']==group]
        by_group[group]=pose_metrics(np.asarray(prediction)[indices],np.asarray(targets)[indices],task)
    overall=pose_metrics(prediction,targets,task)
    overall['groups']=len(groups)
    overall['macro_group_selection_score']=float(np.mean([v['selection_score'] for v in by_group.values()])) if groups else None
    return {'overall':overall,'by_group':by_group}


def group_folds(rows,count=3,seed=42):
    import hashlib
    counts={g:sum(r['group_id']==g for r in rows) for g in {r['group_id'] for r in rows}}
    if len(counts)<count:raise ValueError('Not enough independent groups for CV')
    groups=sorted(counts,key=lambda g:(-counts[g],hashlib.sha256(f'{seed}:{g}'.encode()).hexdigest()))
    bins=[[] for _ in range(count)];sizes=[0]*count
    for g in groups:
        b=min(range(count),key=lambda i:(sizes[i],i));bins[b].append(g);sizes[b]+=counts[g]
    return [([i for i,r in enumerate(rows) if r['group_id'] not in groups],
             [i for i,r in enumerate(rows) if r['group_id'] in groups]) for groups in bins]
