import type {
  Collider,
  ColliderDesc,
  EventQueue,
  RigidBody,
  World as RapierWorld,
} from '@dimforge/rapier2d-compat';
import { isHandleEnabled, worldMatrixOf } from '../../core/graph';
import { apply, determinant, IDENTITY, invert, type Mat } from '../../core/model/matrix';
import type { Vec2 } from '../../core/model/Properties';
import { parentChain } from '../../core/model/walk';
import { type ColliderShape, isTrigger, shapesOverlap } from '../../core/properties/collider';
import { hasTransformProps } from '../../core/properties/PropertySchema';
import {
  type BodyType,
  colliderMaterial,
  type RigidbodySettings,
  rigidbodySettings,
  sceneGravity,
} from '../../core/properties/rigidbody';
import type { Entity } from '../Entity';
import type { World } from '../World';
import {
  type Blocked,
  blockingCollidersOf,
  bodyOf,
  type CollisionPair,
  type CollisionSource,
  type MoveResult,
  partsOf,
  shapeOf,
  Solids,
} from '../systems/solids';
import type { System } from '../systems/System';
import { type Rapier, toMetres, toPixels } from './engine';

/** The engine always steps this long, whatever the frame took. */
export const PHYSICS_STEP = 1 / 60;
/** After a hitch the world catches up this many steps at most, and forgets the rest. */
const MAX_STEPS_PER_TICK = 8;
/** A pose that moved less than this (world units, radians) was not moved by anyone. */
const POSE_EPSILON = 1e-6;

interface Pose {
  x: number;
  y: number;
  angle: number;
}

/**
 * One body in the engine. `rigid` is an object with a rigidbody child, carrying every collider below
 * it; `free` is a collider of an object without one, a body of its own that goes where the object
 * is put; `tilemap` is a placed map with a box per merged rectangle of its solid cells.
 */
interface Body {
  kind: 'rigid' | 'free' | 'tilemap';
  body: RigidBody;
  colliders: Collider[];
  /** What the colliders were built from; they are built again when it changes (a scaled object). */
  shapeKey: string;
  settingsKey: string;
  /** Whether the engine may turn it – what `writeBack` hands on as the object's rotation. */
  turns: boolean;
  /** The mass it carries of its own because no collider gives it one (kg, 0 = none). */
  ownMass: number;
  /** Where the pose was last put, by us or by the object moving. */
  placed: Pose;
  /** A dynamic body's pose before and after the last step, for drawing in between. */
  previous: Pose;
  current: Pose;
}

/** An engine collider: the entity it stands for and its world shape at the last sync. */
interface Part {
  entity: Entity;
  shape: ColliderShape;
  owner: Body;
}

/**
 * The played game's physics on Rapier (#146): rigidbodies fall, bounce and rest, and the collision
 * events come from the engine's contact pairs. Ticked first, like the collision system it stands in
 * for, so the behaviors see this frame's events.
 *
 * What has no rigidbody keeps behaving as it did before the engine: a collider is a body of its own
 * that goes wherever the object is put, and whether two such colliders touch is decided by the
 * editor's own overlap test – the engine only proposes the pairs. Touching without overlapping is
 * no collision, as it never was. Only where a dynamic body is involved does the engine's contact say it.
 */
export class PhysicsSystem implements System, CollisionSource {
  enters: CollisionPair[] = [];
  stays: CollisionPair[] = [];
  exits: CollisionPair[] = [];
  private engine: RapierWorld | null = null;
  private queue: EventQueue | null = null;
  private readonly bodies = new Map<Entity, Body>();
  private readonly parts = new Map<number, Part>();
  /** Collider handle pairs the engine reports in contact or intersecting, keyed `h1|h2`. */
  private readonly candidates = new Map<string, [number, number]>();
  private current = new Map<string, CollisionPair>();
  private readonly solids = new Solids();
  private backlog = 0;
  /** The length of the frame being played – the time a move of the graph's is spread over. */
  private frameDt = PHYSICS_STEP;
  private readonly hinted = new Set<string>();
  private world: World | null = null;

  constructor(private readonly rapier: Rapier) {}

  start(world: World): void {
    const engine = new this.rapier.World(this.gravity(world));
    engine.timestep = PHYSICS_STEP;
    this.engine = engine;
    this.queue = new this.rapier.EventQueue(true);
    this.world = world;
  }

  stop(): void {
    this.engine?.free();
    this.queue?.free();
    this.engine = null;
    this.world = null;
    this.queue = null;
    this.bodies.clear();
    this.parts.clear();
    this.candidates.clear();
    this.current = new Map();
    this.enters = [];
    this.stays = [];
    this.exits = [];
    this.backlog = 0;
    this.frameDt = PHYSICS_STEP;
    this.hinted.clear();
  }

  update(world: World, dt: number): void {
    const engine = this.engine;
    if (!engine) return;
    this.solids.reset();
    this.frameDt = dt > 0 ? dt : PHYSICS_STEP;
    this.enters = [];
    this.exits = [];
    this.sync(world);
    engine.gravity = this.gravity(world);
    this.backlog += dt;
    let steps = 0;
    const entered = new Set<string>();
    while (this.backlog >= PHYSICS_STEP - 1e-9 && steps < MAX_STEPS_PER_TICK) {
      for (const body of this.bodies.values()) if (body.body.isDynamic()) body.previous = body.current;
      engine.step(this.queue!);
      for (const body of this.bodies.values()) if (body.body.isDynamic()) body.current = poseOf(body.body);
      this.collect(world, entered);
      this.backlog -= PHYSICS_STEP;
      steps++;
    }
    this.backlog = steps === MAX_STEPS_PER_TICK ? 0 : Math.max(0, this.backlog);
    this.writeBack(this.backlog / PHYSICS_STEP);
    this.stays = [...this.current].filter(([key]) => !entered.has(key)).map(([, pair]) => pair);
  }

  /** Whether any collider of `a` is in contact with any collider of `b` as of the last step. */
  touching(world: World, a: Entity, b: Entity): boolean {
    for (const ca of partsOf(world, a))
      for (const cb of partsOf(world, b))
        if (this.current.has(`${ca.id}|${cb.id}`) || this.current.has(`${cb.id}|${ca.id}`)) return true;
    return false;
  }

  /**
   * `Move` with "Collide with solids": the object stops at what is in the way and slides along it,
   * as in the editor run, and a dynamic body that stops it gets a push (#147).
   */
  moveAndSlide(world: World, entity: Entity, dx: number, dy: number): MoveResult {
    return this.solids.moveAndSlide(world, entity, dx, dy, (blocked) => this.push(world, entity, blocked));
  }

  /** A dynamic body's velocity in world units per second; null for an object the engine does not move. */
  velocityOf(target: Entity): Vec2 | null {
    const body = this.dynamicBodyOf(target)?.body;
    if (!body) return null;
    const v = body.linvel();
    return { x: toPixels(v.x), y: toPixels(v.y) };
  }

  /** Sets a dynamic body's velocity (world units per second); false when the object has none. */
  setVelocity(target: Entity, velocity: Vec2): boolean {
    const body = this.dynamicBodyOf(target)?.body;
    body?.setLinvel({ x: toMetres(velocity.x), y: toMetres(velocity.y) }, true);
    return Boolean(body);
  }

  /** Degrees per second, counted the way the object's rotation is; null as for `velocityOf`. */
  angularVelocityOf(target: Entity): number | null {
    const body = this.dynamicBodyOf(target);
    if (!body) return null;
    return body.turns ? (this.turnSign(target) * body.body.angvel() * 180) / Math.PI : 0;
  }

  setAngularVelocity(target: Entity, degrees: number): boolean {
    const body = this.dynamicBodyOf(target);
    // With fixed rotation the engine would keep the speed and never turn: nothing to set.
    if (body?.turns) body.body.setAngvel((this.turnSign(target) * degrees * Math.PI) / 180, true);
    return Boolean(body);
  }

  /** A kick (world units × kg per second): the velocity changes by impulse ÷ mass, at once. */
  applyImpulse(target: Entity, impulse: Vec2): boolean {
    const body = this.dynamicBodyOf(target)?.body;
    body?.applyImpulse({ x: toMetres(impulse.x), y: toMetres(impulse.y) }, true);
    return Boolean(body);
  }

  /** The scene's gravity for the rest of this run, in world units per second squared. */
  setGravity(world: World, gravity: Vec2): void {
    world.setSceneProp('gravityX', gravity.x);
    world.setSceneProp('gravityY', gravity.y);
    if (this.engine) this.engine.gravity = this.gravity(world);
  }

  /**
   * Says once per run and object what the physics did not do, in the log and the console – the
   * one place a quiet no-op gets noticed.
   */
  hint(world: World, target: Entity, text: string): void {
    const key = `${target.id} ${text}`;
    if (this.hinted.has(key)) return;
    this.hinted.add(key);
    const line = `Physics: "${target.name}" ${text}`;
    world.log.push(line);
    console.warn(`[physics] ${line}`);
  }

  /** The engine body of the object a graph names – the object with the rigidbody, or the rigidbody. */
  private dynamicBodyOf(target: Entity): Body | null {
    const owner = target.type === 'rigidbody' ? target.parent : target;
    if (!owner) return null;
    // On start, and an object spawned this frame, come before the engine has its body: it gets it now.
    const world = this.world;
    if (world && !this.bodies.has(owner))
      if (world.childrenOf(owner).some((e) => e.type === 'rigidbody' && isHandleEnabled(e))) this.sync(world);
    const body = this.bodies.get(owner);
    return body?.kind === 'rigid' && body.body.isDynamic() && body.body.isEnabled() ? body : null;
  }

  /** Under a mirrored parent the engine turns against the rotation property. */
  private turnSign(target: Entity): number {
    const owner = target.type === 'rigidbody' ? target.parent : target;
    return owner?.parent && determinant(worldMatrixOf(owner.parent)) < 0 ? -1 : 1;
  }

  /**
   * A mover that runs into a dynamic body pushes it into the contact, as one body meeting another
   * without bounce: the wanted speed is the move spread over the frame, and a light mover barely
   * shifts a heavy crate. Both keep moving – the mover as the graph says, the crate as the engine does.
   */
  private push(world: World, mover: Entity, { by, along, distance }: Blocked): void {
    const part = [...this.parts.values()].find((p) => p.entity === by);
    const body = part?.owner.body;
    if (!body || part.owner.kind !== 'rigid' || !body.isDynamic() || !body.isEnabled()) return;
    const v = body.linvel();
    const closing = toMetres(distance / this.frameDt) - (v.x * along.x + v.y * along.y);
    const m = this.massOf(world, mover);
    const M = body.mass();
    if (closing <= 0 || m <= 0 || M <= 0) return;
    const impulse = ((m * M) / (m + M)) * closing;
    body.applyImpulse({ x: impulse * along.x, y: impulse * along.y }, true);
  }

  /**
   * What a moved object weighs against what it pushes: a dynamic body what the engine says, else
   * the mass of its rigidbody, else what its colliders weigh by their density.
   */
  private massOf(world: World, entity: Entity): number {
    const own = this.bodies.get(entity);
    if (own?.kind === 'rigid' && own.body.isDynamic()) return own.body.mass();
    const rigidbody = world.childrenOf(entity).find((e) => e.type === 'rigidbody' && isHandleEnabled(e));
    const mass = rigidbody ? rigidbodySettings(rigidbody.props).mass : 0;
    if (mass > 0) return mass;
    return blockingCollidersOf(world, entity).reduce(
      (sum, c) => sum + colliderMaterial(c.props).density * areaOf(shapeOf(c)) * toMetres(1) ** 2,
      0,
    );
  }

  private gravity(world: World): Vec2 {
    const g = sceneGravity(world.sceneProps);
    return { x: toMetres(g.x), y: toMetres(g.y) };
  }

  /** Makes the engine's bodies match the entities: new ones in, gone ones out, moved ones moved. */
  private sync(world: World): void {
    const wanted = this.wantedBodies(world);
    for (const [key, body] of this.bodies) {
      const want = wanted.get(key);
      if (!want || want.kind !== body.kind) this.remove(key, body);
    }
    for (const [key, want] of wanted) {
      let body = this.bodies.get(key);
      if (!body) {
        body = this.create(want, key);
        this.bodies.set(key, body);
      }
      this.place(body, want, key, world);
      this.build(body, want, key, world);
    }
  }

  /**
   * Every body the entities ask for, keyed by the entity that stands for it. A collider belongs to
   * the nearest object above it with a rigidbody switched on; without one it is a body of its own.
   */
  private wantedBodies(world: World): Map<Entity, Wanted> {
    const owners = new Map<Entity, Entity>();
    for (const e of world.entities)
      if (
        e.type === 'rigidbody' &&
        e.parent &&
        e.parent.type !== 'tilemap' &&
        !owners.has(e.parent) &&
        isHandleEnabled(e)
      )
        owners.set(e.parent, e);
    const wanted = new Map<Entity, Wanted>();
    for (const [owner, rigidbody] of owners)
      wanted.set(owner, { kind: 'rigid', settings: rigidbodySettings(rigidbody.props), colliders: [] });
    for (const e of world.entities) {
      if (e.type === 'tilemap') wanted.set(e, { kind: 'tilemap', settings: null, colliders: [] });
      if (e.type !== 'collider') continue;
      const owner = [...parentChain(e, (x) => x.parent)].slice(1).find((x) => owners.has(x));
      if (owner) wanted.get(owner)!.colliders.push(e);
      else wanted.set(e, { kind: 'free', settings: null, colliders: [e] });
    }
    return wanted;
  }

  private create(want: Wanted, key: Entity): Body {
    const R = this.rapier;
    const pose = this.poseWanted(want, key);
    const desc =
      want.kind === 'tilemap'
        ? R.RigidBodyDesc.fixed()
        : want.kind === 'free'
          ? R.RigidBodyDesc.kinematicPositionBased()
          : want.settings!.type === 'dynamic'
            ? R.RigidBodyDesc.dynamic()
            : want.settings!.type === 'static'
              ? R.RigidBodyDesc.fixed()
              : R.RigidBodyDesc.kinematicPositionBased();
    desc.setTranslation(toMetres(pose.x), toMetres(pose.y)).setRotation(pose.angle);
    return {
      kind: want.kind,
      body: this.engine!.createRigidBody(desc),
      colliders: [],
      shapeKey: '',
      settingsKey: '',
      turns: false,
      ownMass: 0,
      placed: pose,
      previous: pose,
      current: pose,
    };
  }

  private remove(key: Entity, body: Body): void {
    for (const collider of body.colliders) this.parts.delete(collider.handle);
    this.engine!.removeRigidBody(body.body);
    this.bodies.delete(key);
  }

  /** Where the entities say the body is: the object's origin, a free collider's shape, a map at zero. */
  private poseWanted(want: Wanted, key: Entity): Pose {
    if (want.kind === 'tilemap') return { x: 0, y: 0, angle: 0 };
    if (want.kind === 'free') {
      const shape = shapeOf(key);
      return { x: shape.x, y: shape.y, angle: shape.kind === 'box' ? shape.angle : 0 };
    }
    return matrixPose(worldMatrixOf(key));
  }

  /**
   * Puts the body where its object is. A dynamic body is only moved when something else moved the
   * object since we last wrote it – that is a teleport, and the body keeps its velocity (#146, 3).
   */
  private place(body: Body, want: Wanted, key: Entity, world: World): void {
    const settings = want.settings;
    const settingsKey = settings ? JSON.stringify(settings) + hasTransformProps(key.type) : '';
    if (settings && settingsKey !== body.settingsKey) {
      body.settingsKey = settingsKey;
      this.applySettings(body, settings, key);
    }
    const on = isHandleEnabled(key);
    if (body.body.isEnabled() !== on) body.body.setEnabled(on);
    const pose = this.poseWanted(want, key);
    if (samePose(pose, body.placed)) return;
    body.placed = pose;
    const t = { x: toMetres(pose.x), y: toMetres(pose.y) };
    if (body.body.isKinematic() && want.kind === 'rigid') {
      body.body.setNextKinematicTranslation(t);
      body.body.setNextKinematicRotation(pose.angle);
      return;
    }
    if (body.body.isDynamic())
      this.hint(
        world,
        key,
        'has a dynamic rigidbody and was moved directly; it keeps its velocity. ' +
          'Apply force, Apply impulse and Set velocity move it with the physics.',
      );
    body.body.setTranslation(t, true);
    body.body.setRotation(pose.angle, true);
    body.previous = pose;
    body.current = pose;
  }

  private applySettings(target: Body, settings: RigidbodySettings, owner: Entity): void {
    const body = target.body;
    body.setBodyType(this.bodyType(settings.type), true);
    body.setGravityScale(settings.gravityScale, true);
    body.setLinearDamping(settings.linearDamping);
    body.setAngularDamping(settings.angularDamping);
    // An object without a rotation of its own could not show the body turning, so it does not turn.
    target.turns = !settings.fixedRotation && hasTransformProps(owner.type);
    body.lockRotations(!target.turns, true);
    body.enableCcd(settings.ccd);
  }

  private bodyType(type: BodyType) {
    const T = this.rapier.RigidBodyType;
    return type === 'dynamic' ? T.Dynamic : type === 'static' ? T.Fixed : T.KinematicPositionBased;
  }

  /** The body's colliders, built again whenever what they were built from changed. */
  private build(body: Body, want: Wanted, key: Entity, world: World): void {
    const shapes = this.shapesWanted(want, key, world);
    const origin = body.placed;
    const local = shapes.map(({ entity, shape }) => ({ entity, shape, at: relativeTo(origin, shape) }));
    const shapeKey =
      JSON.stringify(
        local.map(({ entity, shape, at }) => [
          entity.id,
          shape.kind === 'box' ? [round(shape.w), round(shape.h)] : round(shape.r),
          round(at.x),
          round(at.y),
          round(at.angle),
          entity.type === 'collider' ? [isTrigger(entity.props), colliderMaterial(entity.props)] : 0,
        ]),
      ) + JSON.stringify(want.settings?.mass ?? 0);
    if (shapeKey !== body.shapeKey) {
      body.shapeKey = shapeKey;
      for (const collider of body.colliders) {
        this.parts.delete(collider.handle);
        this.engine!.removeCollider(collider, true);
      }
      body.colliders = [];
      const mass = want.settings?.mass ?? 0;
      const area = local.reduce((sum, { entity, shape }) => sum + (isSolid(entity) ? areaOf(shape) : 0), 0);
      for (const { entity, shape, at } of local) {
        if (areaOf(shape) <= 0) continue;
        const desc = this.colliderDesc(
          entity,
          shape,
          at,
          mass > 0 && area > 0 ? (mass * areaOf(shape)) / area : 0,
        );
        const collider = this.engine!.createCollider(desc, body.body);
        body.colliders.push(collider);
        this.parts.set(collider.handle, { entity, shape, owner: body });
      }
    }
    // Shapes are refreshed every tick even when the colliders stay: the overlap test reads them.
    const built = local.filter(({ shape }) => areaOf(shape) > 0);
    body.colliders.forEach((collider, i) => {
      const { entity, shape } = built[i]!;
      this.parts.set(collider.handle, { entity, shape, owner: body });
      const on = isHandleEnabled(entity);
      if (collider.isEnabled() === on) return;
      collider.setEnabled(on);
      // A resting body sleeps, and losing what held it up does not wake it by itself.
      body.body.wakeUp();
    });
    if (body.kind !== 'rigid') return;
    // Without a collider the engine would weigh it nothing, and a thing of no weight never falls.
    const weighed = built.some(({ entity }) => isSolid(entity) && isHandleEnabled(entity));
    const own = weighed ? 0 : want.settings?.mass || 1;
    if (own !== body.ownMass) {
      body.ownMass = own;
      body.body.setAdditionalMass(own, true);
    }
  }

  private shapesWanted(want: Wanted, key: Entity, world: World): { entity: Entity; shape: ColliderShape }[] {
    if (want.kind === 'tilemap')
      return this.solids.shapesOf(world, key).map((shape) => ({ entity: key, shape }));
    return want.colliders.map((entity) => ({ entity, shape: shapeOf(entity) }));
  }

  private colliderDesc(entity: Entity, shape: ColliderShape, at: Pose, mass: number): ColliderDesc {
    const R = this.rapier;
    const desc =
      shape.kind === 'circle'
        ? R.ColliderDesc.ball(toMetres(shape.r))
        : R.ColliderDesc.cuboid(toMetres(shape.w / 2), toMetres(shape.h / 2));
    desc.setTranslation(toMetres(at.x), toMetres(at.y)).setRotation(at.angle);
    // A tile has no material of its own; it gets what a new collider starts with.
    const material = colliderMaterial(entity.type === 'collider' ? entity.props : {});
    desc.setFriction(material.friction).setRestitution(material.bounciness);
    // The livelier side wins, as in Box2D: a ball that should bounce does so on plain ground too.
    desc.setRestitutionCombineRule(R.CoefficientCombineRule.Max);
    if (!isSolid(entity)) desc.setSensor(true).setDensity(0);
    else if (mass > 0) desc.setMass(mass);
    else desc.setDensity(material.density);
    desc.setActiveEvents(R.ActiveEvents.COLLISION_EVENTS);
    desc.setActiveCollisionTypes(R.ActiveCollisionTypes.ALL);
    return desc;
  }

  /**
   * The pairs in contact after a step, compared with those before it. The engine proposes the
   * pairs; where no dynamic body takes part, the overlap test decides as it always did.
   */
  private collect(world: World, entered: Set<string>): void {
    this.queue!.drainCollisionEvents((h1, h2, started) => {
      const key = h1 < h2 ? `${h1}|${h2}` : `${h2}|${h1}`;
      if (started) this.candidates.set(key, [h1, h2]);
      else this.candidates.delete(key);
    });
    const order = new Map(world.entities.map((e, i) => [e, i]));
    const now = new Map<string, CollisionPair>();
    const first = new Map<string, [Collider, Collider]>();
    for (const [key, [h1, h2]] of this.candidates) {
      let p = this.parts.get(h1);
      let q = this.parts.get(h2);
      if (!p || !q) {
        this.candidates.delete(key);
        continue;
      }
      const a0 = p.entity;
      const b0 = q.entity;
      if (!order.has(a0) || !order.has(b0) || !isHandleEnabled(a0) || !isHandleEnabled(b0)) continue;
      if (bodyOf(a0) === bodyOf(b0) || (a0.type === 'tilemap' && b0.type === 'tilemap')) continue;
      const engineSays = p.owner.body.isDynamic() || q.owner.body.isDynamic();
      if (!engineSays && !shapesOverlap(p.shape, q.shape)) continue;
      let c1 = h1;
      let c2 = h2;
      if (order.get(a0)! > order.get(b0)!) {
        [p, q] = [q, p];
        [c1, c2] = [c2, c1];
      }
      const pairKey = `${p.entity.id}|${q.entity.id}`;
      if (now.has(pairKey)) continue;
      now.set(pairKey, { a: p.entity, b: q.entity });
      first.set(pairKey, [this.engine!.getCollider(c1), this.engine!.getCollider(c2)]);
    }
    for (const [key, pair] of now) {
      if (this.current.has(key)) {
        now.set(key, this.current.get(key)!);
        continue;
      }
      const [c1, c2] = first.get(key)!;
      this.enters.push(Object.assign(pair, this.contactOf(c1, c2)));
      entered.add(key);
    }
    for (const [key, pair] of this.current) if (!now.has(key)) this.exits.push(pair);
    this.current = now;
  }

  /** How the pair met: the normal from the second collider towards the first, and the impulse. */
  private contactOf(c1: Collider, c2: Collider): { normal: Vec2; impulse: number } {
    let normal = { x: 0, y: 0 };
    let impulse = 0;
    this.engine!.contactPair(c1, c2, (manifold, flipped) => {
      if (manifold.numContacts() === 0) return;
      const n = manifold.normal();
      // The manifold's normal runs from its first collider to its second; ours runs back to `a`.
      const sign = flipped ? 1 : -1;
      if (normal.x === 0 && normal.y === 0) normal = { x: sign * n.x, y: sign * n.y };
      for (let i = 0; i < manifold.numContacts(); i++) impulse += manifold.contactImpulse(i);
    });
    return { normal, impulse: toPixels(impulse) };
  }

  /**
   * Hands the dynamic bodies' poses to their objects, a parent before its children, drawn `alpha`
   * of the way from the step before to the last one so the picture moves evenly between steps.
   */
  private writeBack(alpha: number): void {
    const dynamic = [...this.bodies].filter(([, body]) => body.kind === 'rigid' && body.body.isDynamic());
    dynamic.sort(([a], [b]) => depthOf(a) - depthOf(b));
    for (const [owner, body] of dynamic) {
      const pose = blend(body.previous, body.current, alpha);
      const parent = owner.parent ? worldMatrixOf(owner.parent) : IDENTITY;
      const local = apply(invert(parent), pose);
      owner.x = local.x;
      owner.y = local.y;
      if (body.turns) {
        const turned = angleBetween(matrixPose(worldMatrixOf(owner)).angle, pose.angle);
        const sign = determinant(parent) < 0 ? -1 : 1;
        owner.props.rotation = Number(owner.props.rotation ?? 0) + (sign * turned * 180) / Math.PI;
      }
      body.placed = matrixPose(worldMatrixOf(owner));
    }
  }
}

interface Wanted {
  kind: Body['kind'];
  settings: RigidbodySettings | null;
  colliders: Entity[];
}

function poseOf(body: RigidBody): Pose {
  const t = body.translation();
  return { x: toPixels(t.x), y: toPixels(t.y), angle: body.rotation() };
}

function matrixPose(m: Mat): Pose {
  return { x: m.tx, y: m.ty, angle: Math.atan2(m.b, m.a) };
}

function samePose(a: Pose, b: Pose): boolean {
  return (
    Math.abs(a.x - b.x) < POSE_EPSILON &&
    Math.abs(a.y - b.y) < POSE_EPSILON &&
    Math.abs(angleBetween(a.angle, b.angle)) < POSE_EPSILON
  );
}

/** The turn from `from` to `to`, the short way round. */
function angleBetween(from: number, to: number): number {
  const d = (to - from) % (2 * Math.PI);
  return d > Math.PI ? d - 2 * Math.PI : d < -Math.PI ? d + 2 * Math.PI : d;
}

function blend(a: Pose, b: Pose, t: number): Pose {
  return {
    x: a.x + (b.x - a.x) * t,
    y: a.y + (b.y - a.y) * t,
    angle: a.angle + angleBetween(a.angle, b.angle) * t,
  };
}

/** A world shape as seen from a body at `origin`: offset and turn in the body's own frame. */
function relativeTo(origin: Pose, shape: ColliderShape): Pose {
  const dx = shape.x - origin.x;
  const dy = shape.y - origin.y;
  const cos = Math.cos(origin.angle);
  const sin = Math.sin(origin.angle);
  return {
    x: dx * cos + dy * sin,
    y: -dx * sin + dy * cos,
    angle: (shape.kind === 'box' ? shape.angle : 0) - origin.angle,
  };
}

function areaOf(shape: ColliderShape): number {
  return shape.kind === 'circle' ? Math.PI * shape.r * shape.r : shape.w * shape.h;
}

/** A tilemap's cells always block; a collider unless it is a trigger. */
function isSolid(entity: Entity): boolean {
  return entity.type !== 'collider' || !isTrigger(entity.props);
}

function depthOf(entity: Entity): number {
  return [...parentChain(entity, (e) => e.parent)].length;
}

function round(n: number): number {
  return Math.round(n * 1e4) / 1e4;
}