from contextlib import ExitStack
from unittest import mock

import pytest

from openpilot.cereal import log, messaging
from openpilot.common.prefix import OpenpilotPrefix
from opendbc.car.structs import car
from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture
from openpilot.starpilot.aol.wire import IntentState, encode_intent


def message(stamp, state=None, *, valid=True):
  sample = messaging.new_message('carState', valid=valid, logMonoTime=stamp)
  value = fixture.car_state() if state is None else state
  value.cruiseState.available = True
  sample.carState = value
  return sample.as_reader()


def queued_step(sd, step, samples, *, tick=5, offset_ns=4_292_276, intent=True):
  selected_ns = samples[-1].logMonoTime
  wire = messaging.new_message('aolIntentWire', 0, valid=True, logMonoTime=selected_ns)
  wire.aolIntentWire = encode_intent(IntentState('authority-card', 50, selected_ns, selected_ns,
    selected_ns + 200_000_000, True, False, False, True, True))
  backlog = iter(samples[1:])
  original_receive = messaging.recv_one_or_none

  def receive(sock):
    return next(backlog, None) if sock is sd.car_state_sock else original_receive(sock)

  with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one', return_value=samples[0]), \
       mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one_or_none', side_effect=receive):
    return step(tick, offset_ns=offset_ns, state_stamp=samples[0].logMonoTime,
                intent_message=wire if intent else None)


def test_route3c_oldest_35ms_advances_to_exact_fresh_pair_and_actual_controls():
  case = fixture.IoniqAvailabilityTests(methodName='runTest')
  with ExitStack() as cleanup:
    cleanup.callback(case.doCleanups)
    with OpenpilotPrefix():
      sd, _, step, _, receive = case._ioniq_availability_callers()
      a, b, now = 10_019_000_000, 10_051_448_441, 10_054_292_276
      # Publish the still-fresh retained source before the target queue interval.
      with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', return_value=10_024_292_276):
        sd.publish_axis_proof(sd.CS_prev)
      receive(10_024_292_276, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
      axis, _, command = queued_step(sd, step, [message(a), message(10_030_000_000), message(b)])
      assert now - a == 35_292_276 and now - b == 2_843_835
      assert axis.sourceCarStateMonoTime == b and sd.aol_last_intent.carStateLogMonoTime == b
      assert axis.lateralActive and axis.longitudinalActive and command.latActive and command.longActive
      assert not sd.aol_authority_lost and log.OnroadEvent.EventName.controlsMismatch not in sd.events.names
      assert receive(now, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])


@pytest.mark.parametrize('pulse', ['cancel', 'brake', 'gas', 'permanent_eps', 'invalid_can', 'invalid_envelope', 'temporary_eps'])
def test_queue_preserves_pulses_and_latest_reader_without_reengaging(pulse):
  case = fixture.IoniqAvailabilityTests(methodName='runTest')
  with ExitStack() as cleanup:
    cleanup.callback(case.doCleanups)
    with OpenpilotPrefix():
      sd, _, step, card, receive = case._ioniq_availability_callers()
      # Publish the still-fresh retained source before the target queue interval.
      with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', return_value=10_024_292_276):
        sd.publish_axis_proof(sd.CS_prev)
      receive(10_024_292_276, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
      first = fixture.car_state()
      first.cruiseState.available = True
      if pulse == 'cancel':
        first.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.cancel, pressed=True)]
      elif pulse == 'brake':
        first.brakePressed = True
      elif pulse == 'gas':
        first.gasPressed = True
        sd.disengage_on_accelerator = True
      elif pulse == 'permanent_eps':
        first.steerFaultPermanent = True
      elif pulse == 'invalid_can':
        first.canValid = False
      elif pulse == 'temporary_eps':
        first.steerFaultTemporary = first.standstill = True
      final = fixture.car_state()
      final.cruiseState.available = True
      final.buttonEnable = True
      axis, _, command = queued_step(sd, step, [message(10_019_000_000, first, valid=pulse != 'invalid_envelope'),
                                                message(10_051_448_441, final)])
      assert axis.sourceCarStateMonoTime == (0 if pulse == 'invalid_envelope' else 10_051_448_441)
      assert not sd.CS_prev.steerFaultTemporary and not sd.CS_prev.steerFaultPermanent
      assert not sd.CS_prev.brakePressed and not sd.CS_prev.gasPressed and sd.CS_prev.canValid
      if pulse not in ('brake', 'gas'):
        assert not axis.lateralActive and not command.latActive
      if pulse == 'temporary_eps':
        assert log.OnroadEvent.EventName.steerTempUnavailableSilent in sd.events.names
        assert not sd.aol_authority_lost and sd.aol_last_intent.allowedLatch
        assert receive(10_054_292_276, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
        assert card.aol_card_intent.allowed_latch
      else:
        assert not axis.longitudinalActive and not command.longActive
        if pulse in ('cancel', 'brake', 'gas'):
          assert not sd.enabled
        assert not sd.events.contains(fixture.ET.ENABLE)


@pytest.mark.parametrize('failure', ['reordered', 'full_bound', 'missing_latest_pair'])
def test_queue_reorder_bound_and_pair_loss_still_withdraw(failure):
  case = fixture.IoniqAvailabilityTests(methodName='runTest')
  with ExitStack() as cleanup:
    cleanup.callback(case.doCleanups)
    with OpenpilotPrefix():
      sd, _, step, _, _ = case._ioniq_availability_callers()
      stamps = ([10_020_000_000, 10_019_000_000] if failure == 'reordered' else
                [10_019_000_000 + i * 1_000_000 for i in range(32)] if failure == 'full_bound' else
                [10_019_000_000, 10_051_448_441])
      axis, _, command = queued_step(sd, step, [message(stamp) for stamp in stamps], intent=failure != 'missing_latest_pair')
      assert not axis.lateralActive and not axis.longitudinalActive and not command.latActive and not command.longActive
      assert str(axis.faultReason) == 'critical' and sd.aol_authority_lost
      if failure != 'missing_latest_pair':
        assert axis.sourceCarStateMonoTime == 0 and not sd.conditional_car_state_valid


@pytest.mark.parametrize('first_gear,valid,retained', [('reverse', True, True), ('neutral', True, True), ('park', True, False), ('unknown', True, False),
                                                    ('reverse', False, False), ('neutral', False, False), ('eco', True, False)])
def test_reverse_queue_pause_requires_known_valid_gears(first_gear, valid, retained):
  from openpilot.starpilot.aol.runtime import known_reverse_pause
  first, last = fixture.car_state(), fixture.car_state()
  first.gearShifter = first_gear
  last.gearShifter = 'reverse' if first_gear != 'reverse' else 'drive'
  assert known_reverse_pause(last, ((first, valid), (last, True))) == retained


def test_reverse_then_drive_in_one_batch_preserves_latch_without_commanding_either_axis():
  case = fixture.IoniqAvailabilityTests(methodName='runTest')
  with ExitStack() as cleanup:
    cleanup.callback(case.doCleanups)
    with OpenpilotPrefix():
      sd, _, step, card, receive = case._ioniq_availability_callers()
      reverse = fixture.car_state(brake=True)
      reverse.gearShifter = 'reverse'
      with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', return_value=10_024_292_276):
        sd.publish_axis_proof(sd.CS_prev)
      receive(10_024_292_276, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
      axis, _, command = queued_step(sd, step, [message(10_040_000_000, reverse), message(10_051_448_441)])
      assert not axis.desiredLateral and not axis.desiredLongitudinal
      assert not axis.lateralActive and not axis.longitudinalActive and not command.latActive and not command.longActive
      assert axis.nativeAcknowledged and str(axis.faultReason) == 'none'
      assert not sd.enabled and not sd.active
      assert log.OnroadEvent.EventName.reverseGear in sd.events.names
      assert receive(10_054_292_276, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
      assert card.aol_card_intent.allowed_latch