Made of little things.

// A Rapier-backed stand-in for the Director Havok Xtra, exposing the subset of
// its Lingo API that SuperSonic RC uses, with the Xtra's units and quirks:
// - everything is in scene units (inches here), mass in kg;
// - angularVelocity is in rad/s (the Lingo Reference says degrees, but the
//   original's own logs show w = 0.59 while rotating 33.8 deg/s);
// - applyForceAtPoint takes the point in MODEL space;
// - centerOfMass is the model-space offset from the model origin;
// - step(t, n) advances t seconds in n equal substeps. Measured in the original
//   (tools/sim logger, docs/physics-calibration.md): forces queued before a
//   step land as ONE impulse F * t * FORCE_IMPULSE_SCALE at its start, ~the
//   0.0254 world scale (the Xtra converts to SI and never converts back).
//   Evidence: resting vz = -3 g t/n, the logged spring loads and drag decay.
import RAPIER from '@dimforge/rapier3d-compat';
import { Matrix4, Quaternion, Vector3 } from 'three';

export interface CollisionInfo {
  /** [name1, name2, contactPoint, contactNormal, normalRelativeVelocity] */
  rbName1: string;
  rbName2: string;
  contactPoint: Vector3;
  contactNormal: Vector3;
  normalRelativeVelocity: number;
}

export interface StaticBody {
  name: string;
  convex: boolean;
  friction: number;
  restitution: number;
  verts: Float32Array;
  faces: Uint32Array;
}

interface Interest {
  frequency: number;
  threshold: number;
  handler: (info: CollisionInfo) => void;
  lastCall: number;
}

export interface HavokOptions {
  /** Scene units per metre: the HKE scale is 0.0254 (inches). */
  scale: number;
  gravity: Vector3;
  /** Chassis friction; the solver uses the lower of it and the scenery's (fits the logs). */
  contactFriction: number;
  /** Experiment: centre of mass starts at the model origin instead of the hull centroid. */
  comFromOrigin?: boolean;
  /**
   * How queued forces act during step(t, n): "impulse" (measured, default) = one
   * impulse F*t*forceScale before the first substep; "firstSubstep" / "span" are
   * the rejected hypotheses, kept for comparison runs.
   */
  forceModel: 'impulse' | 'firstSubstep' | 'span';
  /** Experiment: applyForceAtPoint adds the model-space point to the position without rotating it. */
  unrotatedForcePoint?: boolean;
  /** Experiment: scale the hull's solid-box inertia. */
  inertiaScale?: number;
  /** Extra multiplier on the angular part of queued impulses (fit against the original). */
  torqueFactor?: number;
  /** Multiplier on queued forces for the impulse model; defaults to FORCE_IMPULSE_SCALE. */
  forceScale?: number;
  /** Gap the solver keeps between movable bodies and the scenery; defaults to HAVOK_CONTACT_GAP. */
  contactGap?: number;
  /** Rapier solver tuning (natural frequency Hz; allowed error and prediction in scene units). */
  contactFrequency?: number;
  allowedPenetration?: number;
  predictionDistance?: number;
  solverIterations?: number;
}

/** Fitted from the original's equilibrium spring loads (pLoad 2429.8 at spring 25.919). */
export const FORCE_IMPULSE_SCALE = 0.02417;

/**
 * Havok holds touching bodies about this far apart (scene units). The world's
 * collision tolerance is 3.937 (0.1 m, logged); an upside-down car rests with
 * its roof 3.3 above the floor (z 344.0 vs 340.7 for exact contact). The gap
 * is what stops the chassis before the springs bottom out on hard landings.
 */
export const HAVOK_CONTACT_GAP = 3.3;

/** The level's Havok collision tolerance (member("havok").tolerance, logged). */
export const HAVOK_TOLERANCE = 3.937;

let rapierReady: Promise<void> | null = null;
export function initPhysics(): Promise<void> {
  rapierReady ??= RAPIER.init();
  return rapierReady;
}

export class HavokRigidBody {
  readonly name: string;
  readonly mass: number;
  restitution = 0;
  friction = 0;
  /** Model-space offset from the model origin to the centre of mass. */
  private com = new Vector3();
  private readonly force = new Vector3();
  private readonly torque = new Vector3();

  constructor(
    private readonly world: HavokWorld,
    readonly body: RAPIER.RigidBody,
    readonly collider: RAPIER.Collider,
    name: string,
    mass: number,
    private readonly inertia: Vector3,
    hullCentroid: Vector3,
  ) {
    this.name = name;
    this.mass = mass;
    this.com.copy(hullCentroid);
    this.applyMassProperties();
  }

  private applyMassProperties(): void {
    this.body.setAdditionalMassProperties(
      this.mass,
      { x: this.com.x, y: this.com.y, z: this.com.z },
      { x: this.inertia.x, y: this.inertia.y, z: this.inertia.z },
      { x: 0, y: 0, z: 0, w: 1 },
      true,
    );
  }

  get position(): Vector3 {
    const t = this.body.translation();
    return new Vector3(t.x, t.y, t.z);
  }

  set position(v: Vector3) {
    this.body.setTranslation({ x: v.x, y: v.y, z: v.z }, true);
  }

  get quaternion(): Quaternion {
    const r = this.body.rotation();
    return new Quaternion(r.x, r.y, r.z, r.w);
  }

  set quaternion(q: Quaternion) {
    this.body.setRotation({ x: q.x, y: q.y, z: q.z, w: q.w }, true);
  }

  /** Model-to-world transform (what Havok writes back onto the W3D model). */
  get transform(): Matrix4 {
    return new Matrix4().compose(this.position, this.quaternion, new Vector3(1, 1, 1));
  }

  get centerOfMass(): Vector3 {
    return this.com.clone();
  }

  /** Havok keeps the inertia tensor and moves the centre of mass. */
  shiftCenterOfMass(v: Vector3): void {
    this.com.add(v);
    this.applyMassProperties();
  }

  get linearVelocity(): Vector3 {
    const v = this.body.linvel();
    return new Vector3(v.x, v.y, v.z);
  }

  set linearVelocity(v: Vector3) {
    this.body.setLinvel({ x: v.x, y: v.y, z: v.z }, true);
  }

  /** Radians per second about the vector's axis (see the header note). */
  get angularVelocity(): Vector3 {
    const w = this.body.angvel();
    return new Vector3(w.x, w.y, w.z);
  }

  set angularVelocity(v: Vector3) {
    this.body.setAngvel({ x: v.x, y: v.y, z: v.z }, true);
  }

  /** World-frame inertia tensor about the centre of mass, in SI (kg m^2). */
  private worldInertiaSI(): Matrix4 {
    const s = this.world.options.scale;
    const r = new Matrix4().makeRotationFromQuaternion(this.quaternion);
    const i = new Matrix4().makeScale(this.inertia.x * s * s, this.inertia.y * s * s, this.inertia.z * s * s);
    return r.clone().multiply(i).multiply(r.transpose());
  }

  /** kg m^2 rad/s, Havok's internal units. */
  get angularMomentum(): Vector3 {
    const w = this.body.angvel();
    return new Vector3(w.x, w.y, w.z).applyMatrix4(this.worldInertiaSI());
  }

  set angularMomentum(l: Vector3) {
    const w = l.clone().applyMatrix4(this.worldInertiaSI().invert());
    this.body.setAngvel({ x: w.x, y: w.y, z: w.z }, true);
  }

  applyForce(f: Vector3): void {
    this.force.add(f);
  }

  /** Force (world frame) at a model-space point; the lever runs to the centre of mass. */
  applyForceAtPoint(f: Vector3, modelPoint: Vector3): void {
    const t = this.transform;
    const point = this.world.options.unrotatedForcePoint ? modelPoint.clone().add(this.position) : modelPoint.clone().applyMatrix4(t);
    const com = this.com.clone().applyMatrix4(t);
    this.force.add(f);
    this.torque.add(point.sub(com).cross(f));
  }

  /** @internal Deliver the queued forces as one impulse (impulse model). */
  pushImpulse(scale: number): void {
    const j = this.force.clone().multiplyScalar(scale);
    const h = this.torque.clone().multiplyScalar(scale * (this.world.options.torqueFactor ?? 1));
    this.body.applyImpulse({ x: j.x, y: j.y, z: j.z }, true);
    this.body.applyTorqueImpulse({ x: h.x, y: h.y, z: h.z }, true);
    this.force.set(0, 0, 0);
    this.torque.set(0, 0, 0);
  }

  /** @internal */
  pushForces(): void {
    this.body.resetForces(true);
    this.body.resetTorques(true);
    this.body.addForce({ x: this.force.x, y: this.force.y, z: this.force.z }, true);
    this.body.addTorque({ x: this.torque.x, y: this.torque.y, z: this.torque.z }, true);
  }

  /** @internal */
  clearForces(): void {
    this.force.set(0, 0, 0);
    this.torque.set(0, 0, 0);
    this.body.resetForces(true);
    this.body.resetTorques(true);
  }
}

export class HavokWorld {
  readonly options: HavokOptions;
  private readonly world: RAPIER.World;
  private readonly bodies: HavokRigidBody[] = [];
  private readonly staticNames = new Map<number, string>();
  private readonly interests = new Map<HavokRigidBody, Interest[]>();
  private stepCallbacks: Array<(dt: number) => void> = [];
  simTime = 0;
  timeStep = 0;
  subSteps = 1;

  constructor(options: HavokOptions, statics: StaticBody[]) {
    this.options = options;
    const g = options.gravity;
    this.world = new RAPIER.World({ x: g.x, y: g.y, z: g.z });
    this.world.lengthUnit = 1 / options.scale;
    // Fitted to the original's scripted drop tests (docs/physics-calibration.md):
    // contacts appear within the Havok tolerance (gap + prediction = 3.937), and
    // a single velocity iteration avoids the springy rebound Rapier's default
    // four give when the chassis slams down.
    const ip = this.world.integrationParameters;
    if (options.contactFrequency !== undefined) ip.contact_natural_frequency = options.contactFrequency;
    if (options.allowedPenetration !== undefined) ip.normalizedAllowedLinearError = options.allowedPenetration * options.scale;
    ip.normalizedPredictionDistance = (options.predictionDistance ?? HAVOK_TOLERANCE - (options.contactGap ?? HAVOK_CONTACT_GAP)) * options.scale;
    ip.numSolverIterations = options.solverIterations ?? 1;
    const ground = this.world.createRigidBody(RAPIER.RigidBodyDesc.fixed());
    for (const body of statics) {
      const desc = body.convex
        ? RAPIER.ColliderDesc.convexHull(body.verts) ?? RAPIER.ColliderDesc.trimesh(body.verts, body.faces)
        : RAPIER.ColliderDesc.trimesh(body.verts, body.faces);
      desc.setFriction(body.friction).setRestitution(0);
      const collider = this.world.createCollider(desc, ground);
      this.staticNames.set(collider.handle, body.name);
    }
  }

  get gravity(): Vector3 {
    const g = this.world.gravity;
    return new Vector3(g.x, g.y, g.z);
  }

  set gravity(v: Vector3) {
    this.world.gravity = { x: v.x, y: v.y, z: v.z };
  }

  /**
   * makeMovableRigidBody(modelName, mass, isConvex): a convex hull of the
   * model's vertices (model space), solid-hull inertia, zero drag.
   */
  makeMovableRigidBody(name: string, mass: number, hull: Float32Array, transform: Matrix4): HavokRigidBody {
    const position = new Vector3();
    const quaternion = new Quaternion();
    transform.decompose(position, quaternion, new Vector3());
    const body = this.world.createRigidBody(
      RAPIER.RigidBodyDesc.dynamic()
        .setTranslation(position.x, position.y, position.z)
        .setRotation({ x: quaternion.x, y: quaternion.y, z: quaternion.z, w: quaternion.w })
        .setLinearDamping(0)
        .setAngularDamping(0)
        .setCanSleep(false),
    );
    const desc = RAPIER.ColliderDesc.convexHull(hull);
    if (!desc) throw new Error(`cannot build convex hull for ${name}`);
    desc
      .setDensity(0)
      .setFriction(this.options.contactFriction)
      .setFrictionCombineRule(RAPIER.CoefficientCombineRule.Min)
      .setRestitution(0)
      .setRestitutionCombineRule(RAPIER.CoefficientCombineRule.Min)
      .setContactSkin(this.options.contactGap ?? HAVOK_CONTACT_GAP);
    const collider = this.world.createCollider(desc, body);
    const { centroid, inertia } = boxInertia(hull, mass);
    inertia.multiplyScalar(this.options.inertiaScale ?? 1);
    const rb = new HavokRigidBody(this, body, collider, name, mass, inertia, this.options.comFromOrigin ? new Vector3() : centroid);
    this.bodies.push(rb);
    return rb;
  }

  removeBody(rb: HavokRigidBody): void {
    this.interests.delete(rb);
    const i = this.bodies.indexOf(rb);
    if (i >= 0) this.bodies.splice(i, 1);
    this.world.removeRigidBody(rb.body);
  }

  rigidBody(name: string): HavokRigidBody | undefined {
    return this.bodies.find((b) => b.name === name);
  }

  /** registerInterest(rb, #all, frequency, threshold, #handler, script) */
  registerInterest(rb: HavokRigidBody, frequency: number, threshold: number, handler: (info: CollisionInfo) => void): void {
    const list = this.interests.get(rb) ?? [];
    list.push({ frequency, threshold, handler, lastCall: -Infinity });
    this.interests.set(rb, list);
  }

  registerStepCallback(cb: (dt: number) => void): void {
    this.stepCallbacks.push(cb);
  }

  step(timeIncrement: number, subSteps: number): void {
    this.timeStep = timeIncrement;
    this.subSteps = subSteps;
    const dt = timeIncrement / subSteps;
    this.world.timestep = dt;
    const model = this.options.forceModel;
    for (const rb of this.bodies) {
      if (model === 'impulse') rb.pushImpulse(timeIncrement * (this.options.forceScale ?? FORCE_IMPULSE_SCALE));
      else rb.pushForces();
    }
    for (let i = 0; i < subSteps; i++) {
      this.world.step();
      this.simTime += dt;
      if (i === 0 && model === 'firstSubstep') {
        for (const rb of this.bodies) rb.clearForces();
      }
      this.dispatchCollisions();
      for (const cb of this.stepCallbacks) cb(dt);
    }
    for (const rb of this.bodies) rb.clearForces();
  }

  /**
   * Havok calls the interest handler for every body pair closer than the
   * collision tolerance. Rapier's narrow phase keeps (predictive) contacts for
   * the pair; the deepest one stands in for Havok's contact point. Solver
   * contacts are not readable from JS after the step, so this uses the
   * geometric contacts.
   */
  private dispatchCollisions(): void {
    for (const [rb, interests] of this.interests) {
      this.world.contactPairsWith(rb.collider, (other) => {
        // Assigned inside the callback, so TS's narrowing needs the cast.
        let best = null as { dist: number; local: Vector3; normal: Vector3 } | null;
        this.world.contactPair(rb.collider, other, (manifold, flipped) => {
          for (let i = 0; i < manifold.numContacts(); i++) {
            const dist = manifold.contactDist(i);
            if (dist > HAVOK_TOLERANCE || (best && dist >= best.dist)) continue;
            const p = flipped ? manifold.localContactPoint2(i) : manifold.localContactPoint1(i);
            if (!p) continue;
            const n = manifold.normal();
            const normal = new Vector3(n.x, n.y, n.z);
            if (flipped) normal.negate();
            best = { dist, local: new Vector3(p.x, p.y, p.z), normal };
          }
        });
        if (!best) return;
        const { local, normal } = best;
        const t = rb.transform;
        const point = local.applyMatrix4(t);
        // Relative contact velocity includes both moving cars.
        const com = rb.centerOfMass.applyMatrix4(t);
        const pointVel = rb.linearVelocity.add(rb.angularVelocity.cross(point.clone().sub(com)));
        const otherRb = this.bodies.find((b) => b.collider.handle === other.handle);
        if (otherRb) {
          const otherCom = otherRb.centerOfMass.applyMatrix4(otherRb.transform);
          pointVel.sub(otherRb.linearVelocity.add(otherRb.angularVelocity.cross(point.clone().sub(otherCom))));
        }
        const nrv = Math.abs(pointVel.dot(normal));
        const info: CollisionInfo = {
          rbName1: rb.name,
          rbName2: otherRb?.name ?? this.staticNames.get(other.handle) ?? '',
          contactPoint: point,
          contactNormal: normal,
          normalRelativeVelocity: nrv,
        };
        for (const interest of interests) {
          if (nrv < interest.threshold) continue;
          if (interest.frequency > 0 && this.simTime - interest.lastCall < 1 / interest.frequency) continue;
          interest.lastCall = this.simTime;
          interest.handler(info);
        }
      });
    }
  }

  free(): void {
    this.world.free();
  }
}

/** Solid-box inertia of the hull's bounds (the car proxy is a box). */
function boxInertia(hull: Float32Array, mass: number): { centroid: Vector3; inertia: Vector3 } {
  const lo = new Vector3(Infinity, Infinity, Infinity);
  const hi = new Vector3(-Infinity, -Infinity, -Infinity);
  for (let i = 0; i < hull.length; i += 3) {
    lo.min(new Vector3(hull[i], hull[i + 1], hull[i + 2]));
    hi.max(new Vector3(hull[i], hull[i + 1], hull[i + 2]));
  }
  const d = hi.clone().sub(lo);
  return {
    centroid: lo.clone().add(hi).multiplyScalar(0.5),
    inertia: new Vector3(
      (mass / 12) * (d.y * d.y + d.z * d.z),
      (mass / 12) * (d.x * d.x + d.z * d.z),
      (mass / 12) * (d.x * d.x + d.y * d.y),
    ),
  };
}