"""Actual Ford factory, parser, Card, SelfdriveD, Controls and native matrix.

CAN, perception, USB and clocks are synthetic; no physical acceptance claim.
"""
import pytest
from openpilot.starpilot.tests.ford_lifecycle_helpers import FORD_FACTORIES, ford_joined as ford_joined, run_actual_producer


@pytest.fixture(autouse=True)
def configuration_topology(monkeypatch, topology, request):
  monkeypatch.ford_topology = topology
  if request.node.name.startswith(('test_brake_retains_lateral_with_native_first_feedback', 'test_cancel_retains_lateral_with_native_first_feedback')):
    monkeypatch.ford_native_10hz = True
    monkeypatch.ford_angle = request.node.callspec.params['angle']
  if request.node.name.startswith('test_recorded_radar_creep_enters_native_stopping'):
    monkeypatch.ford_angle = request.node.callspec.params['angle']
  if request.node.name.startswith('test_route49_first_main_engagement'):
    monkeypatch.ford_first_main = True
    monkeypatch.ford_angle = request.node.callspec.params['angle']


@pytest.mark.parametrize('identity', FORD_FACTORIES)
@pytest.mark.parametrize('alpha', [False, True])
@pytest.mark.parametrize('hands_free', [False])
@pytest.mark.parametrize('topology', ['automatic', 'manual', 'multi-panda', 'manual-multi'])
def test_actual_configuration_lifecycle(ford_joined, monkeypatch):
  run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity')


@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
@pytest.mark.parametrize('alpha', [False])
@pytest.mark.parametrize('hands_free', [False, True])
@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
@pytest.mark.parametrize('angle', [False, True])
def test_route49_first_main_engagement_arms_before_cruise_cancel(ford_joined, monkeypatch, angle):
  run_actual_producer(ford_joined, monkeypatch, scenario='route49')


@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
@pytest.mark.parametrize('alpha', [True])
@pytest.mark.parametrize('hands_free', [False, True])
@pytest.mark.parametrize('angle', [False, True])
@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
def test_brake_retains_lateral_with_native_first_feedback(ford_joined, monkeypatch, angle):
  run_actual_producer(ford_joined, monkeypatch, scenario='brake-native-cadence')


@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
@pytest.mark.parametrize('alpha', [False, True])
@pytest.mark.parametrize('hands_free', [False, True])
@pytest.mark.parametrize('angle', [False, True])
@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
def test_cancel_retains_lateral_with_native_first_feedback(ford_joined, monkeypatch, angle):
  run_actual_producer(ford_joined, monkeypatch, scenario='cancel-native-cadence')


@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
@pytest.mark.parametrize('alpha', [True])
@pytest.mark.parametrize('hands_free', [True])
@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
@pytest.mark.parametrize('angle', [False, True])
def test_recorded_radar_creep_enters_native_stopping(ford_joined, angle):
  import json
  import gzip
  from pathlib import Path
  from opendbc.car import structs
  from opendbc.car.ford import fordcan
  from opendbc.safety.tests.libsafety import libsafety_py
  from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
  from openpilot.selfdrive.controls.lib.longcontrol import LongControl
  from openpilot.starpilot.longitudinal.tests.test_lead_approach_planner import Frame, DRIVE_NS

  recorded = json.loads(gzip.decompress((Path(__file__).parent / 'testdata/ford_mache_radar_creep.json.gz').read_bytes()))['frames']
  joined = ford_joined
  endpoint, safety = joined.native, joined.safety
  endpoint.arm()
  endpoint.pump(10, main=5, mask=3)
  assert safety.aol_get_permission_mask() == 3
  planner, controller = LongitudinalPlanner(joined.host.CP, init_v=recorded[0]['vEgo']), LongControl(joined.host.CP)
  checked = 0
  for index, sample in enumerate(recorded):
    sm = Frame(index)
    # Retain the route's actual cross-service age and validity, including radar cadence.
    for service, age in sample['sourceAgeNs'].items():
      sm.logMonoTime[service] = sm.now_ns - 10_000_000 - age
      sm.valid[service] = sample['sourceValid'][service]
    car = sm['carState']
    car.vEgo, car.vEgoRaw, car.aEgo = sample['vEgo'], sample['vEgoRaw'], sample['aEgo']
    car.standstill, car.gasPressed, car.brakePressed = sample['standstill'], sample['gas'], sample['brake']
    car.gearShifter = 'drive'
    car.cruiseState.available = car.cruiseState.enabled = True
    sm['carControl'].longActive = sample['longActive']
    sm['selfdriveState'].enabled = sample['enabled']
    sm['selfdriveState'].experimentalMode = sample['experimental']
    sm['controlsState'].longControlState = controller.long_control_state
    sm['modelV2'].action.shouldStop = sample['shouldStop']
    sm['modelV2'].action.desiredAcceleration = sample['aTarget']
    for key, values in sample['model'].items():
      getattr(sm['modelV2'], key).x = values
    for lead, values in zip((sm['radarState'].leadOne, sm['radarState'].leadTwo), sample['leads'], strict=True):
      for key, value in values.items():
        setattr(lead, key, value)
    planner.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS)
    assert planner.mpc.solution_status == 0
    for _ in range(5):
      output = controller.update(sample['longActive'], car, float(planner.output_a_target), planner.output_should_stop, (-3.5, 2.))
      endpoint.pump(1, main=5, mask=3, speed=sample['vEgoRaw'])
      stopping = controller.long_control_state == structs.CarControl.Actuators.LongControlState.stopping
      active = sample['longActive']
      gas = -5. if output < 0. or not active else float(output)
      address, data, bus = fordcan.create_acc_msg(endpoint.packer, joined.host.CI.CC.CAN, active, gas,
        float(output) if active else 0., stopping if active else False, bool(output < 0. and active), v_ego_kph=80.)
      assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus - joined.offset, data))
      if 821. <= sample['routeSeconds'] <= 828.:
        assert planner.output_should_stop, ('Creeping toward stopped radar lead', sample['routeSeconds'])
        assert stopping and output < 0.
        checked += 1
  assert checked > 500