"""Inference for the best learned v3 benchmark models, without label access."""
import argparse
import hashlib
import json
from pathlib import Path

import numpy as np
import torch

from . import pose_benchmark_models as architectures
from .pose_benchmark import EndToEnd
from .pose_refinement import feature_values, decoded, REFERENCE
from .pose_review_geometry import raster_mask, basis_for
from .fork_geometry import decompose_roi, DEFAULTS, valid_xyz


def automatic_input(points, mapping, box, scale_to_m, route):
    points=np.asarray(points);mapping=np.asarray(mapping)
    if points.ndim!=2 or points.shape[1]!=4 or mapping.ndim!=2 or not np.issubdtype(mapping.dtype,np.integer):
        raise ValueError('Expected points_xyzi[N,4] and integer pixel_to_source_index[H,W]')
    if mapping.min()<0 or mapping.max()>=len(points):raise ValueError('Invalid point registration')
    if scale_to_m is not None and (not np.isfinite(scale_to_m) or scale_to_m<=0):raise ValueError('Invalid scale')
    height,width=mapping.shape
    x,y,w,h=np.asarray(box,float)
    if not np.isfinite(box).all() or min(w,h)<=0:raise ValueError('Invalid box')
    b=dict(zip(('x','y','width','height'),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.))
    factor=scale_to_m if scale_to_m is not None else 1.
    ids=mapping[vv,uu];good=valid_xyz(points[ids,:3]);ids=ids[good]
    full_uv=np.column_stack((uu,vv))[good]
    full=points[ids].astype(float);full[:,:3]*=factor
    if len(full)<16:raise ValueError('Fewer than 16 valid original-box points')
    fallback=not ranked or abs(ranked[0]['plane'][2])>.85
    if fallback:
        if route!='roi':raise ValueError('No valid automatic fork plane')
        center=np.median(full[:,:3],axis=0)
        span=max(float(np.quantile(full[:,1],.99)-np.quantile(full[:,1],.01)),1e-8)
        direction=np.array([1.,0.]);basis=np.column_stack(([0,1,0],[1,0,0],[0,0,1]))
        candidate=dict(center=center,half_width=span/2,basis=basis)
        return feature_values(full,full_uv,[width,height],b,candidate,42,512)
    face=ranked[0];uv=result['uv']+np.array([x0,y0]);ids=mapping[uv[:,1],uv[:,0]]
    xyzi=points[ids].astype(float);xyzi[:,:3]*=factor
    basis=basis_for(face['plane'][:3],dict(up_axis=2,up_sign=1,forward_axis=0,forward_sign=1))
    local=xyzi[face['indices'],:3]@basis;low,high=np.quantile(local,[.01,.99],axis=0)
    center=basis@np.array([(low[0]+high[0])/2,np.median(local[:,1]),np.median(local[:,2])])
    candidate=dict(center=center,basis=basis,half_width=max(float((high[0]-low[0])/2),1e-8))
    f,g,base,span,direction=feature_values(xyzi,uv,[width,height],b,candidate,42,512)
    if route=='roi':
        f,_,_,_,_=feature_values(full,full_uv,[width,height],b,candidate,42,512)
    return f,g,base,span,direction


class BenchmarkPosePredictor:
    def __init__(self,bundle,device='cuda',allow_native=False):
        self.root=Path(bundle).resolve();self.device=device;self.allow_native=allow_native
        self.config=json.loads((self.root/'bundle_config.json').read_text())
        for name,expected in self.config['files'].items():
            path=(self.root/name).resolve()
            if not path.is_relative_to(self.root) or hashlib.sha256(path.read_bytes()).hexdigest()!=expected:
                raise ValueError('Bundle hash mismatch: '+name)
        architectures.ROOT=self.root
        architectures.UPSTREAM=self.root/'upstream'
        self.models={}
        for branch,spec in self.config['profiles'].items():
            state=torch.load(self.root/spec['checkpoint'],map_location='cpu',weights_only=True)
            if state['frozen_encoder']:
                backbone=architectures.Backbone(spec['family']).to(device).eval()
                weights=state['model'];head=architectures.PoseHead(len(weights['mean']),weights['mean'],weights['std'])
                head.load_state_dict(weights,strict=True)
                self.models[branch]=(backbone,head.to(device).eval())
            else:
                model=EndToEnd(spec['family'],np.zeros((2,16),np.float32),tune=spec['tune'])
                model.load_state_dict(state['model'],strict=True)
                self.models[branch]=(None,model.to(device).eval())

    def residual(self,f,g,branch):
        backbone,model=self.models[branch]
        with torch.no_grad():
            ft=torch.as_tensor(f,dtype=torch.float32,device=self.device).unsqueeze(0)
            gt=torch.as_tensor(g,dtype=torch.float32,device=self.device).unsqueeze(0)
            pred=model(torch.cat((backbone(ft),gt),-1)) if backbone is not None else model(ft,gt)
        value=pred.cpu().numpy()[0]
        if not np.isfinite(value).all():raise ValueError('Nonfinite prediction')
        return value

    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 --allow-native')
        branch='metric' if scale_to_m is not None else 'native_unknown'
        spec=self.config['profiles'][branch]
        if domain not in spec['domains']:raise ValueError('Domain not covered by this unit branch')
        f,g,base,span,direction=automatic_input(points,mapping,box,scale_to_m,spec['route'])
        pred=self.residual(f,g,branch)
        value=decoded(pred,base,np.asarray(span),direction)
        return dict(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,
                    model_family=spec['family'],route=spec['route'],position_unit='m' if branch=='metric' else 'native_unknown',
                    source_scale_to_m=scale_to_m,human_pose_confirmed=False,review_required=True,
                    full_six_dof=False,robot_command_eligible=False)


def main():
    parser=argparse.ArgumentParser(description=__doc__)
    parser.add_argument('--bundle',required=True,type=Path)
    parser.add_argument('--cloud',required=True,type=Path)
    parser.add_argument('--box',required=True,nargs=4,type=float,metavar=('X','Y','W','H'))
    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='cuda')
    parser.add_argument('--output',required=True,type=Path)
    args=parser.parse_args()
    predictor=BenchmarkPosePredictor(args.bundle,args.device,args.allow_native)
    with np.load(args.cloud,allow_pickle=False) as z:
        result=predictor.pose(z['points_xyzi'],z['pixel_to_source_index'],args.box,
                              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()
