"""Portable box/registered-cloud or detector->cloud fork-face-center inference.

No human polygon, face selection, final ruler or pose target is accepted. Unknown
units require explicit --allow-native and never generate metric_pose_estimate.
"""
from __future__ import annotations

import argparse
import json
from pathlib import Path

import numpy as np
import torch

from .cluster_targets import digest
from .fork_geometry import DEFAULTS,decompose_roi
from .pose_refinement import (SCHEMA,REFERENCE,decoded,feature_values,
    load_predictors,predict_residual)
from .pose_review_geometry import basis_for,raster_mask


class RefinedPosePredictor:
    def __init__(self,bundle,device='cpu',allow_native=False):
        self.root=Path(bundle).resolve();self.device=device;self.allow_native=allow_native
        self.spec=json.loads((self.root/'bundle_config.json').read_text())
        if self.spec['schema']!=SCHEMA:raise ValueError('Unsupported refinement bundle')
        for name,sha in self.spec['files'].items():
            path=(self.root/name).resolve()
            if not path.is_relative_to(self.root) or digest(path)!=sha:raise ValueError('Invalid bundle file/hash')
        self.models=load_predictors(self.root,self.spec['profiles'],device)

    def pose(self,points,mapping,box,*,scale_to_m=None,domain):
        if scale_to_m is None and not self.allow_native:raise ValueError('Unknown units require explicit allow_native')
        if scale_to_m is not None and (not np.isfinite(scale_to_m) or scale_to_m<=0):raise ValueError('Invalid scale_to_m')
        if points.ndim!=2 or points.shape[1]!=4 or mapping.ndim!=2 or mapping.min()<0 or mapping.max()>=len(points):
            raise ValueError('Cloud registration invalid')
        branch='metric' if scale_to_m is not None else 'native_unknown';model=self.models[branch]
        if domain not in model['profile']['domains']:raise ValueError('Sensor domain outside this branch training')
        x,y,w,h=np.asarray(box,float);height,width=mapping.shape
        if min(w,h)<=0 or not np.isfinite(box).all():raise ValueError('Invalid original box')
        mask=raster_mask([[x,y],[x+w,y],[x+w,y+h],[x,y+h]],width,height)
        vv,uu=np.nonzero(mask)
        if not len(vv):raise ValueError('Box contains no pixels')
        x0,x1,y0,y1=uu.min(),uu.max()+1,vv.min(),vv.max()+1
        grid=points[mapping[y0:y1,x0:x1],:3].astype(float);grid[~mask[y0:y1,x0:x1]]=np.nan
        result=decompose_roi(grid,dict(DEFAULTS,ransac_max_iterations=180,maximum_planes_per_region=6),
            scale_to_mm=scale_to_m*1000 if scale_to_m is not None else None)
        ranked=sorted(result['faces'],key=lambda f:-f['points']*max(.01,abs(f['plane'][0]))**2*
            (.05 if f['status']!='selected_candidate' or abs(f['plane'][2])>.75 else 1.))
        if not ranked:raise ValueError('No automatic plane candidate')
        face=ranked[0]
        if abs(face['plane'][2])>.85:raise ValueError('Automatic candidate is not a fork face')
        uv=result['uv']+np.array([x0,y0]);ids=mapping[uv[:,1],uv[:,0]]
        xyzi=points[ids].astype(float);xyzi[:,:3]*=scale_to_m if scale_to_m is not None else 1.
        b=basis_for(face['plane'][:3],{'up_axis':2,'up_sign':1,'forward_axis':0,'forward_sign':1})
        p=xyzi[face['indices'],:3];local=p@b;low,high=np.quantile(local,[.01,.99],axis=0)
        center=b@np.array([(low[0]+high[0])/2,np.median(local[:,1]),np.median(local[:,2])])
        candidate={'center':center.tolist(),'basis':b.tolist(),'half_width':max(float((high[0]-low[0])/2),1e-8)}
        f,g,base,span,direction=feature_values(xyzi,uv,[width,height],dict(zip(('x','y','width','height'),box)),
            candidate,self.spec['seed'],self.spec['points'])
        residual=predict_residual(model,f[None,None],g[None,None],self.device)[0]
        if np.linalg.norm(residual[:2])>2:raise ValueError('Residual outside frozen limits')
        value=decoded(residual[None],base[None],np.array([span]),direction[None])[0]
        pose={'x':float(value[0]),'y':float(value[1]),'theta_degrees':float(np.degrees(np.arctan2(value[3],value[2]))),
            'z':None,'normal':None,'roll':None,'pitch':None,'quaternion':None,'reference_point':REFERENCE,
            'position_unit':'m' if branch=='metric' else 'native_unknown','source_scale_to_m':scale_to_m,
            'full_six_dof':False,'robot_command_eligible':False}
        return {'pose_estimate':pose,'metric_pose_estimate':pose if branch=='metric' else None,
            'branch':branch,'model_family':model['profile']['selected_family'],
            'human_pose_confirmed':False,'review_required':True,'robot_command_eligible':False,
            'automatic_plane_points':len(face['indices']),'automatic_ROI_points':len(ids)}

    def detect_and_pose(self,image,cloud,*,scale_to_m=None,domain):
        from ultralytics import YOLO
        with np.load(cloud,allow_pickle=False) as z:points,mapping=z['points_xyzi'],z['pixel_to_source_index']
        from PIL import Image
        with Image.open(image) as original:
            if original.size!=(mapping.shape[1],mapping.shape[0]):raise ValueError('Image/grid dimensions mismatch')
        detector=YOLO(str(self.root/'detector.pt'))
        detection=detector.predict(str(image),conf=.25,imgsz=640,device=self.device,verbose=False)[0]
        results=[]
        for box,confidence in zip(detection.boxes.xyxy.cpu().numpy(),detection.boxes.conf.cpu().numpy()):
            x,y,x1,y1=box;xywh=[float(x),float(y),float(x1-x),float(y1-y)]
            try:value=self.pose(points,mapping,xywh,scale_to_m=scale_to_m,domain=domain)
            except ValueError as error:value={'pose_estimate':None,'unavailable_reason':str(error),'review_required':True}
            results.append({'box_xywh':xywh,'detection_confidence':float(confidence),**value})
        return {'detections':results,'pose_reference':REFERENCE,'robot_command_eligible':False}


def main():
    parser=argparse.ArgumentParser(description=__doc__)
    parser.add_argument('--bundle',type=Path,required=True);parser.add_argument('--image',type=Path,required=True)
    parser.add_argument('--cloud',type=Path,required=True);parser.add_argument('--domain',required=True)
    parser.add_argument('--scale-to-m',type=float);parser.add_argument('--allow-native',action='store_true')
    parser.add_argument('--device',default='cpu');parser.add_argument('--output',type=Path,required=True)
    args=parser.parse_args();torch.set_num_threads(1)
    predictor=RefinedPosePredictor(args.bundle,args.device,args.allow_native)
    result=predictor.detect_and_pose(args.image,args.cloud,scale_to_m=args.scale_to_m,domain=args.domain)
    args.output.write_text(json.dumps(result,ensure_ascii=False,indent=2,allow_nan=False)+'\n')


if __name__=='__main__':main()
