"""Disclosed ROI/XY-yaw adaptations of pallet-specific geometric papers.

Xiao et al. (2017), DOI 10.1177/1729881417737799: plane regions and
occupancy-template matching. Li et al. (2021), DOI 10.1109/CACRE52464.2021.9501390:
fixed-box geometry backend, not its RGB detector. Shao et al. (2023), DOI
10.3390/s23031217: ISS, entropy-selected radius, Gaussian FPFH, SAC-IA and ICP.
These are independent adaptations, NOT author code or claims of exact replication.
"""
from __future__ import annotations

import numpy as np
from scipy.spatial import cKDTree
from scipy.sparse import coo_matrix
from scipy.sparse.csgraph import connected_components


def rotation(theta):
    c,s=np.cos(theta),np.sin(theta)
    return np.array([[c,-s,0.],[s,c,0.],[0.,0.,1.]])


def voxel_points(xyz, voxel=.015, maximum=1024):
    """Measured point nearest each voxel centroid (Shao §2.2.1)."""
    p=np.asarray(xyz,dtype=float)
    p=p[np.isfinite(p).all(1)]
    if len(p)<16:raise ValueError('insufficient_roi_points')
    _,inverse=np.unique(np.floor(p/voxel).astype(np.int64),axis=0,return_inverse=True)
    count=np.bincount(inverse)
    center=np.column_stack([np.bincount(inverse,weights=p[:,j])/count for j in range(3)])
    distance=np.linalg.norm(p-center[inverse],axis=1)
    order=np.lexsort((distance,inverse))
    ids=order[np.r_[True,inverse[order][1:]!=inverse[order][:-1]]]
    if len(ids)>maximum:
        ids=ids[np.random.default_rng(42).choice(len(ids),maximum,replace=False)]
    return p[ids]


def normals_and_eigenvalues(p,k=16):
    tree=cKDTree(p)
    distance,idx=tree.query(p,k=min(k,len(p)))
    local=p[idx]-p[idx].mean(1,keepdims=True)
    covariance=np.einsum('nki,nkj->nij',local,local)/local.shape[1]
    values,vectors=np.linalg.eigh(covariance)
    normals=vectors[:,:,0]
    # Consistent sensor-facing normals; no GT orientation used.
    normals*=np.where(np.einsum('ij,ij->i',normals,-p)>=0,1.,-1.)[:,None]
    return normals,np.maximum(values,0),tree,distance,idx


def plane_fit(p,threshold=.015,iterations=128):
    if len(p)<16:raise ValueError('too_few_plane_points')
    rng=np.random.default_rng(42)
    sample=p if len(p)<=1800 else p[rng.choice(len(p),1800,replace=False)]
    ids=rng.integers(0,len(sample),(iterations,3))
    a,b,c=(sample[ids[:,i]] for i in range(3))
    n=np.cross(b-a,c-a);length=np.linalg.norm(n,axis=1)
    keep=(length>1e-10)&(np.abs(n[:,0])>.2*length)&(np.abs(n[:,2])<.65*length)
    n=n[keep]/length[keep,None];a=a[keep]
    if not len(n):raise ValueError('no_front_plane_hypothesis')
    residual=np.abs(sample@n.T-np.einsum('ij,ij->i',a,n))
    score=(residual<threshold).sum(0)
    best=score.argmax();normal=n[best];origin=a[best]
    for _ in range(3):
        mask=np.abs((p-origin)@normal)<threshold
        if mask.sum()<16:raise ValueError('too_few_plane_inliers')
        origin=p[mask].mean(0)
        _,_,vt=np.linalg.svd(p[mask]-origin,full_matrices=False)
        normal=vt[-1]
    if normal[0]<0:normal=-normal
    if abs(normal[2])>.65 or normal[0]<.2:raise ValueError('invalid_front_plane')
    mask=np.abs((p-origin)@normal)<threshold
    return origin,normal,mask


def plane_regions(p,uv,threshold=.015):
    """Organized neighboring normals/point-to-plane region growing."""
    normals,_,_,_,_=normals_and_eigenvalues(p)
    uv=np.asarray(uv,dtype=int)
    shape=uv.max(0)+2
    grid=np.full((shape[1],shape[0]),-1,dtype=int)
    grid[uv[:,1],uv[:,0]]=np.arange(len(p))
    pairs=[]
    for delta in ((1,0),(0,1),(1,1),(-1,1)):
        q=uv+delta
        valid=(q>=0).all(1)&(q<shape).all(1)
        src=np.flatnonzero(valid);dest=grid[q[valid,1],q[valid,0]]
        exists=dest>=0;src,dest=src[exists],dest[exists]
        diff=p[dest]-p[src]
        smooth=(np.abs(np.einsum('ij,ij->i',normals[src],normals[dest]))>np.cos(np.deg2rad(18)))
        smooth &= np.abs(np.einsum('ij,ij->i',diff,normals[src]))<threshold
        smooth &= np.linalg.norm(diff,axis=1)<.15
        pairs.append((src[smooth],dest[smooth]))
    a=np.concatenate([v[0] for v in pairs]);b=np.concatenate([v[1] for v in pairs])
    graph=coo_matrix((np.ones(len(a)),(a,b)),shape=(len(p),len(p)))
    _,labels=connected_components(graph,directed=False)
    counts=np.bincount(labels);regions=[]
    for label in np.argsort(-counts)[:12]:
        if counts[label]<16:continue
        mask=labels==label;origin=p[mask].mean(0)
        _,_,vt=np.linalg.svd(p[mask]-origin,full_matrices=False)
        normal=vt[-1]
        if normal[0]<0:normal=-normal
        if normal[0]<.2 or abs(normal[2])>.65:continue
        regions.append((origin,normal,mask))
    return regions


def planar_coordinates(p,origin,normal):
    direction=normal.copy();direction[2]=0;direction/=np.linalg.norm(direction)
    lateral=np.cross(direction,[0.,0.,1.])
    basis=np.column_stack((lateral,direction,[0.,0.,1.]))
    return (p-origin)@basis,basis


def pila_geometry(p,threshold=.015):
    """Fixed-ROI plane/edge-line backend; RGB detector not reproduced."""
    origin,normal,mask=plane_fit(p,threshold)
    local,basis=planar_coordinates(p[mask],origin,normal)
    lo,hi=np.quantile(local[:,2],[.02,.98])
    edges=[]
    for lower,upper in zip(np.linspace(lo,hi,9)[:-1],np.linspace(lo,hi,9)[1:]):
        part=local[(local[:,2]>=lower)&(local[:,2]<=upper)]
        if len(part)<4:continue
        left,right=np.quantile(part[:,0],[.01,.99])
        edges.append((np.median(part[:,2]),left,right))
    if len(edges)<2:raise ValueError('insufficient_boundary_lines')
    edges=np.asarray(edges)
    # Least-squares vertical boundary lines, evaluated at measured mid-height.
    z=(lo+hi)/2
    left=np.polyval(np.polyfit(edges[:,0],edges[:,1],1),z)
    right=np.polyval(np.polyfit(edges[:,0],edges[:,2],1),z)
    if right-left<.04:raise ValueError('degenerate_width')
    # Project the width midpoint back to the fitted 3-D plane.
    w=(left+right)/2
    target=origin+basis@np.array([w,0.,z])
    if abs(normal@basis[:,1])>1e-8:
        target-=basis[:,1]*(normal@(target-origin))/(normal@basis[:,1])
    return np.r_[target[:2],np.arctan2(basis[1,1],basis[0,1])],dict(inliers=int(mask.sum()),width=float(right-left))


def occupancy(p,shape=(32,12),bounds=None):
    points=np.asarray(p)
    if bounds is None:bounds=np.quantile(points,[.01,.99],axis=0)
    lo,hi=np.asarray(bounds)
    scaled=(points-lo)/np.maximum(hi-lo,1e-6)
    ij=np.floor(scaled*np.array(shape)).astype(int)
    good=((ij>=0)&(ij<shape)).all(1)
    grid=np.zeros(shape,dtype=float)
    grid[ij[good,0],ij[good,1]]=1
    return grid


def xiao_template(p,uv,templates,threshold=.015):
    regions=plane_regions(p,uv,threshold)
    if not regions:raise ValueError('no_region_growing_front_plane')
    best=None
    for origin,normal,mask in regions[:6]:
        local,basis=planar_coordinates(p[mask],origin,normal)
        lo,hi=np.quantile(local[:,[0,2]],[.01,.99],axis=0)
        width,height=hi-lo
        if width<.04 or height<.015:continue
        midpoint=(lo+hi)/2
        for t in templates:
            tw,th=t['dimensions']
            if not .5<width/tw<1.8 or not .3<height/th<3.:continue
            for shift in np.linspace(-.08,.08,9):
                center=midpoint+np.array([shift,0.])
                bounds=np.array([center-[tw,th]/np.array([2.,2.]),center+[tw,th]/np.array([2.,2.])])
                grid=occupancy(local[:,[0,2]],bounds=bounds)
                target=t['grid']
                # Balanced solid/empty agreement, not just positive occupancy.
                pos=target>.5;neg=~pos
                similarity=.5*(grid[pos].mean() if pos.any() else 0.)+.5*(1-grid[neg].mean() if neg.any() else 0.)
                similarity-=.08*abs(np.log(width/tw))
                if best is None or similarity>best[0]:
                    point=origin+basis@np.array([center[0],0.,center[1]])
                    best=(similarity,point,basis[:,1],t['index'],mask.sum())
    if best is None:raise ValueError('no_compatible_structure_template')
    score,point,direction,index,inliers=best
    if score<.35:raise ValueError('template_similarity_below_threshold')
    return np.r_[point[:2],np.arctan2(direction[1],direction[0])],dict(similarity=float(score),template=index,inliers=int(inliers))


def iss_keypoints(p,tree,spacing,maximum=96):
    """Inverse-distance covariance eigenvalue ratios, then nonmaximum suppression."""
    groups=tree.query_ball_point(p,6*spacing)
    candidates=[]
    for i,idx in enumerate(groups):
        idx=np.asarray(idx);idx=idx[idx!=i]
        if len(idx)<5:continue
        diff=p[idx]-p[i];weight=1/np.maximum(np.linalg.norm(diff,axis=1),1e-10)
        val=np.linalg.eigvalsh((diff.T*weight)@diff/weight.sum())
        if val[2]>1e-12 and val[1]/val[2]<.975 and val[0]/max(val[1],1e-12)<.975:
            candidates.append((float(val[0]),i))
    selected=[];blocked=np.zeros(len(p),bool)
    for _,i in sorted(candidates,reverse=True):
        if blocked[i]:continue
        selected.append(i);blocked[tree.query_ball_point(p[i],4*spacing)]=True
        if len(selected)>=maximum:break
    return np.asarray(selected,dtype=int)


def gaussian_weights(distance):
    """Shao Eq. 5; sigma floor is an explicitly disclosed numerical safeguard."""
    d=np.asarray(distance,dtype=float)
    sigma=max(float(d.std()),1e-6)
    return np.exp(-.5*((d-d.mean())/sigma)**2)/(np.sqrt(2*np.pi)*sigma)


def descriptors(p,adaptive=False):
    normals,_,tree,distance,_=normals_and_eigenvalues(p)
    nearest=distance[:,1]
    spacing=max(float(np.mean(nearest[nearest>1e-9])),1e-5)
    radii=np.full(len(p),5*spacing)
    if adaptive:
        radii[:]=2*spacing
        for i,point in enumerate(p):
            best=np.inf
            for radius in np.linspace(spacing,2*spacing,5):
                idx=tree.query_ball_point(point,radius)
                if len(idx)<4:continue
                local=p[idx]-p[idx].mean(0)
                eigen=np.maximum(np.linalg.eigvalsh(local.T@local/len(idx)),0)
                prob=eigen/max(eigen.sum(),1e-15)
                entropy=-np.sum(prob*np.log(np.maximum(prob,1e-15)))
                if entropy<best:best=entropy;radii[i]=radius
    neighbors=tree.query_ball_point(p,radii)
    spfh=np.zeros((len(p),33))
    for i,idx in enumerate(neighbors):
        idx=np.asarray(idx,dtype=int);idx=idx[idx!=i]
        if not len(idx):continue
        diff=p[idx]-p[i];d=np.linalg.norm(diff,axis=1)
        valid=d>1e-10;idx,diff,d=idx[valid],diff[valid],d[valid]
        unit=diff/np.maximum(d[:,None],1e-10)
        u=np.repeat(normals[i,None],len(idx),axis=0)
        v=np.cross(unit,u);v/=np.maximum(np.linalg.norm(v,axis=1,keepdims=True),1e-10)
        w=np.cross(u,v)
        alpha=np.einsum('ij,ij->i',v,normals[idx])
        phi=-unit@normals[i]
        theta=np.arctan2(np.einsum('ij,ij->i',w,normals[idx]),normals[idx]@normals[i])
        for j,(a,limits) in enumerate(((alpha,(-1,1)),(phi,(-1,1)),(theta,(-np.pi,np.pi)))):
            spfh[i,j*11:(j+1)*11]=np.histogram(a,bins=11,range=limits)[0]/max(len(idx),1)
    feature=spfh.copy()
    for i,idx in enumerate(neighbors):
        idx=np.asarray(idx,dtype=int);idx=idx[idx!=i]
        if not len(idx):continue
        d=np.linalg.norm(p[idx]-p[i],axis=1)
        weight=gaussian_weights(d) if adaptive else 1/np.maximum(d,1e-8)
        feature[i]+=(spfh[idx]*weight[:,None]).mean(0)
    feature/=np.maximum(np.linalg.norm(feature,axis=1,keepdims=True),1e-12)
    keys=iss_keypoints(p,tree,spacing)
    return feature,keys,radii


def rigid_fit(source,target):
    a,b=source.mean(0),target.mean(0)
    u,_,vt=np.linalg.svd((source-a).T@(target-b))
    r=vt.T@u.T
    if np.linalg.det(r)<0:vt[-1]*=-1;r=vt.T@u.T
    return r,b-r@a


def huber(distance,delta=.04):
    return np.where(distance<delta,.5*distance**2,delta*(distance-.5*delta))


def register(source,target,sf,tf,sk,tk,iterations=128):
    """SAC-IA sampled descriptor hypotheses, then unconstrained rigid ICP."""
    if min(len(sk),len(tk))<3:raise ValueError('insufficient_ISS_keypoints')
    tree=cKDTree(target)
    _,matches=cKDTree(tf[tk]).query(sf[sk],k=min(5,len(tk)))
    if matches.ndim==1:matches=matches[:,None]
    rng=np.random.default_rng(42);best=None
    for _ in range(iterations):
        ids=rng.choice(len(sk),3,replace=False)
        a=source[sk[ids]]
        if min(np.linalg.norm(a[0]-a[1]),np.linalg.norm(a[0]-a[2]),np.linalg.norm(a[1]-a[2]))<.03:continue
        chosen=tk[matches[ids,rng.integers(0,matches.shape[1],3)]]
        if len(np.unique(chosen))<3:continue
        r,t=rigid_fit(a,target[chosen])
        # Declared source +X fork direction/+Z upright prior, not GT routing.
        if r[0,0]<.15 or r[2,2]<.7:continue
        d,_=tree.query(source@r.T+t)
        score=float(huber(d).mean())
        if best is None or score<best[0]:best=(score,r,t)
    if best is None:raise ValueError('SAC_IA_no_valid_hypothesis')
    _,r,t=best
    steps=0
    for steps in range(40):
        transformed=source@r.T+t
        d,idx=tree.query(transformed)
        good=d<.08
        if good.sum()<12:raise ValueError('ICP_insufficient_correspondences')
        dr,dt=rigid_fit(transformed[good],target[idx[good]])
        r,t=dr@r,dr@t+dt
        if np.linalg.norm(dt)<1e-6 and np.linalg.norm(dr-np.eye(3))<1e-5:break
    d,_=tree.query(source@r.T+t)
    if r[0,0]<.15 or r[2,2]<.7 or (d<.08).mean()<.3:
        raise ValueError('ICP_rejected_alignment')
    return r,t,float(huber(d).mean()),steps+1


def registration_pose(query,templates,adaptive=True):
    tag='agwf' if adaptive else 'fpfh'
    p=query['points'];features=query[tag];keys=query[tag+'_keys']
    # Geometry-only shortlist; neither labels nor IDs may choose a template.
    dims=np.quantile(p,[.01,.99],axis=0).ptp(axis=0)[[1,2]]
    ranked=sorted(templates,key=lambda t:np.abs(np.log(np.maximum(dims,.01)/np.maximum(t['dimensions'],.01))).sum())[:2]
    best=None;errors=[]
    for template in ranked:
        try:
            r,t,score,steps=register(template['points'],p,template[tag],features,
                template[tag+'_keys'],keys)
            if best is None or score<best[0]:best=(score,r,t,template['index'],steps)
        except ValueError as exc:errors.append(str(exc))
    if best is None:raise ValueError('registration_failed:'+';'.join(sorted(set(errors))))
    score,r,t,index,steps=best
    return np.r_[t[:2],np.arctan2(r[1,0],r[0,0])],dict(template=index,huber=score,ICP_steps=steps,ISS_keypoints=len(keys))
