"""Bounded functional fixtures; no device, socket, capture or load generation."""
import json
import threading
import time
from queue import Queue, Empty
from types import SimpleNamespace
import numpy as np
import pytest
from k1link.sessions.live_planning import PlanningLiveEvent
from k1link.missions.live_buffer import LiveCloudBuffer
from k1link.missions.live_tests import PlanningLiveTests
from k1link.missions.registration_colors import query_colors
from k1link.missions.live_scene import scene_bytes
from k1link.compute.live_perception import LivePerceptionIngress
from k1link.device_plugins.xgrids_k1.planning_live import K1PlanningLiveSource
from test_stream_summary import _pose_payload, _pcl_payload


def event(kind, *, t=1, p=(0,0,0), points=None, sequence=1):
    return PlanningLiveEvent('B',2,sequence,int(t*1e9),int(t*1e9),kind,points,p)


def test_causal_pose_and_bounded_point_window():
    b=LiveCloudBuffer([[10,10,0],[14,10,0]])
    cloud=np.array([[1,1,1],[1.01,1,1],[99,1,1],[1,1,20]])
    b.ingest(event('points',points=cloud));assert len(b.snapshot()['points'])==0
    b.ingest(event('pose',t=2))
    b.ingest(event('points',t=1.9,points=cloud));assert len(b.snapshot()['points'])==0
    b.ingest(event('points',t=2.6,points=cloud));assert len(b.snapshot()['points'])==0
    b.ingest(event('points',t=2.1,points=cloud));assert len(b.snapshot()['points'])==1
    # No double pose transform: query remains in its native local K1 frame.
    assert np.allclose(b.snapshot()['points'][0],[1,1,1])
    for i in range(1,80):
        b.ingest(event('pose',t=3+i,p=(i*.05,0,0)))
        b.ingest(event('points',t=3.1+i,points=cloud,sequence=i))
    assert len(b.chunks)==40 and len(b.events)==40
    assert len(b.snapshot()['points'])<=40000
    with pytest.raises(ValueError,match='Разрыв координат'):b.ingest(event('pose',t=200,p=(100,0,0)))


def test_route_initialization_can_request_an_explicit_wider_k1_scene():
    b=LiveCloudBuffer([[0,0,0],[10,0,0]],point_radius_m=80)
    b.ingest(event('pose',t=1,p=(0,0,0)))
    b.ingest(event('points',t=1.1,points=np.array([[79.9,0,1],[80.1,0,1]])))
    sample=b.snapshot()
    assert sample['point_radius_m']==80
    assert len(sample['points'])==1


def test_only_accepted_correspondences_are_green():
    p=np.array([[0,0,0],[0,0,1],[0,0,2]])
    green=np.array([154,235,75])
    assert not (query_colors(p)==green).all(axis=1).any()
    assert not (query_colors(p,{'status':'rejected','matched_query_indices':[0,1,2]})==green).all(axis=1).any()
    c=query_colors(p,{'status':'candidate','matched_query_indices':[1]})
    assert (c==green).all(axis=1).tolist()==[False,True,False]


def test_plugin_adapter_reads_existing_committed_ingress_only():
    ingress=LivePerceptionIngress();adapter=K1PlanningLiveSource(ingress)
    adapter.open('test');ingress.begin_session('B')
    assert adapter.take('test').kind=='session-start'
    ingress.publish(modality='pose',source_id='x/lio_pose',source_sequence=3,
                    captured_at_epoch_ns=4,received_monotonic_ns=5,payload=_pose_payload((5,0,0)))
    p=adapter.take('test');assert p.position==(5.,0.,0.) and p.generation==1
    ingress.publish(modality='lidar',source_id='x/lio_pcl',source_sequence=4,
                    captured_at_epoch_ns=5,received_monotonic_ns=6,payload=_pcl_payload(scaler=1000,point_count=4))
    frame=adapter.take('test');assert np.allclose(frame.points[0],[1,-2,.5])
    with pytest.raises(RuntimeError):adapter.open('other-profile')
    adapter.close('test');assert not ingress.snapshot()['consumer_connected']
    ingress.close()


class Source:
    def __init__(self):self.state=dict(active=False,session_generation=1,session_id='old');self.queue=Queue();self.owner=None
    def snapshot(self):return dict(self.state)
    def open(self,id):
        if self.owner:raise RuntimeError('busy')
        self.owner=id
    def close(self,id):assert self.owner==id;self.owner=None
    def take(self,id):
        try:return self.queue.get(timeout=.02)
        except Empty:return None


def fixture_service(tmp_path,monkeypatch):
    import k1link.missions.live_tests as module
    draft=dict(id='draft',name='Test route',revision=1,zone=dict(session_id='A',generation='a'),
               route=dict(length_m=20,start_index=0,end_index=20,points=[dict(position=[i,0,0]) for i in range(21)]))
    r=np.random.default_rng(19);points=r.normal(size=(1000,3))
    sources=SimpleNamespace(store=SimpleNamespace(get_session=lambda _:SimpleNamespace(plugin_id='test-plugin')),
                            reference_map=lambda *args,**kwargs:(points,dict(session_id='A',source_digests={'raw':'a'*64})))
    drafts=SimpleNamespace(database=tmp_path/'db',sources=sources,get=lambda _:json.loads(json.dumps(draft)))
    source=Source();lock=threading.Lock();service=PlanningLiveTests(drafts,{'test-plugin':source},lock)
    def calculate(directory,ref,query,hint):
        # Real numeric regression is tested separately; this fixture verifies orchestration.
        return dict(status='candidate',T_reference_query=hint.tolist(),matched_query_indices=[0],
                    overlap=.9,inlier_rmse_m=.1,reasons=[])
    monkeypatch.setattr(module,'run_registration',calculate)
    from k1link.missions.route_relocalization import ROUTE_RELOCALIZATION_POLICY
    monkeypatch.setattr(module,'run_route_relocalization',
                        lambda directory,ref,path,query,anchor:dict(
                            status='candidate',T_reference_query=np.eye(4).tolist(),
                            matched_query_indices=[0],overlap=.9,inlier_rmse_m=.1,reasons=[],
                            initialization=dict(complete=True,scope='selected-route',
                                policy=ROUTE_RELOCALIZATION_POLICY,expected_attempts=1,attempts=[{}])))
    return service,source,lock,draft


def until(fn):
    deadline=time.monotonic()+5
    while time.monotonic()<deadline:
        if fn():return
        time.sleep(.01)
    raise AssertionError('condition did not become true')


def test_profile_requires_new_session_free_lease_and_frozen_revision(tmp_path,monkeypatch):
    service,source,lock,draft=fixture_service(tmp_path,monkeypatch)
    with pytest.raises(ValueError,match='изменён'):service.start('draft',2)
    source.state['active']=True
    with pytest.raises(ValueError,match='завершите'):service.start('draft',1)
    source.state['active']=False;source.owner='AI'
    with pytest.raises(ValueError,match='другим'):service.start('draft',1)
    assert not lock.locked()
    source.owner=None
    run=service.start('draft',1)
    try:
        until(lambda:service.get()['state']=='waiting')
        assert run['profile']=='planning' and lock.locked()
        draft['revision']=2
        assert service.get()['draft']['revision']==1
        with pytest.raises(ValueError,match='ещё выполняется'):service.start('draft',1)
        # Old queued acquisition cannot be rebound to reference A or mistaken for B.
        source.queue.put(PlanningLiveEvent('old',1,1,time.monotonic_ns(),time.time_ns(),'pose',position=[0,0,0]))
        time.sleep(.05);assert service.get()['query_session_id'] is None
        source.state.update(active=True,session_id='B',session_generation=2)
        now=time.monotonic()
        source.queue.put(event('pose',t=now,sequence=10))
        source.queue.put(event('points',t=now+.001,sequence=11,
                               points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))))
        until(lambda:service.get().get('planning_phase')=='collecting')
        assert service.get()['query_session_id']=='B' and service.get()['state']=='running'
        assert service.get()['result'] is None
        assert service.scene(run['id'],True).startswith(b'RRF2')
        source.state.update(session_id='C',session_generation=3)
        until(lambda:service.get()['state']=='error')
    finally:service.close()
    assert not lock.locked() and source.owner is None
    restored=PlanningLiveTests(service.drafts,{'test-plugin':source},lock)
    assert restored.get()['query_session_id']=='B' and restored.get()['state']=='error'
    assert restored.get()['scene_available'] and restored.sample is not None
    assert restored.get()['stale'] and not lock.locked()
    assert restored.history()[0]['query_session_id']=='B'
    assert restored.scene(run['id'],True).startswith(b'RRF2')


def test_cancel_retains_raw_and_releases_only_derived_lease(tmp_path,monkeypatch):
    service,source,lock,_=fixture_service(tmp_path,monkeypatch)
    run=service.start('draft',1)
    until(lambda:service.get()['state']=='waiting')
    service.stop(run['id']);service.close()
    assert service.get()['state']=='cancelled' and not lock.locked() and source.owner is None
    assert service.get()['vehicle_control'] is False
    with pytest.raises(KeyError):service.stop('wrong-id')


def test_native_rerun_base_and_incremental_update_are_valid():
    reference=np.random.default_rng(1).normal(size=(400,3));path=np.array([[0,0,0],[4,0,0]])
    sample=dict(points=reference,path=path,hint=np.eye(4))
    assert scene_bytes('test',reference,path,sample,base=True).startswith(b'RRF2')
    assert scene_bytes('test',reference,path,sample,base=False).startswith(b'RRF2')


def test_receipt_gap_is_not_an_instantaneous_jump_and_clears_fit_window():
    b=LiveCloudBuffer([[0,0,0],[30,0,0]])
    b.ingest(event('pose',t=1,p=(0,0,0)))
    b.ingest(event('points',t=1.1,points=np.array([[0,0,1]])))
    b.ingest(event('pose',t=11,p=(10.4,0,0)))
    sample=b.snapshot()
    assert sample['segment']==1 and len(sample['points'])==0
    assert sample['gaps'][0]['seconds']==10
    assert sample['distance']==pytest.approx(10.4)
    # The same displacement over 100 ms still rejects; no threshold bypass.
    with pytest.raises(ValueError,match='Разрыв координат'):
        b.ingest(event('pose',t=11.1,p=(21,0,0)))


@pytest.mark.parametrize("loss", ["stale", "reference-coverage", "fit-rejected", "worker-error", "pose-jump", "operator-stop", "operator-stop-pending"])
def test_live_stationary_bootstrap_keeps_calibration_separate_and_requires_fresh_windows(tmp_path,monkeypatch,loss):
    import k1link.missions.live_tests as module
    from k1link.missions.route_relocalization import ROUTE_RELOCALIZATION_POLICY
    service,source,lock,_=fixture_service(tmp_path,monkeypatch)
    clock=[100.]
    monkeypatch.setattr(module,'time',SimpleNamespace(
        monotonic=lambda:clock[0],monotonic_ns=lambda:int(clock[0]*1e9)))
    calls=[]
    fail_next=[None]
    entry_release=threading.Event()
    fresh_entered, fresh_release = threading.Event(), threading.Event()
    def fit(directory,ref,query,hint):
        failure,fail_next[0]=fail_next[0],None
        if failure=="worker-error":raise ValueError("fixture worker unavailable")
        if failure=="fit-rejected":return dict(status="rejected",reasons=["fixture bad fit"])
        if failure == "operator-stop-pending":
            fresh_entered.set()
            assert fresh_release.wait(5)
        calls.append('fresh')
        return dict(status='candidate',T_reference_query=hint.tolist(),matched_query_indices=[0],
                    overlap=.9,inlier_rmse_m=.1,reasons=[])
    def entry(directory,ref,path,query,anchor,**kwargs):
        if calls:
            assert kwargs["reference_position"] == [0.,0.,0.]
        calls.append('entry')
        entry_release.wait(3)
        return dict(status='candidate',T_reference_query=np.eye(4).tolist(),matched_query_indices=[0],
                    overlap=.9,inlier_rmse_m=.1,reasons=[],initialization=dict(
                    complete=True,scope='selected-route',policy=ROUTE_RELOCALIZATION_POLICY,
                    expected_attempts=1,attempts=[{}]))
    monkeypatch.setattr(module,'run_registration',fit)
    monkeypatch.setattr(module,'run_route_relocalization',entry)
    service.start('draft',1)
    points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))
    sequence=0
    def frame(t,p=(0,0,0)):
        nonlocal sequence
        sequence+=1;source.queue.put(event('pose',t=t,p=p,sequence=sequence))
        sequence+=1;source.queue.put(event('points',t=t+.001,points=points,sequence=sequence))
        return sequence
    try:
        until(lambda:service.get()['state']=='waiting')
        source.state.update(active=True,session_id='B',session_generation=2)
        # Session opens during hardware calibration. Neither control nor poses
        # start route initialization without a usable cloud.
        source.queue.put(event('session-start',t=100,sequence=0))
        until(lambda:service.get()['state']=='running')
        clock[0]=160
        source.queue.put(event('pose',t=159,sequence=0))
        until(lambda:source.queue.empty())
        assert service.get()['planning_phase']=='waiting-cloud' and not calls
        for i in range(20):frame(160+i*.5)
        until(lambda:source.queue.empty())
        assert service.get()['planning_phase']=='collecting'
        clock[0]=170.01
        until(lambda:calls==['entry'])
        # Worker submission and publication happen on separate threads; the
        # fit callback is not an acknowledgement that UI state was persisted.
        until(lambda:service.get()['planning_phase']=='searching')
        # Capture continues while the worker searches; an actual post-ready
        # receipt gap must invalidate the prior, not be hidden by this fixture.
        for i in range(40):frame(170+i*.5)
        until(lambda:source.queue.empty())
        clock[0]=190
        entry_release.set()
        until(lambda:service.get().get('initialization_result') is not None
              and service.get()['planning_phase']=='refreshing')
        assert service.get()['planning_phase']=='refreshing'
        assert service.get()['result'] is None and service.accepted_sample is None
        previous_result=None
        for j in range(3):
            begin=190.5+j*5
            for i in range(10):
                clock[0]=begin+i*.5+.002
                frame(begin+i*.5)
                until(lambda:source.queue.empty())
            until(lambda:service.get().get('result_source_sequence') not in (None,previous_result))
            previous_result=service.get()['result_source_sequence']
            assert service.get()['tracking_state']==('tracking' if j==2 else 'acquiring')
            assert service.get()['tracking_established'] == (j == 2)
            assert 'Кандидат совмещения' != service.get()['message']
            assert service._scene_result['matched_query_indices']==([0] if j==2 else [])
        assert calls==['entry','fresh','fresh','fresh']
        assert service.get()['planning_phase']=='tracking'
        if loss in {"operator-stop", "operator-stop-pending"}:
            if loss == "operator-stop-pending":
                fail_next[0] = loss
                begin = clock[0] + .5
                for i in range(10):
                    clock[0] = begin + i*.5 + .002
                    frame(begin+i*.5)
                    until(source.queue.empty)
                until(fresh_entered.is_set)
            source.state['spatial_stop_requested'] = True
            # Model the recorded 51-second raw finalisation with a virtual clock.
            clock[0] += 51
            until(lambda: service.get()['state'] == 'completed')
            assert service.get()['termination_reason'] == 'spatial-stop-requested'
            assert service.get().get('recovery_attempt', 0) == 0
            assert service.get()['planning_phase'] == 'ended'
            assert service.accepted_sample is None and source.state['active']
            assert service.get()['result_source_sequence'] == previous_result
            assert not any(t['phase'] in {'lost', 'recovering'}
                           for t in service.get()['phase_transitions'])
            fresh_release.set()
            until(lambda: not service.thread.is_alive())
            assert service.get()['result_source_sequence'] == previous_result
            assert service.accepted_sample is None
            return
        if loss=="stale":
            clock[0]+=9
        else:
            import k1link.missions.stationary_live as live_loop
            from k1link.missions.reference_window import ReferenceCoverageError
            original_window=live_loop.reference_window
            failed=[False]
            def window(*args,**kwargs):
                if not failed[0] and loss=="reference-coverage":
                    failed[0]=True
                    raise ReferenceCoverageError("fixture coverage failure")
                return original_window(*args,**kwargs)
            monkeypatch.setattr(live_loop,"reference_window",window)
            fail_next[0]=loss
            begin=clock[0]+.5
            for i in range(8):
                clock[0]=begin+i*.5+.002
                frame(begin+i*.5,p=(100,0,0) if loss=="pose-jump" else (0,0,0))
                until(source.queue.empty)
                if service.get()['planning_phase']=='recovering':break
            monkeypatch.setattr(live_loop,"reference_window",original_window)
        until(lambda:service.get()['tracking_state']=='lost')
        until(lambda:service.get()['planning_phase']=='recovering')
        assert service.accepted_sample is None
        assert service.get()['tracking_established'] is True
        assert service.get()['state']=='running' and source.state['active']
        assert service.get()['recovery_attempt']==1
        previous_fresh=calls.count('fresh')
        # A fresh stationary prefix, never replay of the previously accepted fit.
        begin=clock[0]+.5
        for i in range(20):
            clock[0]=begin+i*.5+.002
            frame(begin+i*.5)
            until(source.queue.empty)
        clock[0]=begin+10.01
        until(lambda:calls.count('entry')==2)
        until(lambda:service.run.get('initialization_temporal',{}).get('provisional') is True
              and service.run.get('planning_phase')=='recovering'
              and service.run.get('planning_reason')=='provisional-prior')
        assert service.accepted_sample is None
        begin=clock[0]+.5
        for j in range(3):
            for i in range(10):
                clock[0]=begin+j*5+i*.5+.002
                frame(begin+j*5+i*.5)
                until(source.queue.empty)
            until(lambda:calls.count('fresh')>=previous_fresh+1+j)
            assert service.get()['planning_phase']==('tracking' if j==2 else 'recovering')
        assert service.get()['tracking_state']=='tracking'
        source.state['active']=False
        until(lambda:service.get()['state']=='completed')
    finally:
        entry_release.set();fresh_release.set();service.close()
    assert not lock.locked() and source.owner is None


def test_operator_can_retry_a_failed_route_identification_without_stopping_capture(tmp_path,monkeypatch):
    import k1link.missions.live_tests as module
    from k1link.missions.route_relocalization import ROUTE_RELOCALIZATION_POLICY
    service,source,lock,_=fixture_service(tmp_path,monkeypatch)
    clock=[100.]
    monkeypatch.setattr(module,'time',SimpleNamespace(
        monotonic=lambda:clock[0],monotonic_ns=lambda:int(clock[0]*1e9)))
    searches=[]
    def rejected_entry(*_args):
        searches.append('entry')
        return dict(status='rejected',T_reference_query=np.eye(4).tolist(),matched_query_indices=[],
                    overlap=.6,inlier_rmse_m=.28,reasons=['Большое расстояние между поверхностями.'],
                    initialization=dict(complete=True,scope='selected-route',
                        policy=ROUTE_RELOCALIZATION_POLICY,expected_attempts=1,attempts=[{}],
                        reason='no-route-location'))
    monkeypatch.setattr(module,'run_route_relocalization',rejected_entry)
    run=service.start('draft',1)
    points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))
    sequence=0
    def frame(t,p=(0,0,0)):
        nonlocal sequence
        sequence+=1;source.queue.put(event('pose',t=t,p=p,sequence=sequence))
        sequence+=1;source.queue.put(event('points',t=t+.001,points=points,sequence=sequence))
    try:
        until(lambda:service.get()['state']=='waiting')
        source.state.update(active=True,session_id='B',session_generation=2)
        for i in range(20):frame(100+i*.5)
        until(lambda:source.queue.empty());clock[0]=110.01
        until(lambda:service.get()['planning_phase']=='lost')
        assert searches==['entry'] and source.owner=='planning-'+run['id']
        retried=service.request_reinitialization(run['id'])
        assert retried['initialization_attempt']==2
        assert retried['reinitialization_count']==1
        assert retried['initialization_result'] is None
        until(lambda:not service.reinitialization_requested)
        # The scanner may be carried to a better point while the first failed
        # location stays recoverable; those receipts must not contaminate a
        # new stationary prefix or stop K1 capture.
        frame(111.0)
        frame(111.5,p=(1,0,0))
        until(lambda:source.queue.empty())
        assert service.get()['planning_phase']=='lost'
        assert service.get()['state']=='running'
        frame(112)
        frame(112.5)
        until(source.queue.empty)
        assert service.get()['planning_phase']=='lost'  # No implicit retry after a moved prefix.
        retried=service.request_reinitialization(run['id'])
        assert retried['initialization_attempt']==3
        until(lambda:not service.reinitialization_requested)
        for i in range(20):frame(120+i*.5)
        until(lambda:source.queue.empty());clock[0]=130.01
        until(lambda:searches==['entry','entry'])
        until(lambda:service.get()['planning_phase']=='lost')
        assert service.get()['query_session_id']=='B'
        assert source.owner=='planning-'+run['id']
    finally:
        source.state['active']=False
        service.close()
    assert not lock.locked() and source.owner is None


def test_stationary_cancel_while_searching_does_not_publish_late_prior(tmp_path,monkeypatch):
    import k1link.missions.live_tests as module
    service,source,lock,_=fixture_service(tmp_path,monkeypatch)
    clock=[100.]; entered=threading.Event();release=threading.Event()
    monkeypatch.setattr(module,'time',SimpleNamespace(monotonic=lambda:clock[0],monotonic_ns=lambda:int(clock[0]*1e9)))
    def search(*args,**kwargs):
        entered.set();release.wait(3)
        return dict(status='rejected',reasons=['test-cancel'])
    monkeypatch.setattr(module,'run_route_relocalization',search)
    run=service.start('draft',1)
    points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))
    try:
        until(lambda:service.get()['state']=='waiting')
        source.state.update(active=True,session_id='B',session_generation=2)
        for i in range(20):
            source.queue.put(event('pose',t=100+i*.5,sequence=i*2+1))
            source.queue.put(event('points',t=100+i*.5+.001,sequence=i*2+2,points=points))
        until(lambda:source.queue.empty());clock[0]=110.01
        until(entered.is_set)
        service.stop(run['id']);release.set()
        until(lambda:service.get()['state']=='cancelled')
    finally:
        release.set();service.close()
    assert service.accepted_sample is None and service.get()['result'] is None
    assert not lock.locked() and source.owner is None