Made of little things.

// Port of "ParentScript 5 - wheel": a raycast suspension wheel that queues
// spring, drive, drag and side-grip forces on the car's rigid body. Kept
// line-for-line with the Lingo, including its quirks:
// - rb.position + rb.centerOfMass adds the model-space COM offset unrotated;
// - forces use the steering angle set at the end of the previous update;
// - tLongitudonalForce is VOID (0) unless a key sets it.
// One addition: with no arrow key held, a controller stick steers to a
// partial angle (Input.steerAxis); without a stick the result is unchanged.
import { Matrix4, Vector3 } from 'three';
import type { Input } from '../input.js';
import type { HavokRigidBody } from '../physics/havok.js';
import type { RayWorld } from '../physics/rayWorld.js';
import type { HavokManager } from './havokManager.js';

const DEG = Math.PI / 180;

/** What the wheel needs from its car (car.ls). */
export interface WheelCar {
  readonly rb: HavokRigidBody;
  pSkidding: number;
  pReversing: number;
  /** The car model's world transform (synced from the rigid body). */
  modelTransform(): Matrix4;
  gravityLength(): number;
}

export class Wheel {
  readonly pTraction: number;
  readonly pIndex: number;
  readonly pPosition: Vector3;
  readonly pLeft: number;
  pOnGround = 0;
  pLoad = 0;
  pRotation = 0;
  pStopped = 0;
  pDistanceCovered = 0;
  readonly pRadius = 25;
  readonly pRestLength = 30;
  pSpringLength = 30;
  pGroundNormal = new Vector3(0, 0, 1);
  pRayStart = new Vector3();
  pRayISect = new Vector3();
  pSpeed = new Vector3();
  pRealWorldPos = new Vector3();

  // Visual wheel (pVisualGroup / tFlipGroup / pVisualModel).
  readonly visualGroupPosition = new Vector3();
  visualRotationZ = 0;

  constructor(
    private readonly car: WheelCar,
    private readonly havokManager: HavokManager,
    private readonly rays: RayWorld,
    private readonly input: Input,
    tPosition: Vector3,
    tTraction: number,
    tIndex: number,
  ) {
    this.pTraction = tTraction;
    this.pIndex = tIndex;
    this.pPosition = tPosition.clone();
    this.pLeft = tPosition.x < 0 ? 1 : 0;
  }

  /** pmodel: child of the car model at pPosition, rotated pRotation about Z. */
  private worldTransform(): Matrix4 {
    return this.car
      .modelTransform()
      .multiply(new Matrix4().makeTranslation(this.pPosition.x, this.pPosition.y, this.pPosition.z))
      .multiply(new Matrix4().makeRotationZ(this.pRotation * DEG));
  }

  update(): void {
    const input = this.input;
    const up = this.pStopped ? false : input.keyPressed(126);
    const down = this.pStopped ? false : input.keyPressed(125);
    const left = this.pStopped ? false : input.keyPressed(123);
    const right = this.pStopped ? false : input.keyPressed(124);
    const rb = this.car.rb;

    this.findRayISect();

    const world = this.worldTransform();
    const wheelPos = new Vector3().setFromMatrixPosition(world);
    const xAxis = new Vector3();
    const yAxis = new Vector3();
    const zAxis = new Vector3();
    world.extractBasis(xAxis, yAxis, zAxis);

    this.pSpeed = rb.linearVelocity.add(rb.angularVelocity.cross(wheelPos.clone().sub(rb.position.add(rb.centerOfMass))));
    const tSpeed = this.pSpeed.clone();
    tSpeed.z = 0;
    if (this.pOnGround) this.pDistanceCovered += tSpeed.length() * 0.1;

    const tDown = zAxis.clone().normalize().negate();
    this.pRealWorldPos = tDown.clone().multiplyScalar(this.pSpringLength - 25).add(wheelPos);
    this.actAsSpring();

    let tSideWayDirection = this.pGroundNormal.clone().cross(yAxis).normalize();
    const tForwardDirection = this.pGroundNormal.clone().negate().cross(tSideWayDirection).normalize();
    if (this.pLeft) tSideWayDirection = tSideWayDirection.negate();
    const tLocalForwardSpeed = this.pSpeed.dot(tForwardDirection);

    let tLongitudonalForce = 0;
    if (up) tLongitudonalForce = 6300;
    this.car.pReversing = 0;
    if (down) {
      if (tLocalForwardSpeed < 0) {
        tLongitudonalForce = -4000;
        this.car.pReversing = 1;
      } else {
        tLongitudonalForce = -10000;
      }
    }
    if ((input.keyPressed(' ') && Math.abs(tLocalForwardSpeed) > 10) || this.pStopped) {
      tLongitudonalForce = -10000 * sign(tLocalForwardSpeed);
    }

    const tWheelPos = this.pRealWorldPos.clone().applyMatrix4(this.car.modelTransform().invert());
    const tTotalForce = new Vector3();
    if (this.pTraction && this.pOnGround) {
      if (this.car.pSkidding) tLongitudonalForce *= 0.5;
      tTotalForce.addScaledVector(tForwardDirection, tLongitudonalForce);
    }
    if (this.pOnGround) {
      let tDrag = tLocalForwardSpeed * 6;
      if (this.car.pSkidding) tDrag *= 0.5;
      tTotalForce.addScaledVector(tForwardDirection, -tDrag);
    }

    if (this.pTraction === 1) {
      if (left) this.pRotation = Math.min(5, this.pRotation + 1);
      else if (right) this.pRotation = Math.max(-5, this.pRotation - 1);
      else {
        // No arrow key: a controller stick's lean sets the target angle (0
        // without one), reached 1° per update as the keys do. With no stick
        // this is the original's return to centre.
        const tTarget = this.pStopped ? 0 : -input.steerAxis() * 5;
        this.pRotation += Math.max(-1, Math.min(1, tTarget - this.pRotation));
      }
      this.visualRotationZ = -this.pRotation * 3;
    } else {
      this.visualRotationZ = 0;
    }

    if (this.pOnGround) {
      const tLocalSideWaySpeed = this.pSpeed.dot(tSideWayDirection);
      let tSideWayDrag = tLocalSideWaySpeed * 150;
      let tMaxDrag = 3.0 * this.pLoad + this.pTraction * 100;
      if (this.car.pSkidding) tMaxDrag *= 0.75;
      if (input.keyPressed(' ') && this.pTraction === 0) tMaxDrag *= 0.3;
      if (Math.abs(tSideWayDrag) > tMaxDrag) {
        tSideWayDrag *= 0.5;
        this.car.pSkidding = 1;
      } else {
        this.car.pSkidding = 0;
      }
      tTotalForce.addScaledVector(tSideWayDirection, -tSideWayDrag);
    }
    this.havokManager.applyForceAtPoint(rb, tTotalForce, tWheelPos);
  }

  private actAsSpring(): void {
    this.pLoad = 0;
    if (this.pOnGround === 0) return;
    const rb = this.car.rb;
    const tStrength = ((rb.mass * this.car.gravityLength()) / 4) * 10;
    const tdiff = this.pRayISect.clone().sub(this.pRayStart);
    const tForceLength = tStrength * (this.pRestLength - this.pSpringLength);
    const tLimit = 30;
    const tSpeedInDirectionOfForce = clamp(this.pSpeed.dot(tdiff.normalize()), -tLimit, tLimit);
    const tDampFactor = 0.09;
    const tDamping = tStrength * tDampFactor * tSpeedInDirectionOfForce;
    this.pLoad = tForceLength + tDamping;
    const tForce = this.pGroundNormal.clone().normalize().multiplyScalar(this.pLoad);
    this.havokManager.applyForceAtPoint(rb, tForce, this.pPosition);
  }

  stopDriving(): void {
    this.pStopped = 1;
  }

  findRayISect(): void {
    const world = this.worldTransform();
    const wheelPos = new Vector3().setFromMatrixPosition(world);
    const zAxis = new Vector3().setFromMatrixColumn(world, 2).normalize();
    const tDown = zAxis.clone().negate();
    const tStartOffset = 25;
    this.pRayStart = wheelPos.clone().addScaledVector(zAxis, tStartOffset);
    const hit = this.rays.modelsUnderRay(this.pRayStart, tDown);
    if (hit) {
      this.pRayISect = hit.isectPosition;
      const dist = this.pRayISect.distanceTo(this.pRayStart);
      if (dist <= this.pRestLength) {
        this.pSpringLength = Math.max(20, dist);
        this.pGroundNormal = hit.isectNormal;
        this.visualGroupPosition.copy(this.pRayStart).addScaledVector(tDown, this.pSpringLength).add(new Vector3(0, 0, 5.2));
        this.pOnGround = 1;
        return;
      }
    }
    this.pOnGround = 0;
    this.pSpringLength = this.pRestLength;
    this.visualGroupPosition.copy(this.pRayStart).addScaledVector(tDown, this.pSpringLength - 5.2);
  }
}

export function sign(v: number): number {
  return v >= 0 ? 1 : -1;
}

function clamp(v: number, lo: number, hi: number): number {
  return Math.min(Math.max(v, lo), hi);
}