"""Train-fold-only template construction, CPU paper comparisons and frozen choices."""
from __future__ import annotations

from concurrent.futures import ProcessPoolExecutor
import io
import json
import multiprocessing
from pathlib import Path
import time

import numpy as np

from . import pose_paper_data as data
from . import pose_paper_methods as methods
from .cluster_targets import digest
from .pose_dataset import protected_write, read_rows, write_rows, write_json
from .pose_refinement import now


def roi(key):
    path=data.ROOT/'dataset/roi'/(key+'.npz')
    with np.load(path,allow_pickle=False) as z:
        return z['xyz'].astype(float),z['uv'].copy()


def preprocess(key):
    path=data.ROOT/'paper_features'/(key+'.npz')
    meta_path=path.with_suffix('.json')
    if meta_path.exists():return json.loads(meta_path.read_text())
    start=time.perf_counter()
    try:
        p,_=roi(key)
        p=methods.voxel_points(p,data.config()['registration_voxel_m'],data.config()['registration_max_points'])
        origin,normal,mask=methods.plane_fit(p,.015)
        lo,hi=np.quantile(p[mask,2],[.01,.99])
        # Ground/load suppression from the automatic plane only, never human masks.
        keep=(np.abs((p-origin)@normal)<.12)&(p[:,2]>=lo-.03)&(p[:,2]<=hi+.03)
        p=p[keep]
        if len(p)<16:raise ValueError('insufficient_automatic_band_points')
        af,ak,ar=methods.descriptors(p,True)
        ff,fk,fr=methods.descriptors(p,False)
        stream=io.BytesIO()
        np.savez_compressed(stream,points=p,agwf=af,agwf_keys=ak,agwf_radii=ar,
            fpfh=ff,fpfh_keys=fk,fpfh_radii=fr)
        protected_write(path,stream.getvalue())
        meta=dict(target_key=key,state='complete',sha256=digest(path),points=len(p),
            seconds=time.perf_counter()-start,agwf_ISS=len(ak),fpfh_ISS=len(fk),
            human_labels_used=False)
    except (ValueError,OSError,np.linalg.LinAlgError) as exc:
        meta=dict(target_key=key,state='failed',reason=str(exc),seconds=time.perf_counter()-start)
    write_json(meta_path,meta)
    return meta


def cached(key):
    path=data.ROOT/'paper_features'/(key+'.npz')
    meta=json.loads(path.with_suffix('.json').read_text())
    if meta['state']!='complete':raise ValueError(meta['reason'])
    if digest(path)!=meta['sha256']:raise ValueError('Cached descriptors changed')
    with np.load(path,allow_pickle=False) as z:result={k:z[k].copy() for k in z.files}
    return result,meta['seconds']


def build_bank(name,fit_rows):
    folder=data.ROOT/'templates'/name
    if (folder/'manifest.json').exists():
        manifest=json.loads((folder/'manifest.json').read_text())
        if manifest['fit_keys']!=[r['target_key'] for r in fit_rows]:raise ValueError('Template cohort changed')
        for item in manifest['templates']:
            if digest(folder/item['file'])!=item['sha256']:raise ValueError('Template changed')
        return manifest
    if any(r['split']!='train' for r in fit_rows):raise ValueError('Holdout used for templates')
    candidates=[]
    for row in fit_rows:
        try:
            query,_=cached(row['target_key']);p=query['points']
            origin,normal,mask=methods.plane_fit(p,.015)
            local,_=methods.planar_coordinates(p[mask],origin,normal)
            lo,hi=np.quantile(local[:,[0,2]],[.01,.99],axis=0);dimensions=hi-lo
            if dimensions[0]<.1 or dimensions[1]<.025 or len(query['agwf_keys'])<3:continue
            candidates.append((row,dimensions,int(mask.sum())))
        except (ValueError,OSError):continue
    if not candidates:raise ValueError('No usable train-only templates')
    dims=np.array([c[1] for c in candidates]);log=np.log(np.maximum(dims,.01))
    chosen=[max(range(len(candidates)),key=lambda i:candidates[i][2])]
    for _ in range(min(data.config()['registration_templates'],len(candidates))-1):
        distance=np.min(((log[:,None]-log[chosen][None])**2).sum(-1),axis=1)
        distance[chosen]=-1
        used={candidates[i][0]['group_id'] for i in chosen}
        available=np.array([c[0]['group_id'] not in used for c in candidates])
        if available.any():distance[~available]=-1
        index=int(distance.argmax())
        if index in chosen:break
        chosen.append(index)
    records=[]
    for number,index in enumerate(chosen):
        row,dimensions,_=candidates[index]
        query,_=cached(row['target_key']);p=query['points']
        angle=np.deg2rad(row['pose']['theta_degrees']);r=methods.rotation(angle)
        # Train poses supply the reference origin/direction, just as labeled
        # templates/CAD require a known object frame. No human support mask used.
        center=np.r_[row['pose']['x'],row['pose']['y'],np.median(p[:,2])]
        canonical=(p-center)@r
        origin,normal,mask=methods.plane_fit(canonical,.015)
        if normal[0]<0:normal=-normal
        bounds=np.quantile(canonical[mask][:,[1,2]],[.01,.99],axis=0)
        grid=methods.occupancy(canonical[mask][:,[1,2]],bounds=bounds)
        # Occupancy coordinate sign matches planar_coordinates' cross(d,+Z)=-Y.
        grid=grid[::-1]
        stream=io.BytesIO()
        np.savez_compressed(stream,points=canonical,grid=grid,dimensions=dimensions,
            **{k:query[k] for k in ('agwf','agwf_keys','fpfh','fpfh_keys')})
        path=folder/f'{number}.npz';protected_write(path,stream.getvalue())
        records.append(dict(index=number,file=path.name,sha256=digest(path),
            source_train_target=row['target_key'],source_train_group=row['group_id'],
            reference='fork_face_width_center',manual_mask_used=False))
    manifest=dict(created_at=now(),fit_keys=[r['target_key'] for r in fit_rows],templates=records,
        input_keys_at_inference='XYZ_and_pixel_adjacency_only_no_target_or_group_routing',
        label_use='training_reference_frame_only_not_inference',robot_command_eligible=False)
    write_json(folder/'manifest.json',manifest)
    return manifest


def load_bank(name):
    folder=data.ROOT/'templates'/name
    manifest=json.loads((folder/'manifest.json').read_text());result=[]
    for item in manifest['templates']:
        path=folder/item['file']
        if digest(path)!=item['sha256']:raise ValueError('Template identity mismatch')
        with np.load(path,allow_pickle=False) as z:result.append(dict(index=item['index'],**{k:z[k].copy() for k in z.files}))
    return result


def prediction_job(job):
    key,method,threshold,bank=job
    start=time.perf_counter();extra=0.
    try:
        if method in ('shao_agwf','fpfh_ablation'):
            query,extra=cached(key)
            pose,evidence=methods.registration_pose(query,load_bank(bank),method=='shao_agwf')
        else:
            p,uv=roi(key)
            # Keep the exact organized adjacency for the Xiao region-growing route.
            if method=='pila_geometry':pose,evidence=methods.pila_geometry(p,threshold)
            else:pose,evidence=methods.xiao_template(p,uv,load_bank(bank),threshold)
        if not np.isfinite(pose).all():raise ValueError('nonfinite_output')
        record=dict(target_key=key,state='complete',pose=pose.tolist(),evidence=evidence)
    except (ValueError,OSError,np.linalg.LinAlgError) as exc:
        record=dict(target_key=key,state='failed',pose=None,reason=str(exc))
    record.update(seconds=time.perf_counter()-start+extra,robot_command_eligible=False)
    return record


def parallel_map(function,jobs,stage):
    result=[]
    with ProcessPoolExecutor(max_workers=data.config()['workers'],mp_context=multiprocessing.get_context('spawn')) as pool:
        for i,value in enumerate(pool.map(function,jobs,chunksize=4),1):
            result.append(value)
            if i==1 or i%100==0:print(json.dumps(dict(stage=stage,completed=i,total=len(jobs))),flush=True)
    return result


def selection_score(records,rows,widths):
    pred={r['target_key']:r for r in records};groups={}
    for row in rows:
        record=pred[row['target_key']]
        score=data.config()['preprocessing_failure_score']
        if record['state']=='complete':
            p=np.asarray(record['pose']);gt=np.array([row['pose']['x'],row['pose']['y']])
            distance=np.linalg.norm(p[:2]-gt)
            angle=abs((np.rad2deg(p[2])-row['pose']['theta_degrees']+180)%360-180)
            score=float(distance/widths[row['target_key']]/.05+angle/5)
        groups.setdefault(row['group_id'],[]).append(score)
    return float(np.mean([np.mean(v) for v in groups.values()]))


def cpu_cv():
    source_files=['pallet/pose_paper_methods.py','pallet/pose_paper_benchmark.py']
    protocol_path=data.ROOT/'paper_protocol.json'
    info=dict(created_at=now(),config_sha256=digest(data.CONFIG),protocol_sha256=digest(data.ROOT/'protocol.json'),
        sources={p:digest(data.PROJECT/p) for p in source_files},
        methods=data.config()['paper_methods'],geometry_thresholds_m=data.config()['plane_thresholds_m'],
        registration_threshold_m=.015,threshold_choices_for_registration=1,holdout_used=False)
    if protocol_path.exists():info['created_at']=json.loads(protocol_path.read_text())['created_at']
    write_json(protocol_path,info)
    labels=read_rows(data.ROOT/'snapshot/eligible.jsonl')
    # Unsupervised descriptors may be computed for all targets; holdout poses are
    # not used or scored here. Template fitting below strictly checks train keys.
    feature_path=data.ROOT/'paper_features/index.jsonl'
    if not feature_path.exists():
        values=parallel_map(preprocess,[r['target_key'] for r in labels],'paper_descriptors')
        write_rows(feature_path,values)
    rows,arrays,all_rows=data.load();bykey={r['target_key']:r for r in rows}
    widths={r['target_key']:float(w) for r,w in zip(rows,arrays['width'])}
    protocol=json.loads((data.ROOT/'protocol.json').read_text())
    entries=[]
    for method in data.config()['paper_methods']:
        thresholds=[.015] if method in ('shao_agwf','fpfh_ablation') else data.config()['plane_thresholds_m']
        for threshold in thresholds:
            records=[]
            for number,fold in enumerate(protocol['branches']['metric']['folds']):
                bank=f'fold_{number}'
                build_bank(bank,[bykey[k] for k in fold['fit_keys']])
                path=data.ROOT/'paper_CV'/method/f'{threshold:.3f}'/f'{number}.json'
                if path.exists():value=json.loads(path.read_text())
                else:
                    jobs=[(key,method,threshold,bank) for key in fold['validation_keys']]
                    value=parallel_map(prediction_job,jobs,f'CV/{method}/{threshold}/{number}')
                    write_json(path,value)
                records.extend(value)
            score=selection_score(records,all_rows,widths)
            entry=dict(method=method,threshold_m=threshold,score=score,records=len(records),
                failures=sum(r['state']!='complete' for r in records))
            entries.append(entry)
            print(json.dumps(dict(stage='CPU_CV_candidate',**entry)),flush=True)
    selected={m:min([e for e in entries if e['method']==m],key=lambda e:e['score']) for m in data.config()['paper_methods']}
    result=dict(created_at=now(),entries=entries,selected=selected,holdout_used=False,
        paper_protocol_sha256=digest(protocol_path))
    path=data.ROOT/'paper_selection.json'
    if path.exists():result['created_at']=json.loads(path.read_text())['created_at']
    write_json(path,result)
    build_bank('full_train',rows)
    return result


def freeze_all():
    paths=['paper_selection.json','learning_selection.json','protocol.json','paper_protocol.json']
    result=dict(created_at=now(),choices={p:digest(data.ROOT/p) for p in paths},
        holdout_metrics_accessed=False,previous_test_seen_not_blind=True,robot_command_eligible=False)
    path=data.ROOT/'selection_frozen.json'
    if path.exists():result['created_at']=json.loads(path.read_text())['created_at']
    write_json(path,result)


def evaluate():
    frozen=json.loads((data.ROOT/'selection_frozen.json').read_text())
    for path,expected in frozen['choices'].items():
        if digest(data.ROOT/path)!=expected:raise ValueError('Frozen selection changed')
    choices=json.loads((data.ROOT/'paper_selection.json').read_text())['selected']
    labels=read_rows(data.ROOT/'snapshot/eligible.jsonl')
    for method,choice in choices.items():
        for split in ('val','test'):
            path=data.ROOT/'predictions'/method/f'{split}.json'
            if path.exists():continue
            jobs=[(r['target_key'],method,choice['threshold_m'],'full_train') for r in labels if r['split']==split]
            records=parallel_map(prediction_job,jobs,f'{split}/{method}')
            write_json(path,records)


if __name__=='__main__':
    import argparse
    parser=argparse.ArgumentParser(description=__doc__)
    parser.add_argument('--stage',choices=['cv','freeze','evaluate'],required=True)
    args=parser.parse_args()
    {'cv':cpu_cv,'freeze':freeze_all,'evaluate':evaluate}[args.stage]()
