"""Independent observer images may constrain a family, never waive ambiguity."""

from dataclasses import replace
import math

import numpy as np
import pytest
from scipy.spatial.transform import Rotation

from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points, solve_square_tag_ippe
from linkerhand_calibration.core.geometry.tag_pose.observer_pose import fit_observer_pose
from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose


@pytest.fixture(scope='module')
def observer_images():
    matrix = np.array([[1600., 0, 640.], [0, 1600., 480.], [0, 0, 1.]])
    parents, candidates, corners = [], [], []
    relative = Rotation.identity()
    translation = np.array([.045, .02, 0.])
    for theta in np.linspace(-35, 35, 48):
        rotation = Rotation.from_euler('xy', [theta, theta*.4], degrees=True)
        origin = np.array([.015, -.02, .5])
        parents.append(SquareTagPose(tuple(rotation.as_quat()), tuple(origin), 0.))
        points = rotation.apply(relative.apply(square_object_points(.016))+translation)+origin
        projected = points@matrix.T
        pixels = projected[:, :2]/projected[:, 2:]
        corners.append(pixels)
        candidates.append(tuple(solve_square_tag_ippe(pixels, tag_size_m=.016, camera_matrix=matrix)))
    return parents, candidates, corners, [matrix]*48, .016, tuple(range(1, 49))


def test_all_legal_candidates_use_fixed_split_and_propagate_parent_uncertainty(observer_images):
    inherited = (math.radians(.05), .00005)
    result = fit_observer_pose(*observer_images, inherited=inherited)
    assert np.allclose(result.translation_xyz_m, [.045, .02, 0.], atol=1e-8)
    assert Rotation.from_quat(result.quaternion_xyzw).magnitude() < 1e-6
    assert result.rotation_uncertainty_rad >= inherited[0]
    assert result.translation_uncertainty_m >= inherited[1]
    assert set(result.training_stamps).isdisjoint(result.validation_stamps)
    assert result.training_stamps == tuple(range(1, 49, 2))


@pytest.mark.parametrize('inherited', [(math.radians(2.274), .0005), (0., .001)])
def test_exhausted_parent_budget_is_rejected_before_fitting(observer_images, monkeypatch, inherited):
    monkeypatch.setattr('linkerhand_calibration.core.geometry.tag_pose.observer_pose.least_squares',
                        lambda *a, **kw: pytest.fail('fitting cannot recover spent parent precision'))
    with pytest.raises(ValueError, match='parent_precision_budget_exhausted'):
        fit_observer_pose(*observer_images, inherited=inherited)


@pytest.mark.parametrize('fault', ['missing', 'moving', 'installation', 'unobservable', 'uncertain_parent'])
def test_invalid_observer_evidence_cannot_authorize_a_pose(observer_images, fault):
    parents, candidates, corners, matrices, size, stamps = observer_images
    inherited = (0., 0.)
    if fault == 'missing':
        parents, candidates, corners, matrices, stamps = [v[:10] for v in (parents, candidates, corners, matrices, stamps)]
    elif fault in {'moving', 'installation'}:
        corners = [np.array(pixels, copy=True) for pixels in corners]
        for i in (range(1, len(corners), 2) if fault == 'moving' else range(len(corners)//2, len(corners))):
            corners[i] += [10., -10.]
    elif fault == 'unobservable':
        parents = [parents[0]]*len(parents)
    else:
        inherited = (math.radians(2), .01)
    with pytest.raises(ValueError, match='observer_'):
        fit_observer_pose(parents, candidates, corners, matrices, size, stamps, inherited=inherited)


@pytest.mark.integration
def test_measured_observer_pose_resolves_only_a_separated_hinge_family(observer_images):
    from linkerhand_calibration.core.geometry.tag_pose.pose_bridge import SharedTagPoseBridge
    from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion
    from test_motion_image_diagnostics import frames, RELATIONS
    motion = frames(alternatives=True)
    unresolved = resolve_image_motion(motion, RELATIONS)
    assert not unresolved.resolved and unresolved.reason == 'image_motion_families_not_distinguishable'
    measured = fit_observer_pose(*observer_images)
    stationary = tuple(replace(motion[0], evidence=replace(motion[0].evidence,
        stamp_ns=10_000_000_000+i*30_000_000)) for i in range(10))
    bridge = SharedTagPoseBridge(RELATIONS[0], measured.quaternion_xyzw, measured.translation_xyz_m,
        stationary, rotation_uncertainty_rad=measured.rotation_uncertainty_rad,
        translation_uncertainty_m=measured.translation_uncertainty_m)
    result = resolve_image_motion(motion, RELATIONS, pose_bridges=(bridge,))
    assert result.resolved, result
    assert all(c.status == 'consistent' for h in result.hypotheses if h.branches == result.model.branches
        for c in h.pose_bridge_checks)


def test_observer_roles_do_not_change_the_current_task_or_require_visibility():
    from linkerhand_calibration.profiles import load_bundled_hand_profile
    from linkerhand_calibration.runtime.motion_execution import MotionCommand
    from linkerhand_calibration.runtime.observer_reference import observer_roles
    profile = load_bundled_hand_profile('o30_right_18')
    spec = profile.motion.joint_zero_references['middle_pip']
    motion = MotionCommand('zero_approach', spec.command, 200., task_key=spec.task_key, reference_joints=('middle_pip',))
    assert 'middle_dip' in observer_roles(profile, motion)
    assert observer_roles(profile, replace(motion, phase='sweep')) == set()
    assert motion.reference_joints == ('middle_pip',)