/**
 * Renderer-independent solo vehicle controls for a route-relative simulation.
 *
 * Inputs are normalized action snapshots rather than DOM events, so keyboards,
 * gamepads, touch controls, remote clients, and recorded replays all drive the
 * same deterministic fixed-step state machine.
 */

import type { GeographicPoint, TransportPack } from "./types.ts";
import {
  VEHICLE_WHEEL_RADIUS_M,
  buildRoutePath,
  sampleRoute,
  type RoutePath,
  type RouteSample,
} from "./vehicleSim.ts";

const MPH_TO_MPS = 0.44704;
const EARTH_RADIUS_M = 6_371_000;
const TWO_PI = Math.PI * 2;

export type VehicleControlMode = "assisted" | "manual";
export type VehicleModeRequest = "none" | VehicleControlMode;

/** Device-neutral actions sampled for one rendered or fixed simulation frame. */
export interface VehicleActionSnapshot {
  /** Accelerator position in the inclusive range [0, 1]. */
  throttle: number;
  /** Service brake position in the inclusive range [0, 1]. */
  brake: number;
  /** Steering input where -1 is full left and 1 is full right. */
  steering: number;
  handbrake: boolean;
  /** One-shot mode request. Meaningful even when all analogue axes are neutral. */
  modeRequest: VehicleModeRequest;
  /** One-shot request to restore the configured initial state. */
  reset: boolean;
}

export const NEUTRAL_VEHICLE_ACTIONS: Readonly<VehicleActionSnapshot> = Object.freeze({
  throttle: 0,
  brake: 0,
  steering: 0,
  handbrake: false,
  modeRequest: "none",
  reset: false,
});

export interface VehicleControllerOptions {
  routeId: string;
  mode?: VehicleControlMode;
  direction?: 1 | -1;
  initialDistanceM?: number;
  initialLateralOffsetM?: number;
  initialSpeedMps?: number;
  /** Defaults to 60 Hz and is clamped to a safe simulation range. */
  fixedStepSeconds?: number;
  /** Caps catch-up after a sleeping tab. Defaults to 0.25 seconds. */
  maxFrameDeltaSeconds?: number;
  maximumSpeedMps?: number;
  assistedCruiseRatio?: number;
  guardrailOffsetM?: number;
  /**
   * Rolling radius, metres. Defaults to `VEHICLE_WHEEL_RADIUS_M`, which is the
   * radius of the wheel the renderer actually draws — see the note on that
   * constant for why the number lives in `vehicleSim.ts` and what happens when
   * it disagrees with the asset.
   */
  wheelRadiusM?: number;
  /**
   * Multiplies longitudinal route progress without changing acceleration or
   * steering response. State-scale boards use compression; metre-scale roads
   * leave this at 1. Defaults to 1.
   *
   * This is the dial that makes one controller serve both scales, and it is
   * worth being precise about what it does and does not touch. Only the
   * distance advanced along the route is multiplied. Speed, acceleration, jerk,
   * lateral offset, guardrail contact and wheel rotation are all still in real
   * metres and real seconds, so a car at 900 crosses California in minutes
   * while its wheels turn at the rate 24 m/s implies and its lane keeping is
   * scored against a real 5.4 m carriageway.
   *
   * `transport/exteriorVehicle.ts` publishes `METRE_SCALE_VEHICLE_OPTIONS` —
   * this at 1, with the ceiling, guardrail and cruise ratio that follow from
   * it — so a forecourt does not have to rediscover the other three. There is
   * deliberately no second controller: two state machines with the same
   * responsibilities and different constants is how two definitions of
   * "assisted" end up shipping in one product.
   */
  travelScale?: number;
}

export interface VehicleControllerState extends GeographicPoint {
  routeId: string;
  mode: VehicleControlMode;
  direction: 1 | -1;
  /** Distance from the route's declared start. A restored endpoint may equal route length. */
  distanceM: number;
  progress: number;
  /** Signed offset from route centre; positive is to the driver's right. */
  lateralOffsetM: number;
  speedMps: number;
  /** Smoothed normalized steering position, independent of input device. */
  steering: number;
  routeHeadingDeg: number;
  headingDeg: number;
  segmentId: string;
  roadName: string;
  speedLimitMph: number;
  wheelRadians: number;
  guardrailContact: boolean;
  /** Signed realized acceleration and jerk from the deterministic fixed step. */
  longitudinalAccelerationMps2: number;
  jerkMps3: number;
  /** 1 is centred/stable, 0 is at the configured road edge. */
  laneKeepingScore: number;
  leadGapM: number | null;
  leadSpeedMps: number | null;
  timeHeadwaySeconds: number | null;
  /** Normalized [0,1] closing/gap risk estimate. */
  collisionRisk: number;
  trafficIntervention: boolean;
  elapsedSteps: number;
}

export interface VehicleTrafficContext {
  leadGapM: number | null;
  leadSpeedMps: number | null;
}

export interface VehicleControllerSnapshot extends VehicleControllerState {}

/** A held input snapshot and its exact duration in fixed simulation steps. */
export interface TimedVehicleInputFrame {
  steps: number;
  actions?: Partial<VehicleActionSnapshot>;
}

export interface VehicleReplayResult {
  /** Initial state followed by one snapshot after every simulated step. */
  trajectory: readonly VehicleControllerSnapshot[];
  final: VehicleControllerSnapshot;
}

interface ResolvedOptions {
  routeId: string;
  mode: VehicleControlMode;
  direction: 1 | -1;
  initialDistanceM: number;
  initialLateralOffsetM: number;
  initialSpeedMps: number;
  fixedStepSeconds: number;
  maxFrameDeltaSeconds: number;
  maximumSpeedMps: number;
  assistedCruiseRatio: number;
  guardrailOffsetM: number;
  wheelRadiusM: number;
  travelScale: number;
}

function finiteOr(value: number | undefined, fallback: number): number {
  return typeof value === "number" && Number.isFinite(value) ? value : fallback;
}

function clamp(value: number, min: number, max: number): number {
  return Math.max(min, Math.min(max, value));
}

function wrap(value: number, modulus: number): number {
  return ((value % modulus) + modulus) % modulus;
}

function moveToward(value: number, target: number, maximumDelta: number): number {
  if (value < target) return Math.min(value + maximumDelta, target);
  if (value > target) return Math.max(value - maximumDelta, target);
  return value;
}

/** Clamp and sanitize input from any adapter before it reaches simulation. */
export function normalizeVehicleActions(
  actions: Partial<VehicleActionSnapshot> | undefined,
): VehicleActionSnapshot {
  const modeRequest = actions?.modeRequest;
  return {
    throttle: clamp(finiteOr(actions?.throttle, 0), 0, 1),
    brake: clamp(finiteOr(actions?.brake, 0), 0, 1),
    steering: clamp(finiteOr(actions?.steering, 0), -1, 1),
    handbrake: actions?.handbrake === true,
    modeRequest: modeRequest === "manual" || modeRequest === "assisted" ? modeRequest : "none",
    reset: actions?.reset === true,
  };
}

function resolveOptions(options: VehicleControllerOptions): ResolvedOptions {
  return {
    routeId: options.routeId,
    mode: options.mode === "manual" ? "manual" : "assisted",
    direction: options.direction === -1 ? -1 : 1,
    initialDistanceM: finiteOr(options.initialDistanceM, 0),
    initialLateralOffsetM: finiteOr(options.initialLateralOffsetM, 0),
    initialSpeedMps: Math.max(0, finiteOr(options.initialSpeedMps, 0)),
    fixedStepSeconds: clamp(finiteOr(options.fixedStepSeconds, 1 / 60), 1 / 240, 0.1),
    maxFrameDeltaSeconds: clamp(finiteOr(options.maxFrameDeltaSeconds, 0.25), 0.05, 1),
    maximumSpeedMps: clamp(finiteOr(options.maximumSpeedMps, 58), 5, 100),
    assistedCruiseRatio: clamp(finiteOr(options.assistedCruiseRatio, 0.92), 0.25, 1.1),
    guardrailOffsetM: clamp(finiteOr(options.guardrailOffsetM, 5.4), 1, 20),
    wheelRadiusM: clamp(finiteOr(options.wheelRadiusM, VEHICLE_WHEEL_RADIUS_M), 0.1, 1),
    travelScale: clamp(finiteOr(options.travelScale, 1), 1, 10_000),
  };
}

function hasManualIntent(actions: VehicleActionSnapshot): boolean {
  return (
    actions.modeRequest === "manual" ||
    actions.handbrake ||
    actions.throttle > 0.02 ||
    actions.brake > 0.02 ||
    Math.abs(actions.steering) > 0.08
  );
}

function offsetPoint(sample: RouteSample, lateralOffsetM: number): GeographicPoint {
  const heading = (sample.headingDeg * Math.PI) / 180;
  // Right-hand normal to a compass bearing: south for eastbound, east for northbound.
  const northM = -Math.sin(heading) * lateralOffsetM;
  const eastM = Math.cos(heading) * lateralOffsetM;
  const latitudeRadians = (sample.lat * Math.PI) / 180;
  return {
    lat: sample.lat + (northM / EARTH_RADIUS_M) * (180 / Math.PI),
    lng:
      sample.lng +
      (eastM / (EARTH_RADIUS_M * Math.max(0.01, Math.cos(latitudeRadians)))) * (180 / Math.PI),
  };
}

/**
 * Deterministic route-relative driving state machine.
 *
 * `tick` adapts render time to a fixed clock. `stepFixed` is the authoritative
 * primitive for tests, networking, and replay and always advances exactly once.
 */
export class VehicleController {
  private readonly pack: TransportPack;
  private readonly options: ResolvedOptions;
  private path: RoutePath;
  private accumulator = 0;
  private traffic: VehicleTrafficContext = { leadGapM: null, leadSpeedMps: null };
  private readonly current: VehicleControllerState;

  constructor(pack: TransportPack, options: VehicleControllerOptions) {
    this.pack = pack;
    this.options = resolveOptions(options);
    this.path = buildRoutePath(pack, this.options.routeId);
    if (options.initialDistanceM === undefined && this.options.direction === -1) {
      this.options.initialDistanceM = this.path.lengthM;
    }
    const sample = sampleRoute(this.path, this.options.initialDistanceM, this.options.direction);
    const point = offsetPoint(sample, 0);
    this.current = {
      ...point,
      routeId: this.path.route.id,
      mode: this.options.mode,
      direction: this.options.direction,
      distanceM: 0,
      progress: 0,
      lateralOffsetM: 0,
      speedMps: 0,
      steering: 0,
      routeHeadingDeg: sample.headingDeg,
      headingDeg: sample.headingDeg,
      segmentId: sample.segmentId,
      roadName: sample.roadName,
      speedLimitMph: sample.speedLimitMph,
      wheelRadians: 0,
      guardrailContact: false,
      longitudinalAccelerationMps2: 0,
      jerkMps3: 0,
      laneKeepingScore: 1,
      leadGapM: null,
      leadSpeedMps: null,
      timeHeadwaySeconds: null,
      collisionRisk: 0,
      trafficIntervention: false,
      elapsedSteps: 0,
    };
    this.reset();
  }

  fixedStepSeconds(): number {
    return this.options.fixedStepSeconds;
  }

  routeId(): string {
    return this.path.route.id;
  }

  /** Stable state object for allocation-free polling. Treat it as read-only. */
  state(): Readonly<VehicleControllerState> {
    return this.current;
  }

  /** Detached state suitable for logs, network frames, and equality assertions. */
  snapshot(): VehicleControllerSnapshot {
    return { ...this.current };
  }

  /** Restore a trusted JSON snapshot for deterministic environment checkpointing. */
  restore(snapshot: VehicleControllerSnapshot): void {
    const numeric = Object.entries(snapshot)
      .filter(([, value]) => typeof value === "number")
      .every(([, value]) => Number.isFinite(value));
    if (
      !numeric || snapshot.routeId !== this.path.route.id ||
      snapshot.direction !== this.options.direction ||
      (snapshot.mode !== "manual" && snapshot.mode !== "assisted") ||
      snapshot.distanceM < 0 || snapshot.distanceM > this.path.lengthM ||
      Math.abs(snapshot.lateralOffsetM) > this.options.guardrailOffsetM ||
      snapshot.speedMps < 0 || snapshot.speedMps > this.options.maximumSpeedMps ||
      !Number.isSafeInteger(snapshot.elapsedSteps) || snapshot.elapsedSteps < 0
    ) throw new RangeError("vehicle snapshot is incompatible or invalid");
    Object.assign(this.current, snapshot);
    this.traffic = {
      leadGapM: snapshot.leadGapM,
      leadSpeedMps: snapshot.leadSpeedMps,
    };
    this.accumulator = 0;
  }

  /** Supply a renderer/simulation-neutral nearest-lead observation. */
  setTrafficContext(context: Partial<VehicleTrafficContext> | null): void {
    const gap = context?.leadGapM;
    const speed = context?.leadSpeedMps;
    this.traffic = {
      leadGapM: typeof gap === "number" && Number.isFinite(gap) && gap >= 0 ? gap : null,
      leadSpeedMps: typeof speed === "number" && Number.isFinite(speed) && speed >= 0 ? speed : null,
    };
  }

  /** Restore the configured spawn state and clear pending fractional time. */
  reset(): void {
    this.accumulator = 0;
    const wrappedDistanceM = wrap(this.options.initialDistanceM, this.path.lengthM);
    const distanceM = wrappedDistanceM === 0 &&
      this.options.initialDistanceM > 0 &&
      this.options.initialDistanceM <= this.path.lengthM
      ? this.path.lengthM
      : wrappedDistanceM;
    const lateralOffsetM = clamp(
      this.options.initialLateralOffsetM,
      -this.options.guardrailOffsetM,
      this.options.guardrailOffsetM,
    );
    const speedMps = clamp(this.options.initialSpeedMps, 0, this.options.maximumSpeedMps);
    const sample = sampleRoute(this.path, distanceM, this.options.direction);
    const point = offsetPoint(sample, lateralOffsetM);
    Object.assign(this.current, point, {
      routeId: this.path.route.id,
      mode: this.options.mode,
      direction: this.options.direction,
      distanceM,
      progress: distanceM / this.path.lengthM,
      lateralOffsetM,
      speedMps,
      steering: 0,
      routeHeadingDeg: sample.headingDeg,
      headingDeg: sample.headingDeg,
      segmentId: sample.segmentId,
      roadName: sample.roadName,
      speedLimitMph: sample.speedLimitMph,
      wheelRadians: 0,
      guardrailContact: false,
      longitudinalAccelerationMps2: 0,
      jerkMps3: 0,
      laneKeepingScore: 1 - Math.abs(lateralOffsetM) / this.options.guardrailOffsetM,
      leadGapM: this.traffic.leadGapM,
      leadSpeedMps: this.traffic.leadSpeedMps,
      timeHeadwaySeconds: null,
      collisionRisk: 0,
      trafficIntervention: false,
      elapsedSteps: 0,
    });
  }

  /**
   * Change corridor as an explicit reset. Progress may optionally be preserved,
   * which is useful for switching route variants without retaining stale metres.
   */
  setRoute(routeId: string, preserveProgress = false): void {
    if (routeId === this.path.route.id) return;
    const previousProgress = this.current.progress;
    this.path = buildRoutePath(this.pack, routeId);
    this.options.routeId = routeId;
    this.options.initialDistanceM = preserveProgress ? previousProgress * this.path.lengthM : 0;
    this.traffic = { leadGapM: null, leadSpeedMps: null };
    this.reset();
  }

  /** Advance rendered seconds and return the number of fixed steps executed. */
  tick(
    deltaSeconds: number,
    actions: Partial<VehicleActionSnapshot> = NEUTRAL_VEHICLE_ACTIONS,
  ): number {
    if (!Number.isFinite(deltaSeconds) || deltaSeconds <= 0) return 0;
    const normalized = normalizeVehicleActions(actions);
    if (normalized.reset) {
      this.reset();
      return 0;
    }
    this.accumulator += Math.min(deltaSeconds, this.options.maxFrameDeltaSeconds);
    let steps = 0;
    while (this.accumulator + Number.EPSILON >= this.options.fixedStepSeconds) {
      this.stepNormalized(normalized);
      this.accumulator -= this.options.fixedStepSeconds;
      steps += 1;
    }
    return steps;
  }

  /** Advance exactly one authoritative simulation step. */
  stepFixed(actions: Partial<VehicleActionSnapshot> = NEUTRAL_VEHICLE_ACTIONS): void {
    const normalized = normalizeVehicleActions(actions);
    if (normalized.reset) {
      this.reset();
      return;
    }
    this.stepNormalized(normalized);
  }

  private stepNormalized(actions: VehicleActionSnapshot): void {
    const dt = this.options.fixedStepSeconds;
    const previousSpeedMps = this.current.speedMps;
    const previousAcceleration = this.current.longitudinalAccelerationMps2;
    const manualIntent = hasManualIntent(actions);
    // Direct human input always wins, including over a simultaneous request to
    // resume assistance. A neutral assisted request can re-engage on the next step.
    if (manualIntent) this.current.mode = "manual";
    else if (actions.modeRequest === "assisted") this.current.mode = "assisted";

    let throttle = actions.throttle;
    let brake = actions.brake;
    let steeringTarget = actions.steering;

    if (this.current.mode === "assisted") {
      const roadTarget = this.current.speedLimitMph * MPH_TO_MPS * this.options.assistedCruiseRatio;
      let targetSpeed = Math.min(roadTarget, this.options.maximumSpeedMps);
      const safeGapM = Math.max(10, this.current.speedMps * 1.8);
      if (this.traffic.leadGapM !== null && this.traffic.leadSpeedMps !== null) {
        targetSpeed = Math.min(
          targetSpeed,
          Math.max(0, this.traffic.leadSpeedMps + (this.traffic.leadGapM - safeGapM) * 0.55),
        );
      }
      const speedError = targetSpeed - this.current.speedMps;
      throttle = clamp(speedError / 5, 0, 1);
      brake = clamp(-speedError / 7, 0, 1);
      steeringTarget = clamp(-this.current.lateralOffsetM / 2.4, -1, 1);
    }

    this.current.steering = moveToward(this.current.steering, steeringTarget, 3.8 * dt);

    const aeroDrag = this.current.speedMps * this.current.speedMps * 0.0018;
    const rollingDrag = this.current.speedMps > 0 ? 0.12 : 0;
    const engineFade = 1 - 0.55 * (this.current.speedMps / this.options.maximumSpeedMps);
    const acceleration =
      throttle * 5.4 * Math.max(0.2, engineFade) -
      brake * 9.5 -
      (actions.handbrake ? 13 : 0) -
      aeroDrag -
      rollingDrag;
    this.current.speedMps = clamp(
      this.current.speedMps + acceleration * dt,
      0,
      this.options.maximumSpeedMps,
    );

    const previousDistance = this.current.distanceM;
    const physicalTravelled = this.current.speedMps * dt;
    const routeTravelled = physicalTravelled * this.options.travelScale;
    this.current.distanceM = wrap(
      previousDistance + routeTravelled * this.current.direction,
      this.path.lengthM,
    );

    let proposedLateral =
      this.current.lateralOffsetM + this.current.steering * this.current.speedMps * 0.2 * dt;
    if (this.current.mode === "assisted") {
      // Assistance damps the final few centimetres without an abrupt lane snap.
      proposedLateral *= Math.exp(-0.35 * dt);
    }
    this.current.guardrailContact = Math.abs(proposedLateral) > this.options.guardrailOffsetM;
    if (this.current.guardrailContact) {
      proposedLateral = clamp(
        proposedLateral,
        -this.options.guardrailOffsetM,
        this.options.guardrailOffsetM,
      );
      this.current.speedMps = Math.min(this.current.speedMps * 0.78, 12);
      this.current.steering *= 0.35;
    }
    this.current.lateralOffsetM = proposedLateral;

    const sample = sampleRoute(this.path, this.current.distanceM, this.current.direction);
    const point = offsetPoint(sample, this.current.lateralOffsetM);
    const wheelDelta = physicalTravelled / this.options.wheelRadiusM;
    const realizedAcceleration = (this.current.speedMps - previousSpeedMps) / dt;
    const leadGapM = this.traffic.leadGapM;
    const leadSpeedMps = this.traffic.leadSpeedMps;
    const timeHeadwaySeconds = leadGapM === null || this.current.speedMps < 0.1
      ? null
      : leadGapM / this.current.speedMps;
    const closingSpeedMps = leadSpeedMps === null ? 0 : Math.max(0, this.current.speedMps - leadSpeedMps);
    const timeToCollision = leadGapM === null || closingSpeedMps < 0.1
      ? Number.POSITIVE_INFINITY
      : leadGapM / closingSpeedMps;
    const gapRisk = leadGapM === null ? 0 : clamp(1 - leadGapM / Math.max(12, this.current.speedMps * 2), 0, 1);
    const collisionRisk = Math.max(gapRisk, clamp(1 - timeToCollision / 6, 0, 1));
    Object.assign(this.current, point, {
      progress: this.current.distanceM / this.path.lengthM,
      routeHeadingDeg: sample.headingDeg,
      headingDeg: sample.headingDeg + this.current.steering * 9,
      segmentId: sample.segmentId,
      roadName: sample.roadName,
      speedLimitMph: sample.speedLimitMph,
      wheelRadians: wrap(this.current.wheelRadians + wheelDelta, TWO_PI),
      longitudinalAccelerationMps2: realizedAcceleration,
      jerkMps3: (realizedAcceleration - previousAcceleration) / dt,
      laneKeepingScore: clamp(1 - Math.abs(this.current.lateralOffsetM) / this.options.guardrailOffsetM, 0, 1),
      leadGapM,
      leadSpeedMps,
      timeHeadwaySeconds,
      collisionRisk,
      trafficIntervention:
        this.current.mode === "assisted" &&
        leadGapM !== null &&
        leadGapM < Math.max(12, this.current.speedMps * 2.1),
      elapsedSteps: this.current.elapsedSteps + 1,
    });
  }
}

/** Execute an exact, renderer-independent input recording. */
export function replayVehicleInputs(
  pack: TransportPack,
  options: VehicleControllerOptions,
  frames: readonly TimedVehicleInputFrame[],
): VehicleReplayResult {
  const controller = new VehicleController(pack, options);
  const trajectory: VehicleControllerSnapshot[] = [controller.snapshot()];
  for (const frame of frames) {
    const steps = Math.max(0, Math.floor(finiteOr(frame.steps, 0)));
    const actions = normalizeVehicleActions(frame.actions);
    for (let index = 0; index < steps; index += 1) {
      // Reset and mode requests are edge-triggered at the start of a timed frame.
      controller.stepFixed(
        index === 0
          ? actions
          : { ...actions, modeRequest: "none", reset: false },
      );
      trajectory.push(controller.snapshot());
    }
  }
  const final = trajectory.at(-1) ?? controller.snapshot();
  return { trajectory, final };
}
