"""Native-image detection -> registered XYZI ROI -> partial-pose prediction.

No ground-truth box, pose, source timestamp, group ID or semantic class label
is accepted as an inference input. Outputs are research estimates, not robot
commands. The caller explicitly chooses the partial task and unit policy.
"""
from __future__ import annotations

import argparse
import hashlib
import json
from pathlib import Path

import numpy as np
import torch

from .pose_model import TASKS, PointNetResidual, RidgeResidual, base_prediction, roi_features


def digest_file(path):
    sha=hashlib.sha256()
    with Path(path).open('rb') as stream:
        for block in iter(lambda:stream.read(4*1024*1024),b''):sha.update(block)
    return sha.hexdigest()


def bundle_path(root,name):
    path=(root/name).resolve()
    if Path(name).is_absolute() or not path.is_relative_to(root.resolve()):
        raise ValueError('Unsafe model bundle path')
    return path


def partial_document(prediction,task):
    p=np.asarray(prediction,dtype=float)
    if not np.isfinite(p).all():raise ValueError('Nonfinite model output')
    document={'x':float(p[0]),'y':float(p[1]),'z':None,'theta_degrees':None,
        'normal':None,'roll':None,'pitch':None,'yaw':None,'quaternion':None,
        'coordinate_frame':'source_annotation_reference_not_unified_robot_frame',
        'full_six_dof':False,'robot_command_eligible':False}
    if task==TASKS[0]:document['theta_degrees']=float(np.rad2deg(np.arctan2(p[3],p[2])))
    elif task==TASKS[1]:document.update(z=float(p[2]),normal=p[3:6].tolist())
    else:raise ValueError('Unknown partial task')
    return document


class PalletPosePredictor:
    def __init__(self,bundle,device='cpu',*,allow_unverified_units=False):
        self.root=Path(bundle).resolve()
        self.spec=json.loads((self.root/'inference_config.json').read_text())
        self.device=torch.device(device)
        self.allow_unverified_units=allow_unverified_units
        for name,expected in self.spec['files'].items():
            if digest_file(bundle_path(self.root,name))!=expected:raise ValueError('Model bundle hash mismatch')
        from ultralytics import YOLO
        self.detector=YOLO(str(bundle_path(self.root,self.spec['detector_weights'])))
        self.networks={}
        for task,name in self.spec['pointnet_weights'].items():
            saved=torch.load(bundle_path(self.root,name),map_location='cpu',weights_only=True)
            if saved['task']!=task:raise ValueError('Partial task/checkpoint mismatch')
            net=PointNetResidual(task);net.load_state_dict(saved['model'])
            self.networks[task]=net.to(self.device).eval()
        self.ridge=RidgeResidual.from_document(json.loads(bundle_path(self.root,self.spec['ridge_weights']).read_text()))

    def pose(self,points,mapping,box,width,height,*,task=TASKS[0],scale_to_m=None,family=None):
        if task not in TASKS:raise ValueError('Specify a supported partial-pose task')
        if scale_to_m is None and not self.allow_unverified_units:
            raise ValueError('Point units unverified; provide scale_to_m or explicitly allow research-only assumptions')
        f,g,metadata=roi_features(points,mapping,box,width,height,self.spec['pose_features'],
            scale_to_m=scale_to_m,seed=42)
        method=family or (self.spec['selected_planar_family'] if task==TASKS[0] else 'pointnet')
        if method=='geometry':pred=base_prediction(g[None],task)[0]
        elif method=='ridge':
            if task!=TASKS[0]:raise ValueError('No trained normal ridge model')
            pred=self.ridge.predict(g[None])[0]
        elif method=='pointnet':
            with torch.inference_mode():
                pred=self.networks[task](torch.from_numpy(f[None]).to(self.device),
                    torch.from_numpy(g[None]).to(self.device))[0].cpu().numpy()
        else:raise ValueError('Unknown model family')
        return {'prediction':pred.tolist(),'pose':partial_document(pred,task),'method':method,
            'task':task,'input_quality':metadata,'normal_generalization_evaluated':False if task==TASKS[1] else None,
            'global_robot_calibration_verified':False}

    def detect(self,image):
        if image.ndim not in (2,3):raise ValueError('Expected native image grid')
        if image.ndim==2:image=np.repeat(image[:,:,None],3,axis=2)
        result=self.detector.predict(image,imgsz=self.spec['detector_imgsz'],
            conf=self.spec['detector_conf'],iou=self.spec['detector_nms_iou'],
            max_det=self.spec['detector_max_det'],device=str(self.device),verbose=False)[0]
        if result.boxes is None:return []
        boxes=result.boxes.xyxy.cpu().numpy();scores=result.boxes.conf.cpu().numpy()
        return [{'box_xywh':[float(b[0]),float(b[1]),float(b[2]-b[0]),float(b[3]-b[1])],
            'confidence':float(score)} for b,score in zip(boxes,scores)]

    def predict(self,image,points,mapping,*,task=TASKS[0],scale_to_m=None):
        if tuple(mapping.shape)!=tuple(image.shape[:2]):raise ValueError('Image/cloud registration mismatch')
        height,width=image.shape[:2]
        predictions=[]
        for box in self.detect(image):
            try:
                result=self.pose(points,mapping,box['box_xywh'],width,height,task=task,scale_to_m=scale_to_m)
                predictions.append({**box,**result,'error':None})
            except ValueError as error:
                predictions.append({**box,'pose':None,'prediction':None,'error':str(error)})
        return {'detections':predictions,'task':task,'full_six_dof':False,
            'robot_command_eligible':False,'ground_truth_used_for_inference':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,help='NPZ with points_xyzi and pixel_to_source_index')
    parser.add_argument('--output',type=Path,required=True)
    parser.add_argument('--task',choices=TASKS,default=TASKS[0])
    parser.add_argument('--device',default='cpu')
    parser.add_argument('--scale-to-m',type=float)
    parser.add_argument('--allow-unverified-units',action='store_true')
    args=parser.parse_args()
    import cv2
    image=cv2.imread(str(args.image),cv2.IMREAD_GRAYSCALE)
    if image is None:raise ValueError('Unable to read input image')
    with np.load(args.cloud,allow_pickle=False) as z:points,mapping=z['points_xyzi'],z['pixel_to_source_index']
    predictor=PalletPosePredictor(args.bundle,args.device,allow_unverified_units=args.allow_unverified_units)
    result=predictor.predict(image,points,mapping,task=args.task,scale_to_m=args.scale_to_m)
    if args.output.exists():raise ValueError('Refusing to overwrite an existing inference output')
    args.output.parent.mkdir(parents=True,exist_ok=True)
    args.output.write_text(json.dumps(result,ensure_ascii=False,indent=2,allow_nan=False)+'\n')
    print(json.dumps(result,ensure_ascii=False))


if __name__=='__main__':main()
