package dev.llan.model.board.pathfinding;

import dev.llan.model.board.Grid;
import dev.llan.model.board.Point;
import dev.llan.model.board.Tile.TileType;

import java.util.*;

public class GridHeuristic implements PathFinder {
  private Grid grid;

  public GridHeuristic(Grid grid) {
    this.grid = grid;
  }

  @Override
  public Optional<Path> findPath(Point from, Point to) {
    PriorityQueue<Node> pq = new PriorityQueue<>();
    Map<Point, Double> costs = new HashMap<>();
    Set<Point> visited = new HashSet<>();

    Node original = new Node(from, Point.getDistance(from, to));
    pq.add(original);
    costs.put(from, 0.0);

    while (!pq.isEmpty()) {
      Node current = pq.poll();
      if (visited.contains(current.getPoint())) {
        continue;
      }
      visited.add(current.getPoint());

      if (current.getPoint().equals(to)) {
        return reconstructPath(current, from);
      }

      Point point = current.getPoint();
      List<Point> neighbors = point.findImmediateNeighbors(grid);
      addEndNode(neighbors, point, to);
      List<Node> nodes = processNeighbors(neighbors, current, to);
      addViableNodes(nodes, costs, pq);
    }

    return Optional.empty();
  }

  private void addViableNodes(List<Node> nodes, Map<Point, Double> costs, PriorityQueue<Node> pq) {
    for (Node node : nodes) {
      Point point = node.getPoint();
      double newCost = node.getCost();
      if (!costs.containsKey(point) || newCost < costs.get(point)) {
        costs.put(point, newCost);
        pq.add(node);
      }
    }
  }

  private List<Node> processNeighbors(List<Point> neighbors, Node parent, Point to) {
    List<Node> nodes = new ArrayList<>();
    for (Point neighbor : neighbors) {
      double cost = parent.getCost() + grid.getMovementCost(neighbor, parent.getPoint());
      Node node = new Node(neighbor, parent, Point.getDistance(neighbor, to), cost);
      nodes.add(node);
    }
    return nodes;
  }

  private void addEndNode(List<Point> neighbors, Point current, Point to) {
    TileType type = grid.tileAt(to).getType();
    if (type == TileType.OCCUPIED || type == TileType.UNMOVABLE) {
      return;
    }
    if (Point.getDistance(current, to) < 1.5) {
      neighbors.add(to);
    }
  }

  private Optional<Path> reconstructPath(Node lastNode, Point start) {
    Path path = new Path();

    Node current = lastNode;
    while (!current.getPoint().equals(start)) {
      path.addFirst(current.getPoint());

      if (current.getParent().isPresent()) {
        current = current.getParent().get();
      }
    }

    path.calculateCosts(start, grid);

    return Optional.of(path);
  }
}