trainsim/track/system.ts

import { Doodler, Point, Vector } from "@bearmetal/doodler";
import { ComplexPath, PathSegment } from "../math/path.ts";
import { getContextItem, setDefaultContext } from "../lib/context.ts";
import { clamp } from "../math/clamp.ts";

export class TrackSystem {
  private segments: Map<string, TrackSegment> = new Map();
  private doodler: Doodler;

  constructor(segments: TrackSegment[]) {
    this.doodler = getContextItem<Doodler>("doodler");
    for (const segment of segments) {
      this.segments.set(segment.id, segment);
    }
  }

  get firstSegment() {
    return this.segments.values().next().value;
  }

  get lastSegment() {
    return this.segments.values().toArray().pop();
  }

  optimize(percent: number) {
    console.log("Optimizing track", percent * 100 / 4);
    for (const segment of this.segments.values()) {
      segment.recalculateRailPoints(Math.round(percent * 100 / 4));
    }
  }

  recalculateAll() {
    for (const segment of this.segments.values()) {
      segment.recalculateRailPoints();
      segment.length = segment.calculateApproxLength();
    }
  }

  registerSegment(segment: TrackSegment) {
    segment.setTrack(this);
    this.segments.set(segment.id, segment);
  }
  unregisterSegment(segment: TrackSegment) {
    this.segments.delete(segment.id);
    for (const s of this.segments.values()) {
      s.backNeighbours = s.backNeighbours.filter((n) => n !== segment);
      s.frontNeighbours = s.frontNeighbours.filter((n) => n !== segment);
    }
    const ends = this.ends.get(segment);
    this.ends.delete(segment);
    this.endArray = this.endArray.filter((e) => !ends?.includes(e));
  }

  draw(showControls = false) {
    for (const [i, segment] of this.segments.entries()) {
      segment.draw(showControls);
    }

    // try {
    //   if (getContextItem<boolean>("showEnds")) {
    //     const ends = this.findEnds();
    //     for (const end of ends) {
    //       this.doodler.fillCircle(end.pos, 2, {
    //         color: "red",
    //         // weight: 3,
    //       });
    //       if (getContextItem<boolean>("debug")) {
    //         this.doodler.line(
    //           end.pos,
    //           end.pos.copy().add(end.tangent.copy().mult(20)),
    //           {
    //             color: "blue",
    //             // weight: 3,
    //           },
    //         );
    //       }
    //     }
    //   }
    // } catch {
    //   setDefaultContext({ showEnds: false });
    // }
  }

  ends: Map<TrackSegment, [End, End]> = new Map();
  endArray: End[] = [];

  findEnds() {
    for (const segment of this.segments.values()) {
      if (this.ends.has(segment)) continue;
      const ends: [End, End] = [
        {
          pos: segment.points[0],
          segment,
          tangent: Vector.sub(segment.points[1], segment.points[0]).normalize(),
          frontOrBack: "back",
        },
        {
          pos: segment.points[3],
          segment,
          tangent: Vector.sub(segment.points[3], segment.points[2]).normalize(),
          frontOrBack: "front",
        },
      ];
      this.ends.set(segment, ends);
      this.endArray.push(...ends);
    }
    return this.endArray;
  }

  serialize() {
    return JSON.stringify(
      this.segments.values().map((s) => s.serialize()).toArray(),
    );
  }

  copy() {
    const track = new TrackSystem([]);
    for (const segment of this.segments.values()) {
      track.segments.set(segment.id, segment.copy());
    }
    return track;
  }

  static deserialize(data: SerializedTrackSegment[]) {
    if (data.length === 0) return undefined;
    const track = new TrackSystem([]);
    const neighborMap = new Map<string, [string[], string[]]>();

    for (const segment of data) {
      track.segments.set(segment.id, TrackSegment.deserialize(segment));
      neighborMap.set(segment.id, [segment.fNeighbors, segment.bNeighbors]);
    }

    for (const segment of track.segments.values()) {
      segment.setTrack(track);
      const neighbors = neighborMap.get(segment.id);
      if (neighbors) {
        segment.backNeighbours = neighbors[1].map((id) =>
          track.segments.get(id)
        ).filter((s) => s) as TrackSegment[];
        segment.frontNeighbours = neighbors[0].map((id) =>
          track.segments.get(id)
        ).filter((s) => s) as TrackSegment[];
      }
    }

    return track;
  }

  translate(v: Vector) {
    for (const segment of this.segments.values()) {
      segment.translate(v);
    }
  }

  private _path?: Spline<TrackSegment>;

  get path() {
    if (!this._path) {
      this._path = this.generatePath();
    }
    return this._path;
  }

  generatePath() {
    if (!this.firstSegment) throw new Error("No first segment");
    const flags = { looping: true };
    const rightOnlyPath = [
      this.firstSegment.copy(),
      ...this.findRightPath(
        this.firstSegment,
        new Set([this.firstSegment.id]),
        flags,
      ),
    ];

    rightOnlyPath.forEach((s, i, arr) => {
      if (i === 0) return;
      const prev = arr[i - 1];
      s.points[0] = prev.points[3];
      s.prev = prev;
      prev.next = s;
    });
    if (flags.looping) {
      const first = rightOnlyPath[0];
      const last = rightOnlyPath[rightOnlyPath.length - 1];
      first.points[0] = last.points[3];
      last.points[3] = first.points[0];
      first.prev = last;
      last.next = first;
    }

    return new Spline<TrackSegment>(rightOnlyPath);
  }

  *findRightPath(
    start: TrackSegment,
    seen: Set<string>,
    flags: { looping: boolean },
  ): Generator<TrackSegment> {
    if (start.frontNeighbours.length === 0) {
      return;
    }
    let rightMost = start.frontNeighbours[0];
    for (const segment of start.frontNeighbours) {
      if (segment.id === rightMost.id) continue;
      const rotatedSegment = segment.copy();
      rotatedSegment.rotateAboutPoint(
        rotatedSegment.tangent(0).heading(),
        rotatedSegment.points[0],
      );
      const rotatedRightMost = rightMost.copy();
      rotatedRightMost.rotateAboutPoint(
        rotatedRightMost.tangent(0).heading(),
        rotatedRightMost.points[0],
      );
      if (rotatedSegment.points[3].y > rotatedRightMost.points[3].y) {
        rightMost = segment;
      }
    }
    if (seen.has(rightMost.id)) {
      if (seen.values().next().value === rightMost.id) {
        flags.looping = true;
      }
      return;
    }
    seen.add(rightMost.id);
    yield rightMost.copy();
    yield* this.findRightPath(rightMost, seen, flags);
  }
  *findLeftPath(
    start: TrackSegment,
    seen: Set<string>,
    flags: { looping: boolean },
  ): Generator<TrackSegment> {
    if (start.frontNeighbours.length === 0) {
      return;
    }
    let leftMost = start.frontNeighbours[0];
    for (const segment of start.frontNeighbours) {
      if (segment.id === leftMost.id) continue;
      const rotatedSegment = segment.copy();
      rotatedSegment.rotateAboutPoint(
        rotatedSegment.tangent(0).heading(),
        rotatedSegment.points[0],
      );
      const rotatedLeftMost = leftMost.copy();
      rotatedLeftMost.rotateAboutPoint(
        rotatedLeftMost.tangent(0).heading(),
        rotatedLeftMost.points[0],
      );
      if (rotatedSegment.points[3].y < rotatedLeftMost.points[3].y) {
        leftMost = segment;
      }
    }
    if (seen.has(leftMost.id)) {
      if (seen.values().next().value === leftMost.id) {
        flags.looping = true;
      }
      return;
    }
    seen.add(leftMost.id);
    yield leftMost.copy();
    yield* this.findLeftPath(leftMost, seen, flags);
  }
}

type VectorSet = [Vector, Vector, Vector, Vector];

export class TrackSegment extends PathSegment {
  frontNeighbours: TrackSegment[] = [];
  backNeighbours: TrackSegment[] = [];

  track?: TrackSystem;

  doodler: Doodler;
  normalPoints: Vector[] = [];
  antiNormalPoints: Vector[] = [];

  constructor(p: VectorSet, id?: string) {
    super(p);
    this.doodler = getContextItem<Doodler>("doodler");
    this.id = id ?? crypto.randomUUID();
    this.recalculateRailPoints();
  }

  recalculateRailPoints(resolution = 100) {
    this.normalPoints = [];
    this.antiNormalPoints = [];
    for (let i = 0; i <= resolution; i++) {
      const t = i / resolution;
      const normal = this.tangent(t).rotate(Math.PI / 2);
      normal.setMag(6);
      const p = this.getPointAtT(t);
      this.normalPoints.push(p.copy().add(normal));
      this.antiNormalPoints.push(p.copy().add(normal.rotate(Math.PI)));
    }
  }

  setTrack(t: TrackSystem) {
    this.track = t;
  }

  override draw(showControls = false, recalculateRailPoints = false) {
    // if (showControls) {
    //   this.doodler.drawBezier(
    //     this.points[0],
    //     this.points[1],
    //     this.points[2],
    //     this.points[3],
    //     {
    //       strokeColor: "#ffffff50",
    //     },
    //   );
    // }
    if (showControls) {
      this.doodler.deferDrawing(() => {
        this.doodler.fillCircle(this.points[0], 1, {
          color: "red",
        });
        this.doodler.fillCircle(this.points[1], 1, {
          color: "red",
        });
        this.doodler.fillCircle(this.points[2], 1, {
          color: "red",
        });
        this.doodler.fillCircle(this.points[3], 1, {
          color: "red",
        });
      });
    }

    const spacing = Math.ceil(this.length / 10);
    const points = this.calculateEvenlySpacedPoints(this.length / spacing);
    for (let i = 0; i < points.length - 1; i++) {
      // const t = i / ties;
      // const p = this.getPointAtT(t);
      const [p, t] = points[i];
      // this.doodler.drawCircle(p, 2, {
      //   color: "red",
      //   weight: 3,
      // });
      this.doodler.drawRotated(p, t, () => {
        this.doodler.line(p, p.copy().add(0, 10), {
          color: "#291b17",
          weight: 4,
        });
        this.doodler.line(p, p.copy().add(0, -10), {
          color: "#291b17",
          weight: 4,
        });
        // this.doodler.line(p.copy().add(-6, 5), p.copy().add(6, 5), {
        //   color: "grey",
        //   weight: 1,
        // });
        // this.doodler.line(p.copy().add(-6, -5), p.copy().add(6, -5), {
        //   color: "grey",
        //   weight: 1,
        // });
      });
    }

    if (recalculateRailPoints) {
      this.recalculateRailPoints();
    }
    this.doodler.deferDrawing(
      () => {
        this.doodler.drawLine(this.normalPoints, {
          color: "grey",
          weight: 1.5,
        });
        this.doodler.drawLine(this.antiNormalPoints, {
          color: "grey",
          weight: 1.5,
        });
      },
    );
    // this.doodler.drawCircle(p, 2, {
    //   color: "red",
    //   weight: 3,
    // });
  }

  serialize(): SerializedTrackSegment {
    return {
      p: this.points.map((p) => p.array()),
      id: this.id,
      bNeighbors: this.backNeighbours.map((n) => n.id),
      fNeighbors: this.frontNeighbours.map((n) => n.id),
    };
  }

  copy() {
    return new TrackSegment(
      this.points.map((p) => p.copy()) as VectorSet,
      this.id,
    );
  }

  cleanCopy() {
    return new TrackSegment(
      this.points.map((p) => p.copy()) as VectorSet,
    );
  }

  propagateTranslation(v: Vector) {
    for (const fNeighbour of this.frontNeighbours) {
      fNeighbour.receivePropagation(v);
    }
    for (const bNeighbour of this.backNeighbours) {
      bNeighbour.receivePropagation(v);
    }
  }

  lastHeading?: number;

  receivePropagation(v: Vector) {
    this.translate(v);
    this.propagateTranslation(v);
  }

  // TODO: this duplicates receivePropagation, but for some reason it doesn't work when called from receivePropagation
  rotate(angle: number | Vector) {
    const [p1, p2, p3, p4] = this.points;
    let newP2;
    if (angle instanceof Vector) {
      const tan = angle;
      angle = tan.heading() - (this.lastHeading ?? 0);
      this.lastHeading = tan.heading();
      newP2 = Vector.add(p1, tan);
    } else {
      const p1ToP2 = Vector.sub(p2, p1);
      p1ToP2.rotate(angle);
      newP2 = Vector.add(p1, p1ToP2);
    }
    const p2ToP3 = Vector.sub(p3, p2);
    p2ToP3.rotate(angle);
    p3.set(Vector.add(newP2, p2ToP3));
    const p2Top4 = Vector.sub(p4, p2);
    p2Top4.rotate(angle);
    p4.set(Vector.add(newP2, p2Top4));
    p2.set(newP2);
  }
  static deserialize(data: any) {
    return new TrackSegment(
      data.p.map((p: [number, number, number]) => new Vector(p[0], p[1], p[2])),
      data.id,
    );
  }

  setPositionByPoint(pos: Vector, point: Vector) {
    if (!this.points.includes(point)) return;
    point = point.copy();
    this.points.forEach((p, i) => {
      const relativePoint = Vector.sub(p, point);
      p.set(pos);
      p.add(relativePoint);
    });
  }

  rotateAboutPoint(angle: number, point: Vector) {
    if (!this.points.includes(point)) return;
    point = point.copy();
    this.points.forEach((p, i) => {
      const relativePoint = Vector.sub(p, point);
      relativePoint.rotate(angle);
      p.set(Vector.add(point, relativePoint));
    });
  }

  // resetRotation() {
  //   const angle = this.tangent(0).heading();
  //   this.rotateAboutPoint(-angle, this.points[0]);
  // }

  translate(v: Point) {
    this.points.forEach((p) => {
      p.add(v.x, v.y);
    });
  }
}

export class Spline<T extends PathSegment = PathSegment> {
  segments: T[] = [];
  ctx?: CanvasRenderingContext2D;

  evenPoints: Vector[];
  pointSpacing: number;

  get points() {
    return Array.from(new Set(this.segments.flatMap((s) => s.points)));
  }

  nodes: IControlNode[];

  looped = false;

  constructor(segs: T[]) {
    this.segments = segs;
    if (this.segments.at(-1)?.next === this.segments[0]) {
      this.looped = true;
    }
    this.pointSpacing = 1;
    this.evenPoints = this.calculateEvenlySpacedPoints(1);
    this.nodes = [];
    // for (let i = 0; i < this.points.length; i += 3) {
    //   const node: IControlNode = {
    //     anchor: this.points[i],
    //     controls: [
    //       this.points.at(i - 1)!,
    //       this.points[(i + 1) % this.points.length],
    //     ],
    //     mirrored: false,
    //     tangent: true,
    //   };
    //   this.nodes.push(node);
    // }
  }

  // setContext(ctx: CanvasRenderingContext2D) {
  //   this.ctx = ctx;
  //   for (const segment of this.segments) {
  //     segment.setContext(ctx);
  //   }
  // }

  draw() {
    for (const segment of this.segments) {
      // segment.draw();
      const doodler = getContextItem<Doodler>("doodler");
      doodler.drawWithAlpha(0.5, () => {
        doodler.drawBezier(...segment.points, { color: "red" });
      });
    }
  }

  calculateEvenlySpacedPoints(spacing: number, resolution = 1) {
    this.pointSpacing = 1;
    // return this.segments.flatMap(s => s.calculateEvenlySpacedPoints(spacing, resolution));
    const points: Vector[] = [];

    points.push(this.segments[0].points[0]);
    let prev = points[0];
    let distSinceLastEvenPoint = 0;
    for (const seg of this.segments) {
      let t = 0;

      const div = Math.ceil(seg.length * resolution * 10);
      while (t < 1) {
        t += 1 / div;
        const point = seg.getPointAtT(t);
        distSinceLastEvenPoint += prev.dist(point);

        if (distSinceLastEvenPoint >= spacing) {
          const overshoot = distSinceLastEvenPoint - spacing;
          const evenPoint = Vector.add(
            point,
            Vector.sub(point, prev).normalize().mult(overshoot),
          );
          distSinceLastEvenPoint = overshoot;
          points.push(evenPoint);
          prev = evenPoint;
        }

        prev = point;
      }
    }

    this.evenPoints = points;

    return points;
  }

  followEvenPoints(t: number) {
    if (this.looped) {
      if (t < 0) t += this.evenPoints.length;
      const i = Math.floor(t) % this.evenPoints.length;
      const a = this.evenPoints[i];
      const b = this.evenPoints[(i + 1) % this.evenPoints.length];
      return Vector.lerp(a, b, t % 1);
    }
    t = clamp(t, 0, this.evenPoints.length - 1);
    const i = clamp(Math.floor(t), 0, this.evenPoints.length - 1);
    const a = this.evenPoints[clamp(i, 0, this.evenPoints.length - 1)];
    const b = this
      .evenPoints[
        clamp((i + 1) % this.evenPoints.length, 0, this.evenPoints.length - 1)
      ];
    return Vector.lerp(a, b, t % 1);
  }

  calculateApproxLength() {
    for (const s of this.segments) {
      s.calculateApproxLength();
    }
  }

  toggleNodeTangent(p: Vector) {
    const node = this.nodes.find((n) => n.anchor === p);

    node && (node.tangent = !node.tangent);
  }
  toggleNodeMirrored(p: Vector) {
    const node = this.nodes.find((n) => n.anchor === p);

    node && (node.mirrored = !node.mirrored);
  }
  handleNodeEdit(p: Vector, movement: { x: number; y: number }) {
    const node = this.nodes.find((n) =>
      n.anchor === p || n.controls.includes(p)
    );
    if (!node || !(node.mirrored || node.tangent)) return;

    if (node.anchor !== p) {
      if (node.mirrored || node.tangent) {
        const mover = node.controls.find((e) => e !== p)!;
        const v = Vector.sub(node.anchor, p);
        if (!node.mirrored) v.setMag(Vector.sub(node.anchor, mover).mag());
        mover.set(Vector.add(v, node.anchor));
      }
    } else {
      for (const control of node.controls) {
        control.add(movement.x, movement.y);
      }
    }
  }
}

export interface IControlNode {
  anchor: Vector;
  controls: [Vector, Vector];
  tangent: boolean;
  mirrored: boolean;
}