"""Read-only coverage of the user-confirmed metre sidecar and a real mm cloud."""
from collections import Counter
import json
from pathlib import Path
import sys

import numpy as np

PROJECT=Path(__file__).resolve().parents[1];sys.path.insert(0,str(PROJECT))
from pallet.cluster_targets import load_dataset,digest
from pallet.data.pcd import read_pcd
from pallet.fork_geometry import valid_xyz
from pallet.pose_review_geometry import GeometryEngine
from pallet.pose_review_units import MetrePolicy
from pallet.pose_refinement import atomic_json


def main():
    formal=Path(json.loads((PROJECT/'configs/active_formal_dataset.json').read_text())['dataset'])
    annotations,sources=load_dataset(formal);config=PROJECT/'configs/pallet_pose_review_units_v1.json'
    policy=MetrePolicy.load(config,formal);assert policy
    counts=Counter();unknown_policies=set()
    for source in sources.values():
        sensor=policy.sensor(None,source);assert sensor['scale_to_m'] is not None and sensor['scale_to_m']>0
        kind='registered' if source['sensor_policy'].get('scale_to_mm') is not None else 'user_confirmed_native_mm' if source['size']==[240,96] else 'user_confirmed_native_m'
        counts[kind]+=1
        if source['sensor_policy'].get('scale_to_mm') is None:unknown_policies.add(source['sensor_policy']['policy_id'])
    sample=next(a for a in annotations if a['boxes'] and sources[a['frame_id']]['size']==[240,96])
    source=sources[sample['frame_id']];path=Path(source['source_paths']['pcd'])
    assert digest(path)==source['source_anchor_hash'];_,points=read_pcd(path)
    raw=points[valid_xyz(points[:,:3]),:3];converted=raw*.001
    out=PROJECT/'outputs/pallet_pose_review_validation_v12';out.mkdir(parents=True,exist_ok=True)
    engine=GeometryEngine(formal,out/'millimetre_sample_geometry')
    b=sample['boxes'][0];x,y,w,h=(b[k] for k in ('x','y','width','height'))
    meta=engine.compute('unit-audit-only',source,[[x,y],[x+w,y],[x+w,y+h],[x,y+h]],policy.sensor(None,source))
    with np.load(engine.cache/(meta['token']+'.npz')) as z:
        measured=z['xyz'].copy();ids=z['source_indices'].copy()
    np.testing.assert_allclose(measured,points[ids,:3]*.001,rtol=1e-6,atol=1e-8)
    assert meta['sensor']['scale_to_m']==.001 and not meta['sensor']['axes_confirmed']
    atomic_json(out/'unit_policy_coverage.json',dict(state='passed',unit_policy=policy.info(),source_frames=len(sources),
        frame_counts=dict(counts),resolved_unknown_sensor_policies=len(unknown_policies),
        sample=dict(frame_id=source['frame_id'],native_scale_to_m=.001,source_PCD_sha256=digest(path),
            raw_scene_radius_median=float(np.median(np.linalg.norm(raw,axis=1))),
            converted_scene_radius_median_m=float(np.median(np.linalg.norm(converted,axis=1))),
            target_ROI_radius_median_m=float(np.median(np.linalg.norm(measured,axis=1))),
            source_indices_and_converted_XYZ_verified=True),source_files_modified=False))
    print(json.dumps(dict(state='passed',source_frames=len(sources),counts=dict(counts))),flush=True)


if __name__=='__main__':main()
