"""Organized-cloud surface proposals. No interpolation or learned segmentation.

Indices always refer to measured points. Convex support is used only to identify
parallel surfaces behind apertures, never to rasterize the output mask.
"""
from __future__ import annotations

import math
import warnings

import cv2
import numpy as np
from scipy.sparse import coo_matrix
from scipy.sparse.csgraph import connected_components


DEFAULTS = {
    'seed': 42, 'neighbor_multiplier': 3.0, 'normal_angle_degrees': 25.0,
    'tolerance_spacing_ratio': 0.5, 'minimum_tolerance_mm': 2.0,
    'maximum_tolerance_mm': 20.0, 'minimum_region_points': 16,
    'minimum_face_points': 32, 'ransac_max_iterations': 1000,
    'ransac_probability': 0.999, 'hypothesis_points': 8192,
    'maximum_planes_per_region': 12, 'coplanar_angle_degrees': 8.0,
    'background_parallel_angle_degrees': 25.0, 'background_fraction': 0.7,
    'ownership_residual_margin': 0.2,
    'normal_window_radius': 2, 'normal_minimum_neighbors': 6,
    'normal_maximum_eigenvalue_ratio': 0.25,
}


def valid_xyz(xyz):
    return np.isfinite(xyz).all(axis=-1) & (np.linalg.norm(np.nan_to_num(xyz), axis=-1) > 1e-8)


def _pair_slices(h, w, dy, dx):
    a = (slice(max(0, -dy), min(h, h-dy)), slice(max(0, -dx), min(w, w-dx)))
    b = (slice(max(0, dy), min(h, h+dy)), slice(max(0, dx), min(w, w+dx)))
    return a, b


def organized_features(xyz, config, scale_to_mm=None, origin=None):
    """8-neighbor spacing and discontinuity-bounded tangent normals."""
    xyz = np.asarray(xyz, dtype=np.float64)
    good = valid_xyz(xyz)
    p = xyz * (scale_to_mm if scale_to_mm is not None else 1.)
    h, w = good.shape
    distances = np.full((8, h, w), np.nan)
    for i, (dy, dx) in enumerate(((-1,-1),(-1,0),(-1,1),(0,-1),(0,1),(1,-1),(1,0),(1,1))):
        a, b = _pair_slices(h, w, dy, dx)
        d = np.linalg.norm(p[b]-p[a], axis=-1)
        distances[i][a] = np.where(good[a] & good[b] & (d > 1e-12), d, np.nan)
    with warnings.catch_warnings():
        warnings.simplefilter('ignore', RuntimeWarning)
        spacing = np.nanmedian(distances, axis=0)
    samples = spacing[np.isfinite(spacing) & good & (spacing > 0)]
    fallback = float(np.median(samples)) if len(samples) else 1.
    spacing = np.where(np.isfinite(spacing) & (spacing > 0), spacing, fallback)
    tolerance = spacing * config['tolerance_spacing_ratio']
    if scale_to_mm is not None:
        tolerance = np.clip(tolerance, config['minimum_tolerance_mm'], config['maximum_tolerance_mm'])
    tolerance = np.maximum(tolerance, 1e-10)
    # Local covariance is less sensitive to ToF edge noise than two raw pixel
    # derivatives. Center-relative moments avoid cancellation at long range.
    counts = np.zeros((h,w),np.int16)
    first = np.zeros((h,w,3),np.float64)
    second = np.zeros((h,w,3,3),np.float64)
    radius = config['normal_window_radius']
    for dy in range(-radius,radius+1):
        for dx in range(-radius,radius+1):
            a,b = _pair_slices(h,w,dy,dx)
            delta = p[b]-p[a]
            keep = good[a]&good[b]&(np.linalg.norm(delta,axis=-1)<=config['neighbor_multiplier']*np.minimum(spacing[a],spacing[b]))
            safe_delta=np.where(keep[...,None],delta,0.)
            counts[a]+=keep
            first[a]+=safe_delta
            second[a]+=safe_delta[..., :,None]*safe_delta[...,None,:]
    mean=first/np.maximum(counts[...,None],1)
    covariance=second/np.maximum(counts[...,None,None],1)-mean[..., :,None]*mean[...,None,:]
    values,vectors=np.linalg.eigh(covariance)
    normals=vectors[..., :,0]
    reliable=(good&(counts>=config['normal_minimum_neighbors'])
              &(values[...,1]>np.maximum(spacing**2*.025,1e-14))
              &(values[...,0]<=config['normal_maximum_eigenvalue_ratio']*values[...,1]))
    origin = np.zeros(3) if origin is None else np.asarray(origin)* (scale_to_mm if scale_to_mm is not None else 1.)
    rays = p-origin
    rays /= np.maximum(np.linalg.norm(rays, axis=-1)[...,None], 1e-20)
    a, b = _pair_slices(h, w, 1, 0)
    ray_delta = rays[b]-rays[a]
    pair = good[a] & good[b]
    vector = np.median(ray_delta[pair], axis=0) if pair.any() else np.zeros(3)
    vertical = vector/max(np.linalg.norm(vector), 1e-20)
    return p, good, spacing, tolerance, normals, reliable, rays, vertical


def neighbor_regions(p, good, spacing, normals, reliable, config):
    h, w = good.shape
    ids = np.full((h, w), -1, dtype=np.int32)
    ids[good] = np.arange(good.sum(), dtype=np.int32)
    edges_a, edges_b = [], []
    cos_limit = math.cos(math.radians(config['normal_angle_degrees']))
    for dy, dx in ((0,1),(1,-1),(1,0),(1,1)):
        a, b = _pair_slices(h, w, dy, dx)
        keep = good[a] & good[b]
        keep &= np.linalg.norm(p[b]-p[a], axis=-1) <= config['neighbor_multiplier']*np.minimum(spacing[a], spacing[b])
        cosine = np.abs(np.sum(normals[a]*normals[b], axis=-1))
        keep &= ~(reliable[a] & reliable[b]) | (cosine >= cos_limit)
        edges_a.append(ids[a][keep]); edges_b.append(ids[b][keep])
    n = int(good.sum())
    if not n:
        return np.empty(0, np.int32), 0
    aa, bb = np.concatenate(edges_a), np.concatenate(edges_b)
    graph = coo_matrix((np.ones(len(aa), np.uint8), (aa, bb)), shape=(n,n)).tocsr()
    count, labels = connected_components(graph, directed=False, return_labels=True)
    return labels, int(count)


def svd_plane(points):
    center = np.mean(points, axis=0)
    centered = points-center
    values, vectors = np.linalg.eigh(centered.T@centered/max(len(points), 1))
    if values[1] <= max(values[0]*1.01, values[0]+1e-14):
        return None  # A line or coincident returns do not identify a plane.
    n = vectors[:, 0]
    return np.r_[n, -center@n]


def ransac_plane(points, tolerances, rng, config):
    """Sample proposals on bounded data, refine and classify on every point."""
    n = len(points)
    if n < config['minimum_region_points']:
        return None, np.zeros(n, bool)
    sample = np.arange(n)
    if n > config['hypothesis_points']:
        sample = np.sort(rng.choice(n, config['hypothesis_points'], replace=False))
    q, t = points[sample], tolerances[sample]
    best, best_count, trials = None, 0, 0
    required = config['ransac_max_iterations']
    while trials < min(required, config['ransac_max_iterations']):
        batch = min(32, config['ransac_max_iterations']-trials)
        picks = rng.integers(0, len(q), size=(batch,3))
        x, y, z = q[picks[:,0]], q[picks[:,1]], q[picks[:,2]]
        cross = np.cross(y-x,z-x)
        norms = np.linalg.norm(cross, axis=1)
        edges = np.linalg.norm(y-x,axis=1)*np.linalg.norm(z-x,axis=1)
        usable = (edges > 1e-16) & (norms > edges*1e-6)
        if usable.any():
            normals = cross[usable]/norms[usable,None]
            offsets = -np.sum(normals*x[usable],axis=1)
            scores = (np.abs(q@normals.T+offsets[None]) <= t[:,None]).sum(axis=0)
            k = int(np.argmax(scores))
            if int(scores[k]) > best_count:
                best_count = int(scores[k]); best = np.r_[normals[k],offsets[k]]
                fraction = min(best_count/len(q),1-1e-12)
                required = max(32, math.ceil(math.log(1-config['ransac_probability'])/math.log(max(1-fraction**3,1e-12))))
        trials += batch
    if best is None:
        return None, np.zeros(n,bool)
    keep = np.abs(points@best[:3]+best[3]) <= tolerances
    for _ in range(3):
        if keep.sum() < config['minimum_region_points']:
            return None, np.zeros(n,bool)
        refined = svd_plane(points[keep])
        if refined is None:
            return None, np.zeros(n,bool)
        new_keep = np.abs(points@refined[:3]+refined[3]) <= tolerances
        best = refined
        if np.array_equal(keep,new_keep):
            break
        keep = new_keep
    return best, keep


def decompose_roi(xyz, config=None, scale_to_mm=None, origin=None, seed=42):
    config = dict(DEFAULTS if config is None else config)
    p, good, spacing, tolerance, normals, reliable, rays, vertical = organized_features(xyz,config,scale_to_mm,origin)
    vv, uu = np.nonzero(good)
    uv = np.column_stack([uu,vv]).astype(np.int32)
    points, ts, ss = p[good], tolerance[good], spacing[good]
    regions, count = neighbor_regions(p,good,spacing,normals,reliable,config)
    candidates = []
    rng = np.random.default_rng(seed)
    origin_work = np.asarray(np.zeros(3) if origin is None else origin)*(scale_to_mm if scale_to_mm is not None else 1.)
    sizes = np.bincount(regions,minlength=count)
    # Avoid scanning every point once per component: one stable sorting pass.
    order = np.argsort(regions,kind='stable'); boundaries = np.r_[0,np.cumsum(sizes)]
    exhausted_regions = 0
    for region in np.flatnonzero(sizes >= config['minimum_region_points']):
        remaining = order[boundaries[region]:boundaries[region+1]]
        for _ in range(config['maximum_planes_per_region']):
            plane, keep = ransac_plane(points[remaining],ts[remaining],rng,config)
            if plane is None or keep.sum() < config['minimum_region_points']:
                break
            inds = remaining[keep]
            if plane[:3]@(np.mean(points[inds],axis=0)-origin_work)>0:
                plane = -plane
            candidates.append({'indices':inds,'plane':plane,'regions':[int(region)]})
            remaining = remaining[~keep]
            if len(remaining) < config['minimum_region_points']:
                break
        else:
            if len(remaining) >= config['minimum_region_points']:
                exhausted_regions += 1
    # Connected pieces of the same measured face may be separated by a fork hole.
    merged = []
    for face in sorted(candidates,key=lambda f:(-len(f['indices']),int(f['indices'].min()))):
        compatible = None
        for prior in merged:
            angle = abs(float(face['plane'][:3]@prior['plane'][:3]))
            if angle < math.cos(math.radians(config['coplanar_angle_degrees'])):
                continue
            i,j=face['indices'],prior['indices']
            limit=2*float(np.median(np.r_[ts[i],ts[j]]))
            if (np.median(np.abs(points[i]@prior['plane'][:3]+prior['plane'][3])) <= limit
                    and np.median(np.abs(points[j]@face['plane'][:3]+face['plane'][3])) <= limit):
                together=np.r_[j,i]
                plane=svd_plane(points[together])
                if plane is not None and np.percentile(np.abs(points[together]@plane[:3]+plane[3])/ts[together],90)<=1.5:
                    if plane[:3]@(np.mean(points[together],axis=0)-origin_work)>0:plane=-plane
                    compatible=prior;prior['indices']=together;prior['plane']=plane
                    prior['regions']+=face['regions'];break
        if compatible is None:merged.append(face)
    faces=[]
    for face in merged:
        inds=face['indices'];plane=face['plane']
        # After merging, keep only measured points satisfying the final model.
        inds=inds[np.abs(points[inds]@plane[:3]+plane[3]) <= ts[inds]]
        if len(inds)<config['minimum_face_points']:continue
        face['indices']=np.sort(inds)
        face.update(status='selected_candidate',reasons=['forkability_not_confirmed'],
                    tolerance=float(np.median(ts[inds])),spacing=float(np.median(ss[inds])),
                    median_range=float(np.median(np.linalg.norm(points[inds]-origin_work,axis=1))))
        cosine=float(np.median(np.abs(rays[good][inds]@plane[:3])))
        vertical_cosine=abs(float(plane[:3]@vertical))
        face.update(view_normal_cosine=cosine, image_vertical_normal_cosine=vertical_cosine)
        if cosine<.2:face['reasons'].append('grazing_view')
        if vertical_cosine>.8 and cosine<.6:
            face['status']='diagnostic_only';face['reasons'].append('possible_top_or_ground_face')
        if float(reliable[good][inds].mean())<.5:face['reasons'].append('limited_reliable_normals')
        if scale_to_mm is None:face['reasons'].append('unit_unknown_relative_thresholds')
        faces.append(face)
    faces.sort(key=lambda f:(f['median_range'],-len(f['indices']),int(f['indices'].min())))
    for j,back in enumerate(faces):
        for front in faces[:j]:
            if front['status']!='selected_candidate':continue
            if abs(float(front['plane'][:3]@back['plane'][:3]))<math.cos(math.radians(config['background_parallel_angle_degrees'])):continue
            fu=uv[front['indices']].astype(np.float32)
            if np.ptp(fu[:,0])<.3*max(xyz.shape[1]-1,1):continue
            hull=cv2.convexHull(fu).reshape(-1,2)
            if len(hull)<3:continue
            # Point-in-convex-hull without filling holes or synthesizing points.
            bu=uv[back['indices']].astype(np.float64)
            edges=np.roll(hull,-1,axis=0)-hull
            delta=bu[:,None,:]-hull[None,:,:]
            signs=edges[None,:,0]*delta[:,:,1]-edges[None,:,1]*delta[:,:,0]
            inside=(signs>=-1e-5).all(axis=1)|(signs<=1e-5).all(axis=1)
            signed=points[back['indices']]@front['plane'][:3]+front['plane'][3]
            separation=max(3*front['tolerance'],2*front['spacing'])
            behind=(signed < -separation)&inside
            if behind.mean()>=config['background_fraction']:
                back['status']='rejected_background';back['reasons'].append('behind_parallel_front_support')
                back['occluding_face_index']=next(k for k,f in enumerate(faces) if f is front)
                break
    for k,face in enumerate(faces):
        residual=np.abs(points[face['indices']]@face['plane'][:3]+face['plane'][3])
        face.update(candidate_index=k,points=len(face['indices']),residual_median=float(np.median(residual)),
                    residual_p95=float(np.percentile(residual,95)),
                    maximum_point_tolerance=float(ts[face['indices']].max()),
                    regions=sorted(set(face['regions'])))
    return {'uv':uv,'xyz_valid_mask':good,'points_work':points,'tolerances':ts,'faces':faces,
            'regions':count,'regions_plane_limit_reached':exhausted_regions,
            'normal_valid_fraction':float(reliable[good].mean()) if good.any() else 0.,
            'geometry_units':'mm' if scale_to_mm is not None else 'raw_unknown'}


def resolve_ownership(point_uv, face_assignments, normalized_residuals, margin=.2):
    """Face ID >0: unique owner; -1: unresolved conflict; 0: no selected face."""
    assigned=np.asarray(face_assignments,dtype=np.int32).copy()
    active=np.flatnonzero(assigned>0)
    if not len(active):return assigned,np.empty((0,2),np.int32)
    uv=np.asarray(point_uv)
    _,groups,counts=np.unique(uv[active],axis=0,return_inverse=True,return_counts=True)
    order=np.argsort(groups,kind='stable');bounds=np.r_[0,np.cumsum(counts)]
    conflicts=[]
    for g in np.flatnonzero(counts>1):
        inds=active[order[bounds[g]:bounds[g+1]]]
        if len(set(assigned[inds]))==1:continue
        ranked=inds[np.argsort(np.asarray(normalized_residuals)[inds],kind='stable')]
        if normalized_residuals[ranked[1]]-normalized_residuals[ranked[0]]>margin:
            assigned[ranked[1:]]=0
        else:
            assigned[inds]=-1;conflicts.append(uv[inds[0]])
    return assigned,np.asarray(conflicts,dtype=np.int32).reshape(-1,2)
