#!/usr/bin/env python3
'''Combine final mechanical-prior engineering release gates without refitting.'''
from __future__ import annotations
import argparse,json,sys
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from imu_lidar.geometry import make_transform
from tools.audit_rtk_imu_factor_consistency import _jsonable

SUMMARY=('Translation is mechanically anchored and dynamically validated. '
    'The current dataset does not independently observe translation accurately '
    'enough for data-only calibration, and does not provide meaningful refinement '
    'beyond the mechanical prior.')

def main():
    p=argparse.ArgumentParser(description=__doc__)
    p.add_argument('--calibration',type=Path,required=True)
    p.add_argument('--heldout-postfit',type=Path,required=True)
    p.add_argument('--innovation',type=Path,required=True)
    p.add_argument('--sensitivity',type=Path,required=True)
    p.add_argument('--convergence-retry',type=Path,required=True)
    p.add_argument('--propagation-root-cause',type=Path)
    p.add_argument('--output',type=Path,required=True)
    args=p.parse_args()
    calibration=json.loads(args.calibration.read_text(encoding='utf-8'))
    heldout=json.loads(args.heldout_postfit.read_text(encoding='utf-8'))
    innovation=json.loads(args.innovation.read_text(encoding='utf-8'))
    sensitivity=json.loads(args.sensitivity.read_text(encoding='utf-8'))
    retry=json.loads(args.convergence_retry.read_text(encoding='utf-8'))
    root_cause=(None if args.propagation_root_cause is None else
        json.loads(args.propagation_root_cause.read_text(encoding='utf-8')))
    lever=np.asarray(calibration['prior_constrained_solution']['result']['final_l_I_m'])
    R=Rotation.from_euler('xyz',[.4543066225,-.0026392019,.0122384129],
                          degrees=True).as_matrix()
    candidate_T=make_transform(-R@lever,R)
    candidate_inverse=np.linalg.inv(candidate_T)
    inverse_error=float(np.linalg.norm(candidate_T@candidate_inverse-np.eye(4)))
    if inverse_error>=1e-10: raise RuntimeError('candidate transforms are not inverse')
    comparison=calibration['comparisons']
    def nonconflicting(name):
        value=comparison[name]
        return (value['relative_cost_delta']<=.05 and
            all(x['p95_abs_delta']<=.25
                for x in value['factor_residual_delta'].values()))
    mechanical_consistent=bool(
        nonconflicting('fixed_vs_free') and nonconflicting('prior_data_vs_free'))
    overall=heldout['heldout_validation']['overall']
    physical_checks={
        'all_267_converged_after_retry':
            retry['heldout_convergence_after_retry']==1.,
        'BEST_position_vector_p95_le_0p20_m':
            overall['best_position_physical_m']['vector_p95']<=.20,
        'Doppler_vector_p95_le_0p50_m_s':
            overall['doppler_physical_m_s']['vector_p95']<=.50,
        'HPR_normalized_p95_le_4':
            overall['residual_by_factor']['hpr']['p95_abs']<=4.,
        'preintegration_normalized_p95_le_3':
            overall['residual_by_factor']['imu_preintegration']['p95_abs']<=3.}
    physical_passed=bool(all(physical_checks.values()))
    statistical_passed=bool(.25<=overall['global_chi_square_per_dof']<=4.)
    underdispersion=bool(physical_passed and not statistical_passed and
                         overall['global_chi_square_per_dof']<.25)
    independent=bool(innovation['independent_heldout_innovation_passed'])
    rotation=bool(sensitivity['rotation_sensitivity_passed'])
    accepted=bool(mechanical_consistent and physical_passed and independent and rotation)
    payload={'scope':'final mechanical-prior RTK-IMU engineering release decision',
        'no_refit_performed':True,'data_only_full_free_called':False,
        'bootstrap_called':False,'loo_called':False,
        'covariance_retuned':False,'new_window_selection_called':False,
        'parser_R0_modified':False,
        'data_only_translation_accepted':False,
        'translation_refined_by_data':False,
        'mechanical_prior_consistent_with_calibration':mechanical_consistent,
        'heldout_physical_validation_passed':physical_passed,
        'heldout_physical_gate_checks':physical_checks,
        'heldout_statistical_scale_passed':statistical_passed,
        'heldout_postfit_chi_square_per_dof':
            overall['global_chi_square_per_dof'],
        'heldout_covariance_underdispersion_warning':underdispersion,
        'independent_heldout_innovation_passed':independent,
        'common_constant_acceleration_error_detected':(
            None if root_cause is None else
            root_cause['common_constant_acceleration_error_detected']),
        'independent_propagation_validation_passed':(
            None if root_cause is None else
            root_cause['independent_propagation_validation_passed']),
        'independent_extrinsic_sensitive_validation_passed':(
            None if root_cause is None else
            root_cause['independent_extrinsic_sensitive_validation_passed']),
        'rotation_sensitivity_passed':rotation,
        'engineering_translation_acceptance_formula':(
            'mechanical_prior_consistent_with_calibration AND '
            'heldout_physical_validation_passed AND '
            'independent_heldout_innovation_passed AND rotation_sensitivity_passed'),
        'engineering_translation_accepted':accepted,
        'result_nature':'mechanically anchored + dynamically validated',
        'summary':SUMMARY,'forbidden_descriptions':[
            'data-only calibrated translation','dynamically refined mechanical lever'],
        'candidate_l_I_engineering_m':lever,
        'candidate_T_RTK_IMU':candidate_T,
        'candidate_T_IMU_RTK':candidate_inverse,
        'candidate_transform_inverse_error_norm':inverse_error,
        'l_I_engineering_m':lever if accepted else None,
        'T_RTK_IMU':candidate_T if accepted else None,
        'T_IMU_RTK':candidate_inverse if accepted else None,
        'transform_convention':{
            'equation':'p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU',
            'translation':'t_RTK_IMU = -R_RTK_IMU * l_I',
            'RTK_origin':'ANT1 phase center'},
        'rotation_source':'R2G_gravity_level_prior',
        'translation_conditional_on_rotation':True,
        'evidence':{
            'calibration_path':str(args.calibration),
            'heldout_postfit_path':str(args.heldout_postfit),
            'innovation_path':str(args.innovation),
            'sensitivity_path':str(args.sensitivity),
            'convergence_retry_path':str(args.convergence_retry),
            'propagation_root_cause_path':(
                None if args.propagation_root_cause is None
                else str(args.propagation_root_cause)),
            'posterior_prior_variance_ratio':
                calibration['prior_constrained_solution']['posterior_prior_variance_ratio'],
            'heldout_convergence_after_retry':
                retry['heldout_convergence_after_retry'],
            'rotation_sensitivity_summary':sensitivity['summary']}}
    args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
                                      allow_nan=False)+'\n',encoding='utf-8')
    print(json.dumps(_jsonable({key:payload[key] for key in (
        'data_only_translation_accepted','translation_refined_by_data',
        'mechanical_prior_consistent_with_calibration',
        'heldout_physical_validation_passed','heldout_statistical_scale_passed',
        'heldout_covariance_underdispersion_warning',
        'independent_heldout_innovation_passed','rotation_sensitivity_passed',
        'engineering_translation_accepted')}),indent=2))
    return 0
if __name__=='__main__': raise SystemExit(main())
