"""Conditional camera fit to IMG_6329; never applies fitted car dimensions.

Run with the CAD environment's numpy/scipy. Updates only the adjacent evidence
JSON's camera_fit. Wheel outer-face/cap offsets, straight steering, principal
point and a common circular trim radius are hypotheses, not owner dimensions.
"""
from pathlib import Path
import json
import numpy as np
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation
from front_clip_layout import panel_z

PATH=Path(__file__).with_name('owner-side-view-6329.json')

def fit(data,body):
    scale=body['published_reference_dimensions_mm']['wheelbase']/2.4
    nominal=body['published_reference_dimensions_mm']
    rim_offset=data['construction_assumptions']['trim_face_outboard_offset_mm']/scale
    cap_offset=data['construction_assumptions']['cap_face_outboard_offset_mm']/scale
    centers=np.array([[-nominal['rear_track_steel_wheels']/2/scale,.39,-1.2],
                      [-nominal['front_track_steel_wheels']/2/scale,.39,1.2]])
    faces=centers+[-rim_offset,0,0];caps=centers+[-cap_offset,0,0]
    observed=np.array(data['spindle_centers_pixels']);width,height=data['image_size']
    principal=np.array([width/2,height/2])
    def project(p,points):
        rot=Rotation.from_rotvec(p[:3]).as_matrix();q=np.asarray(points)@rot.T+p[3:6]
        return q[:,:2]/q[:,2,None]*np.exp(p[6])+principal
    def residual(p):
        rot=Rotation.from_rotvec(p[:3]).as_matrix();f=np.exp(p[6])
        intrinsics=np.array([[f,0,principal[0]],[0,f,principal[1]],[0,0,1.]])
        errors=list(((project(p,caps)-observed)/2).ravel())
        for center,key in zip(faces,['rear','front']):
            homography=intrinsics@np.column_stack([rot[:,1],rot[:,2],rot[:,0]*center[0]+p[3:6]])
            xy=np.array(data['wheel_trim_samples_pixels'][key]);q=np.column_stack([xy,np.ones(len(xy))])@np.linalg.inv(homography).T;q=q[:,:2]/q[:,2,None]
            errors.extend((np.linalg.norm(q-center[1:],axis=1)-np.exp(p[7]))*500)
        return np.array(errors)
    position=np.array([-4,3.5,3]);forward=np.array([0,.7,0])-position;forward/=np.linalg.norm(forward)
    right=np.cross(forward,[0,1,0]);right/=np.linalg.norm(right);down=np.cross(forward,right)
    rotation=np.stack([right,down,forward]);initial=np.r_[Rotation.from_matrix(rotation).as_rotvec(),-rotation@position,np.log(1800),np.log(.21)]
    result=least_squares(residual,initial,max_nfev=3000)
    assert result.success and np.isfinite(result.x).all()
    p=result.x;rotation=Rotation.from_rotvec(p[:3]).as_matrix()
    return dict(camera_position_body_units=(-rotation.T@p[3:6]).tolist(),world_to_camera_rotation=rotation.tolist(),world_to_camera_translation=p[3:6].tolist(),focal_length_pixels=float(np.exp(p[6])),estimated_trim_radius_mm=float(np.exp(p[7])*scale),projected_spindle_centers_pixels=project(p,caps).tolist(),center_residuals_pixels=(project(p,caps)-observed).tolist(),fit_parameters=p.tolist(),trim_face_centers_body_units=faces.tolist(),cap_face_centers_body_units=caps.tolist())

if __name__=='__main__':
    data=json.loads(PATH.read_text());body=json.loads(PATH.parent.parent.joinpath('body-data.json').read_text())
    data['camera_fit']=fit(data,body)
    result=data['camera_fit'];result['center_residual_rms_pixels']=float(np.sqrt(np.mean(np.array(result['center_residuals_pixels'])**2)))
    point=np.array([-.78,.74,panel_z(1.68)]);q=np.array(result['world_to_camera_rotation'])@point+np.array(result['world_to_camera_translation'])
    projected=q[:2]/q[2]*result['focal_length_pixels']+np.array(data['image_size'])/2
    data['held_out_side_marker_current_body_units']=point.tolist()
    old_q=np.array(result['world_to_camera_rotation'])@np.array([-.78,.74,1.68])+np.array(result['world_to_camera_translation'])
    old_projected=old_q[:2]/old_q[2]*result['focal_length_pixels']+np.array(data['image_size'])/2
    data['previous_drawing_marker_projected_pixels']=old_projected.tolist()
    data['previous_drawing_marker_residual_pixels']=(old_projected-np.array(data['held_out_side_marker_center_pixels'])).tolist()
    data['held_out_side_marker_projected_pixels']=projected.tolist()
    data['held_out_side_marker_residual_pixels']=(projected-np.array(data['held_out_side_marker_center_pixels'])).tolist()
    PATH.write_text(json.dumps(data,indent=2)+'\n')
    print(json.dumps(data['camera_fit'],indent=2))