export type Point = [x: number, y: number, r: number]

export function add([ax, ay, ar]: Point, [bx, by, br]: Point): Point {
  return [ax + bx, ay + by, ar + br]
}

export function sub([ax, ay, ar]: Point, [bx, by, br]: Point): Point {
  return [ax - bx, ay - by, ar - br]
}

export function smul([x, y, r]: Point, s: number): Point {
  return [x * s, y * s, r * s]
}

export function norm([x, y, r]: Point): Point {
  return [x / Math.sqrt(x ** 2 + y ** 2), y / Math.sqrt(x ** 2 + y ** 2), r]
}

export function rot([x, y, r]: Point, rad: number): Point {
  return [Math.cos(rad) * x - Math.sin(rad) * y, Math.sin(rad) * x + Math.cos(rad) * y, r]
}

export function plerp(a: Point, b: Point, t: number): Point {
  return add(a, smul(sub(b, a), t))
}

export function lerp(a: number, b: number, t: number): number {
  return a + (b - a) * t
}

export function angle(p: Point, p1: Point, p2: Point) {
  return Math.atan2(p2[1] - p[1], p2[0] - p[0]) - Math.atan2(p1[1] - p[1], p1[0] - p[0])
}

export function normAngle(a: number) {
  return Math.atan2(Math.sin(a), Math.cos(a))
}

export function mag([x, y]: Point) {
  return Math.sqrt(x ** 2 + y ** 2)
}

export function dist([ax, ay]: Point, [bx, by]: Point): number {
  return Math.sqrt((bx - ax) ** 2 + (by - ay) ** 2)
}

export function getCircleAndPerpendicularLineIntersectionsAtPoint(
  point: Point,
  direction: Point,
  radius: number
): [Point, Point] {
  return [
    add(point, smul(norm(rot(direction, Math.PI / 2)), radius)),
    add(point, smul(norm(rot(direction, -Math.PI / 2)), radius)),
  ]
}

export function runLength(ps: Point[]): number {
  if (ps.length < 2) return 0

  let len = 0

  for (let i = 1; i <= ps.length - 1; i++) {
    len += dist(ps[i - 1], ps[i])
  }

  len += dist(ps[ps.length -2], ps[ps.length - 1])

  return len
}

export const clamp = (v: number, min: number, max: number) => Math.max(min, Math.min(max, v))

export function distancePointToSegment(p3: Point, p1: Point, p2: Point) {
  const sMag = dist(p1, p2)

  if (sMag === 0) return dist(p3, p1)

  const u = clamp(((p3[0] - p1[0]) * (p2[0] - p1[0]) + (p3[1] - p1[1]) * (p2[1] - p1[1])) / sMag ** 2, 0, 1)

  const pi: Point = [p1[0] + u * (p2[0] - p1[0]), p1[1] + u * (p2[1] - p1[1]), p3[2]]

  return dist(pi, p3)
}