import Entity from "./entity.js"
import PriorityQueue from "./priority-queue.js"
import SATX from "./satx.js"

export default class Pathfind {
  static keyFor(pos) {
    return `${pos.x},${pos.y}`
  }

  static shortestPath(graph, start, goal) {
    const queue = new PriorityQueue((a, b) => a[1] < b[1])
    const visited = new Map()

    queue.push([[[start, null]], 0])

    while (!queue.isEmpty()) {
      const [path, cost] = queue.pop()
      const waypoint = path.at(-1).at(0)

      if (waypoint.equals(goal)) {
        path.shift()
        return path
      }

      const key = this.keyFor(waypoint)
      if (!visited.has(key) || visited.get(key) > cost) {
        visited.set(key, cost)

        for (const { to, direction, distance } of graph.filter(e => e.from.equals(waypoint))) {
          const keyTo = this.keyFor(to)
          if (!visited.has(keyTo) || visited.get(keyTo) > cost + distance) {
            queue.push([[...path, [to, direction]], cost + distance])
          }
        }
      }
    }

    return []
  }

  static buildGraph(toWaypoints, colliders, fromWaypoints = null) {
    const graph = []
    const calculated = new Set()

    for (const [from, reverseDirection] of (fromWaypoints ?? toWaypoints)) {
      for (const [to, direction] of toWaypoints) {
        if (from.equals(to)) {
          continue
        }

        const key = `${from.x},${from.y};${to.x},${to.y}`
        if (!calculated.has(key)) {
          calculated.add(key)
          calculated.add(`${to.x},${to.y};${from.x},${from.y}`)

          const lineOfSight = !SATX.collideObjects(SATX.line(from, to), colliders)

          if (lineOfSight) {
            const distance = from.distanceTo(to)
            graph.push({ from, to, distance, direction })
            graph.push({ to: from, from: to, distance, direction: reverseDirection })
          }
        }
      }
    }

    return graph
  }
}