import { Doodler, Vector } from "@bearmetal/doodler";
import { getContextItem } from "../lib/context.ts";
import { Spline, TrackSegment } from "../track/system.ts";

export class DotFollower {
  position: Vector;
  velocity: Vector;
  acceleration: Vector;
  maxSpeed: number;
  maxForce: number;
  _trailingPoint: number;
  protected _leadingPoint: number;

  path: Spline<TrackSegment>;

  get trailingPoint() {
    const desired = this.velocity.copy();
    desired.normalize();
    desired.mult(-this._trailingPoint);

    return Vector.add(this.position, desired);
  }

  constructor(path: Spline<TrackSegment>, pos: Vector) {
    this.path = path;
    this.position = pos;
    this.velocity = new Vector();
    this.acceleration = new Vector();
    this.maxSpeed = 3;
    this.maxForce = 0.3;

    this._trailingPoint = 0;
    this._leadingPoint = 0;

    this.init();
  }

  init() {
  }

  move(dt: number) {
    dt *= 10;
    const force = calculatePathForce(this, this.path.points);
    this.applyForce(force.mult(dt));
    this.velocity.limit(this.maxSpeed);
    this.acceleration.limit(this.maxForce);
    this.velocity.add(this.acceleration.copy().mult(dt));
    this.position.add(this.velocity.copy().mult(dt));
    this.edges();
  }

  edges() {
    const doodler = getContextItem<Doodler>("doodler");

    if (this.position.x > doodler.width) this.position.x = 0;
    if (this.position.y > doodler.height) this.position.y = 0;
    if (this.position.x < 0) this.position.x = doodler.width;
    if (this.position.y < 0) this.position.y = doodler.height;
  }

  draw() {
    const doodler = getContextItem<Doodler>("doodler");
    doodler.drawRotated(this.position, this.velocity.heading() || 0, () => {
      doodler.fillCenteredRect(this.position, 20, 20, { fillColor: "white" });
    });
    for (const point of this.path.points) {
      doodler.drawCircle(point, 4, { color: "red", weight: 3 });
    }
  }

  applyForce(force: Vector) {
    this.velocity.add(force);
  }

  static edges(point: Vector, width: number, height: number) {
    if (point.x > width) point.x = 0;
    if (point.y > height) point.y = 0;
    if (point.x < 0) point.x = width;
    if (point.y < 0) point.y = height;
  }
}

function closestPointOnLineSegment(p: Vector, a: Vector, b: Vector): Vector {
  // Vector AB
  // const AB = { x: b.x - a.x, y: b.y - a.y };
  const AB = Vector.sub(b, a);
  // Vector AP
  // const AP = { x: p.x - a.x, y: p.y - a.y };
  const AP = Vector.sub(p, a);
  // Dot product of AP and AB
  // const AB_AB = AB.x * AB.x + AB.y * AB.y;
  const AB_AB = Vector.dot(AB, AB);
  // const AP_AB = AP.x * AB.x + AP.y * AB.y;
  const AP_AB = Vector.dot(AP, AB);
  // Project AP onto AB
  const t = AP_AB / AB_AB;

  // Clamp t to the range [0, 1] to restrict to the segment
  const tClamped = Math.max(0, Math.min(1, t));

  // Closest point on the segment
  return new Vector({ x: a.x + AB.x * tClamped, y: a.y + AB.y * tClamped });
}

function calculatePathForce(f: DotFollower, path: Vector[]) {
  let closestPoint: Vector = path[0];
  let minDistance = Infinity;

  // Loop through each segment to find the closest point on the path
  for (let i = 0; i < path.length - 1; i++) {
    const segmentStart = path[i];
    const segmentEnd = path[i + 1];

    // Find the closest point on the segment
    const closest = closestPointOnLineSegment(
      f.position,
      segmentStart,
      segmentEnd,
    );

    // Calculate the distance from the follower to the closest point
    // const distance = Math.sqrt(
    //   Math.pow(follower.position.x - closest.x, 2) +
    //     Math.pow(follower.position.y - closest.y, 2),
    // );

    const distance = Vector.dist(f.position, closest);

    // Track the closest point
    if (distance < minDistance) {
      minDistance = distance;
      closestPoint = closest;
    }
  }

  // Calculate the force to apply toward the closest point
  // const force = {
  //   x: closestPoint.x - f.position.x,
  //   y: closestPoint.y - f.position.y,
  // };
  const force = Vector.sub(closestPoint, f.position);

  // Normalize the force and apply a magnitude (this will depend on your desired strength)
  const magnitude = 100; // Adjust this based on your needs
  force.setMag(magnitude);
  return force;
}