import assert from "node:assert/strict";
import { describe, it } from "node:test";
import CALIFORNIA_TRANSPORT from "../transport/california.ts";
import {
  VehicleController,
  normalizeVehicleActions,
  replayVehicleInputs,
} from "../transport/vehicleController.ts";
import { buildRoutePath, sampleRoute } from "../transport/vehicleSim.ts";

describe("vehicle controller", () => {
  it("normalizes arbitrary adapter input into safe device-neutral actions", () => {
    assert.deepEqual(
      normalizeVehicleActions({
        throttle: 4,
        brake: Number.NaN,
        steering: -3,
        handbrake: true,
        modeRequest: "manual",
        reset: true,
      }),
      {
        throttle: 1,
        brake: 0,
        steering: -1,
        handbrake: true,
        modeRequest: "manual",
        reset: true,
      },
    );
    assert.deepEqual(normalizeVehicleActions(undefined), {
      throttle: 0,
      brake: 0,
      steering: 0,
      handbrake: false,
      modeRequest: "none",
      reset: false,
    });
  });

  it("advances assisted driving on a deterministic fixed clock", () => {
    const a = new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-us-101",
      initialDistanceM: 1_000,
    });
    const b = new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-us-101",
      initialDistanceM: 1_000,
    });
    for (let index = 0; index < 240; index += 1) {
      a.stepFixed();
      b.stepFixed();
    }
    assert.deepEqual(a.snapshot(), b.snapshot());
    assert.equal(a.state().mode, "assisted");
    assert.ok(a.state().distanceM > 1_000);
    assert.ok(a.state().speedMps > 0);
    assert.ok(Number.isFinite(a.state().lat));
    assert.ok(Number.isFinite(a.state().lng));
  });

  it("gives manual input priority and cleanly resumes assistance", () => {
    const controller = new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-i-5",
      initialSpeedMps: 25,
    });
    for (let index = 0; index < 90; index += 1) {
      controller.stepFixed({ throttle: 0.7, steering: 0.8 });
    }
    assert.equal(controller.state().mode, "manual");
    assert.ok(controller.state().lateralOffsetM > 0.5);

    controller.stepFixed({ modeRequest: "assisted", steering: 0.9 });
    assert.equal(controller.state().mode, "manual", "simultaneous human input must win");
    const takeoverOffset = controller.state().lateralOffsetM;

    controller.stepFixed({ modeRequest: "assisted" });
    assert.equal(controller.state().mode, "assisted");
    assert.ok(
      Math.abs(controller.state().lateralOffsetM - takeoverOffset) < 0.25,
      "assistance must not teleport the car to centre",
    );
    for (let index = 0; index < 300; index += 1) controller.stepFixed();
    assert.ok(Math.abs(controller.state().lateralOffsetM) < Math.abs(takeoverOffset));
  });

  it("enforces speed and road-edge guardrails and exposes contact", () => {
    const controller = new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-us-101",
      mode: "manual",
      initialSpeedMps: 200,
      initialLateralOffsetM: 100,
      maximumSpeedMps: 30,
      guardrailOffsetM: 4,
    });
    assert.equal(controller.state().speedMps, 30);
    assert.equal(controller.state().lateralOffsetM, 4);
    for (let index = 0; index < 120; index += 1) {
      controller.stepFixed({ throttle: 1, steering: 1 });
    }
    assert.ok(controller.state().lateralOffsetM <= 4);
    assert.ok(controller.state().speedMps <= 30);
    assert.equal(controller.state().guardrailContact, true);
  });

  it("resets exactly to its configured spawn state", () => {
    const controller = new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-i-5",
      mode: "manual",
      initialDistanceM: 12_345,
      initialLateralOffsetM: -1.25,
      initialSpeedMps: 8,
    });
    const spawn = controller.snapshot();
    for (let index = 0; index < 100; index += 1) {
      controller.stepFixed({ throttle: 1, steering: 0.5 });
    }
    controller.stepFixed({ reset: true });
    assert.deepEqual(controller.snapshot(), spawn);
  });

  it("caps sleeping-tab catch-up and ignores invalid render deltas", () => {
    const controller = new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-us-101",
    });
    assert.equal(controller.tick(Number.NaN), 0);
    assert.equal(controller.tick(-1), 0);
    const steps = controller.tick(600);
    assert.ok(steps <= 15);
    assert.equal(controller.state().elapsedSteps, steps);
  });

  it("replays timed input frames bit-for-bit", () => {
    const options = {
      routeId: "la-sf-i-5",
      initialSpeedMps: 15,
      fixedStepSeconds: 1 / 30,
    } as const;
    const frames = [
      { steps: 40, actions: { throttle: 0.8, steering: -0.3 } },
      { steps: 1, actions: { modeRequest: "assisted" as const } },
      { steps: 80 },
      { steps: 20, actions: { brake: 0.7 } },
    ];
    const first = replayVehicleInputs(CALIFORNIA_TRANSPORT, options, frames);
    const second = replayVehicleInputs(CALIFORNIA_TRANSPORT, options, frames);
    assert.deepEqual(first, second);
    assert.equal(first.trajectory.length, 142);
    assert.equal(first.final.elapsedSteps, 141);
    assert.equal(first.final.mode, "manual");
  });

  it("brakes deterministically for a lead vehicle and emits safety telemetry", () => {
    const make = () => new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-us-101",
      initialSpeedMps: 30,
      mode: "assisted",
    });
    const first = make();
    const replay = make();
    for (const controller of [first, replay]) {
      controller.setTrafficContext({ leadGapM: 12, leadSpeedMps: 8 });
      for (let index = 0; index < 90; index += 1) controller.stepFixed();
    }
    assert.deepEqual(first.snapshot(), replay.snapshot());
    assert.ok(first.state().speedMps < 20);
    assert.equal(first.state().trafficIntervention, true);
    assert.ok(first.state().collisionRisk > 0);
    assert.ok((first.state().timeHeadwaySeconds ?? 0) > 0);
    assert.ok(Number.isFinite(first.state().jerkMps3));
    assert.ok(first.state().laneKeepingScore >= 0 && first.state().laneKeepingScore <= 1);
  });

  it("restores nearest-lead context with the checkpointed controller state", () => {
    const make = () => new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-us-101",
      initialSpeedMps: 24,
      mode: "assisted",
    });
    const original = make();
    original.setTrafficContext({ leadGapM: 18, leadSpeedMps: 7 });
    for (let index = 0; index < 30; index += 1) original.stepFixed();

    const restored = make();
    restored.restore(original.snapshot());
    original.stepFixed();
    restored.stepFixed();

    assert.deepEqual(restored.snapshot(), original.snapshot());
    assert.equal(restored.state().leadGapM, 18);
    assert.equal(restored.state().leadSpeedMps, 7);
  });

  it("can switch route variants while preserving normalized progress", () => {
    const controller = new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-us-101",
      initialDistanceM: 220_000,
    });
    const progress = controller.state().progress;
    controller.setRoute("la-sf-i-5", true);
    assert.equal(controller.routeId(), "la-sf-i-5");
    assert.ok(Math.abs(controller.state().progress - progress) < 1e-12);
    assert.equal(controller.state().elapsedSteps, 0);
  });

  it("preserves the authored Bay endpoint across completion restore and reverse handoff", () => {
    const route = buildRoutePath(CALIFORNIA_TRANSPORT, "la-sf-us-101");
    const endpoint = sampleRoute(route, route.lengthM);
    assert.ok(Math.abs(endpoint.lat - 37.7749) < 1e-9);
    assert.ok(Math.abs(endpoint.lng - -122.4194) < 1e-9);

    const restored = new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-us-101",
      initialDistanceM: route.lengthM,
    });
    assert.equal(restored.state().progress, 1);
    assert.ok(Math.abs(restored.state().lat - endpoint.lat) < 1e-12);
    assert.ok(Math.abs(restored.state().lng - endpoint.lng) < 1e-12);

    restored.setRoute("la-sf-i-5", true);
    assert.equal(restored.state().progress, 1);
    assert.ok(Math.abs(restored.state().lat - 37.7749) < 1e-9);
    assert.ok(Math.abs(restored.state().lng - -122.4194) < 1e-9);

    const southbound = new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-i-5",
      direction: -1,
      initialSpeedMps: 20,
    });
    assert.equal(southbound.state().progress, 1);
    const before = southbound.state().distanceM;
    southbound.stepFixed({ throttle: 0.5 });
    assert.ok(southbound.state().distanceM < before);
    assert.ok(southbound.state().progress < 1 && southbound.state().progress > 0.99);
  });

  it("compresses corridor progress without changing vehicle dynamics", () => {
    const normal = new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-us-101",
      initialSpeedMps: 20,
      mode: "manual",
    });
    const compressed = new VehicleController(CALIFORNIA_TRANSPORT, {
      routeId: "la-sf-us-101",
      initialSpeedMps: 20,
      mode: "manual",
      travelScale: 900,
    });
    normal.stepFixed();
    compressed.stepFixed();
    assert.equal(compressed.state().speedMps, normal.state().speedMps);
    assert.equal(compressed.state().steering, normal.state().steering);
    assert.ok(compressed.state().distanceM > normal.state().distanceM * 800);
    assert.equal(compressed.state().wheelRadians, normal.state().wheelRadians);
  });
});
