import { Polygon } from "../geometry/polygon.ts";
import { Point, Vector } from "../geometry/vector.ts";

export const satCollision = (s1: Polygon, s2: Polygon) => {
  const shape1 = s1.points.map((p) => new Vector(p).add(s1.center));
  const shape2 = s2.points.map((p) => new Vector(p).add(s2.center));

  if (shape1.length < 2 || shape2.length < 2) {
    throw "Insufficient shape data in satCollision";
  }
  const offset = new Vector(s1.center).sub(s2.center);
  // Take one side of the polygon and find the normal
  let last: Vector[] = [];
  for (let i = 0; i < shape1.length; i++) {
    const axis = shape1[i].normal(shape1.at(i - 1)!);
    // axis.draw();
    let [p1min, p1minDot] = Vector.vectorProjectionAndDot(shape1[0], axis);
    let [p1max, p1maxDot] = [p1min, p1minDot];
    for (const point of shape1) {
      const [projected, dot] = Vector.vectorProjectionAndDot(point, axis);
      // projected.drawDot();
      p1min = dot < p1minDot ? projected : p1min;
      p1minDot = Math.min(dot, p1minDot);
      p1max = dot > p1maxDot ? projected : p1max;
      p1maxDot = Math.max(dot, p1maxDot);
    }
    let [p2min, p2minDot] = Vector.vectorProjectionAndDot(shape2[0], axis);
    let [p2max, p2maxDot] = [p2min, p2minDot];
    for (const point of shape2) {
      const [projected, dot] = Vector.vectorProjectionAndDot(point, axis);
      // projected.drawDot();
      p2min = dot < p2minDot ? projected : p2min;
      p2minDot = Math.min(dot, p2minDot);
      p2max = dot > p2maxDot ? projected : p2max;
      p2maxDot = Math.max(dot, p2maxDot);
    }

    // const scale = axis.dot(offset);
    // p1max += scale;
    // p1min += scale;

    // let p2min = axis.dot(shape1[0]);
    // let p2max = p2min;
    // for (const point of shape2) {
    //   const dot = point.dot(axis);
    //   p2max = Math.max(p2max, dot);
    //   p2min = Math.min(p2min, dot);
    //   Vector.vectorProjection(point, axis).drawDot();
    // }

    if (p1minDot - p2maxDot > 0 || p2minDot - p1maxDot > 0) {
      // axis.draw();
      // p1max.drawDot("blue");
      // p1min.drawDot("lime");
      // p2max.drawDot("blue");
      // p2min.drawDot("lime");
      return false;
    }
    last = [axis, p1max, p1min, p2max, p2min];
  }
  for (let i = 0; i < shape2.length; i++) {
    const axis = shape2[i].normal(shape2.at(i - 1)!);
    // axis.draw();
    let [p1min, p1minDot] = Vector.vectorProjectionAndDot(shape2[0], axis);
    let [p1max, p1maxDot] = [p1min, p1minDot];
    for (const point of shape2) {
      const [projected, dot] = Vector.vectorProjectionAndDot(point, axis);
      // projected.drawDot();
      p1min = dot < p1minDot ? projected : p1min;
      p1minDot = Math.min(dot, p1minDot);
      p1max = dot > p1maxDot ? projected : p1max;
      p1maxDot = Math.max(dot, p1maxDot);
    }
    let [p2min, p2minDot] = Vector.vectorProjectionAndDot(shape1[0], axis);
    let [p2max, p2maxDot] = [p2min, p2minDot];
    for (const point of shape1) {
      const [projected, dot] = Vector.vectorProjectionAndDot(point, axis);
      // projected.drawDot();
      p2min = dot < p2minDot ? projected : p2min;
      p2minDot = Math.min(dot, p2minDot);
      p2max = dot > p2maxDot ? projected : p2max;
      p2maxDot = Math.max(dot, p2maxDot);
    }

    if (p1minDot - p2maxDot > 0 || p2minDot - p1maxDot > 0) {
      // axis.draw();
      // p1max.drawDot("blue");
      // p1min.drawDot("lime");
      // p2max.drawDot("blue");
      // p2min.drawDot("lime");
      return false;
    }
    last = [axis, p1max, p1min, p2max, p2min];
  }

  // for (const [i, l] of last.entries()) {
  //   if (i === 0) {
  //     l.draw();
  //   } else {
  //     l.drawDot();
  //   }
  // }

  return true;
};