"""Read-only axis/sign evidence and before/after views; no live review writes."""
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 digest,load_dataset
from pallet.data.orientation import orient_grid
from pallet.data.pcd import read_pcd
from pallet.pose_review_geometry import basis_for,raster_mask
from pallet.pose_refinement import atomic_json


def fit(x,y):
    a,b=np.linalg.lstsq(np.c_[x,np.ones(len(x))],y,rcond=None)[0]
    residual=y-(a*x+b)
    return dict(slope=float(a),intercept=float(b),r2=float(1-np.sum(residual**2)/np.sum((y-y.mean())**2)),
                rmse_pixels=float(np.sqrt(np.mean(residual**2))))


def main():
    import matplotlib
    matplotlib.use('Agg')
    import matplotlib.pyplot as plt
    from PIL import Image
    out=PROJECT/'outputs/pallet_pose_review_validation_v13';out.mkdir(parents=True,exist_ok=True)
    if (out/'axis_audit.json').exists():raise ValueError('Do not overwrite axis evidence')
    formal=Path(json.loads((PROJECT/'configs/active_formal_dataset.json').read_text())['dataset'])
    annotations,sources=load_dataset(formal);rows={a['frame_id']:a for a in annotations};records=[];examples=[]
    wanted=['400mm/left','600mm/center','800mm/left','1000mm/right','1200mm/center','1400mm/left']
    found=set()
    for frame,source in sorted(sources.items()):
        if source['sensor_policy']['policy_id']!='2024_11_27|(96, 240)|primary':continue
        assert source['size']==[240,96] and source['pixel_rotation_degrees']==180
        path=Path(source['source_paths']['pcd']);assert digest(path)==source['source_anchor_hash']
        _,points=read_pcd(path);w,h=source['size'];grid=orient_grid(points[:,:3].reshape(h,w,3),source['sensor_policy'])
        vv,uu=np.mgrid[:h,:w];ok=np.isfinite(grid).all(2)&(grid[:,:,2]>50)
        xyz=grid[ok];u=uu[ok];v=vv[ok]
        horizontal=fit(xyz[:,0]/xyz[:,2],u);vertical=fit(xyz[:,1]/xyz[:,2],v)
        # The registered upright pixel grid has u increasing along -X and v
        # increasing along -Y. +Z has positive range; +Y points screen-up.
        assert horizontal['slope']<0 and vertical['slope']<0
        # Wide-angle distortion is not calibrated away: use this only to
        # distinguish axes/signs, and report the actual residuals below.
        assert horizontal['r2']>.95 and vertical['r2']>.95
        records.append(dict(frame_id=frame,source_pcd_sha256=source['source_anchor_hash'],valid_points=int(ok.sum()),
            horizontal_u_from_X_over_Z=horizontal,vertical_v_from_Y_over_Z=vertical,
            positive_Z_fraction=float(np.mean(grid[np.isfinite(grid).all(2)&(np.linalg.norm(grid,axis=2)>0),2]>0))))
        category='/'.join(path.parent.parts[-2:])
        if category in wanted and category not in found and rows[frame]['boxes']:
            found.add(category);b=rows[frame]['boxes'][0];x,y,bw,bh=(b[k] for k in ('x','y','width','height'))
            polygon=[[x,y],[x+bw,y],[x+bw,y+bh],[x,y+bh]]
            roi=raster_mask(polygon,w,h)&ok;examples.append((source,grid[roi]*.001,polygon))
    assert len(records)==300 and len(examples)==6
    fig,axs=plt.subplots(6,5,figsize=(19,16),layout='constrained')
    old=basis_for([1,0,0],dict(forward_axis=0,forward_sign=1,up_axis=2,up_sign=1))
    new=basis_for([0,0,1],dict(forward_axis=2,forward_sign=1,up_axis=1,up_sign=1))
    for i,(source,p,polygon) in enumerate(examples):
        ax=axs[i,0];ax.imshow(Image.open(formal/source['image']),cmap='gray');q=np.array(polygon+[polygon[0]])
        ax.plot(q[:,0],q[:,1],color='orange');ax.set_title('/'.join(Path(source['source_paths']['pcd']).parent.parts[-2:]));ax.axis('off')
        for j,(b,title,axis) in enumerate([(old,'OLD front: -Y / +Z',2),(old,'OLD top: -Y / +X',1),
                                          (new,'NEW front: -X / +Y',2),(new,'NEW top: -X / +Z',1)],1):
            local=p@b;ax=axs[i,j];ax.scatter(local[:,0],local[:,axis],s=1,c='#207db8')
            ax.set_aspect('equal',adjustable='datalim');ax.grid(alpha=.25);ax.set_title(title);ax.set_xlabel('width (m)')
    fig.savefig(out/'axis_comparison.png',dpi=130);plt.close(fig)
    atomic_json(out/'axis_audit.json',dict(state='passed',formal_manifest_sha256=digest(formal/'manifest.json'),
        scope_policy_id='2024_11_27|(96, 240)|primary',frames=len(records),targets=sum(len(rows[r['frame_id']]['boxes']) for r in records),
        axes=dict(forward_axis=2,forward_sign=1,up_axis=1,up_sign=1,width_source_direction=[-1,0,0]),
        evidence_kind='all_frame_pixel_projection_sign_check_not_factory_or_robot_calibration',
        horizontal_r2_min=min(r['horizontal_u_from_X_over_Z']['r2'] for r in records),
        vertical_r2_min=min(r['vertical_v_from_Y_over_Z']['r2'] for r in records),
        image_pixel_rotation_preserved_degrees=180,XYZ_rotated=False,production_database_writes=0,
        examples=[dict(frame_id=s['frame_id'],image=s['image']) for s,_,_ in examples],records=records))
    print(json.dumps(dict(state='passed',frames=len(records),horizontal_r2_min=min(r['horizontal_u_from_X_over_Z']['r2'] for r in records),
        vertical_r2_min=min(r['vertical_v_from_Y_over_Z']['r2'] for r in records),comparison=str(out/'axis_comparison.png'))))


if __name__=='__main__':main()
