/*
	Time Complexity = O(E), where E is equal to the number of edges
*/

package A_Star;

import java.util.*;

public class A_Star {

 private static class Graph {
 // Graph's structure can be changed only applying changes to this class.
 private ArrayList<Edge>[] graph;

 // Initialise ArrayLists in Constructor
 public Graph(int size) {
 this.graph = new ArrayList[size];
 for (int i = 0; i < size; i++) {
 this.graph[i] = new ArrayList<>();
 }
 }

 private ArrayList<Edge> getNeighbours(int from) {
 return this.graph[from];
 }

 // Graph is bidirectional, for just one direction remove second instruction of this method.
 private void addEdge(Edge edge) {
 this.graph[edge.getFrom()].add(new Edge(edge.getFrom(), edge.getTo(), edge.getWeight()));
 this.graph[edge.getTo()].add(new Edge(edge.getTo(), edge.getFrom(), edge.getWeight()));
 }
 }

 private static class Edge {
 private int from;
 private int to;
 private int weight;

 public Edge(int from, int to, int weight) {
 this.from = from;
 this.to = to;
 this.weight = weight;
 }

 public int getFrom() {
 return from;
 }

 public int getTo() {
 return to;
 }

 public int getWeight() {
 return weight;
 }
 }

 // class to iterate during the algorithm execution, and also used to return the solution.
 private static class PathAndDistance {
 private int distance; // distance advanced so far.
 private ArrayList<Integer> path; // list of visited nodes in this path.
 private int
 estimated; // heuristic value associated to the last node od the path (current node).

 public PathAndDistance(int distance, ArrayList<Integer> path, int estimated) {
 this.distance = distance;
 this.path = path;
 this.estimated = estimated;
 }

 public int getDistance() {
 return distance;
 }

 public ArrayList<Integer> getPath() {
 return path;
 }

 public int getEstimated() {
 return estimated;
 }

 private void printSolution() {
 if (this.path != null)
 System.out.println(
 "Optimal path: " + this.path.toString() + ", distance: " + this.distance);
 else System.out.println("There is no path available to connect the points");
 }
 }

 private static void initializeGraph(Graph graph, ArrayList<Integer> data) {
 for (int i = 0; i < data.size(); i += 4) {
 graph.addEdge(new Edge(data.get(i), data.get(i + 1), data.get(i + 2)));
 }
 /*
 .x. node
 (y) cost
 - or | or / bidirectional connection

 ( 98)- .7. -(86)- .4.
 |
 ( 85)- .17. -(142)- .18. -(92)- .8. -(87)- .11.
 |
 . 1. -------------------- (160)
 | \ |
 (211) \ .6.
 | \ |
 . 5. (101)-.13. -(138) (115)
 | | | /
 ( 99) ( 97) | /
 | | | /
 .12. -(151)- .15. -(80)- .14. | /
 | | | | /
 ( 71) (140) (146)- .2. -(120)
 | | |
 .19. -( 75)- . 0. .10. -(75)- .3.
 | |
 (118) ( 70)
 | |
 .16. -(111)- .9.
 */
 }

 public static void main(String[] args) {
 // heuristic function optimistic values
 int[] heuristic = {
 366, 0, 160, 242, 161, 178, 77, 151, 226, 244, 241, 234, 380, 98, 193, 253, 329, 80, 199, 374
 };

 Graph graph = new Graph(20);
 ArrayList<Integer> graphData =
 new ArrayList<>(
 Arrays.asList(
 0, 19, 75, null, 0, 15, 140, null, 0, 16, 118, null, 19, 12, 71, null, 12, 15, 151,
 null, 16, 9, 111, null, 9, 10, 70, null, 10, 3, 75, null, 3, 2, 120, null, 2, 14,
 146, null, 2, 13, 138, null, 2, 6, 115, null, 15, 14, 80, null, 15, 5, 99, null, 14,
 13, 97, null, 5, 1, 211, null, 13, 1, 101, null, 6, 1, 160, null, 1, 17, 85, null,
 17, 7, 98, null, 7, 4, 86, null, 17, 18, 142, null, 18, 8, 92, null, 8, 11, 87));
 initializeGraph(graph, graphData);

 PathAndDistance solution = aStar(3, 1, graph, heuristic);
 solution.printSolution();
 }

 public static PathAndDistance aStar(int from, int to, Graph graph, int[] heuristic) {
 // nodes are prioritised by the less value of the current distance of their paths, and the
 // estimated value
 // given by the heuristic function to reach the destination point from the current point.
 PriorityQueue<PathAndDistance> queue =
 new PriorityQueue<>(Comparator.comparingInt(a -> (a.getDistance() + a.getEstimated())));

 // dummy data to start the algorithm from the beginning point
 queue.add(new PathAndDistance(0, new ArrayList<>(Arrays.asList(from)), 0));

 boolean solutionFound = false;
 PathAndDistance currentData = new PathAndDistance(-1, null, -1);
 while (!queue.isEmpty() && !solutionFound) {
 currentData = queue.poll(); // first in the queue, best node so keep exploring.
 int currentPosition =
 currentData.getPath().get(currentData.getPath().size() - 1); // current node.
 if (currentPosition == to) solutionFound = true;
 else
 for (Edge edge : graph.getNeighbours(currentPosition))
 if (!currentData.getPath().contains(edge.getTo())) { // Avoid Cycles
 ArrayList<Integer> updatedPath = new ArrayList<>(currentData.getPath());
 updatedPath.add(edge.getTo()); // Add the new node to the path, update the distance,
 // and the heuristic function value associated to that path.
 queue.add(
 new PathAndDistance(
 currentData.getDistance() + edge.getWeight(),
 updatedPath,
 heuristic[edge.getTo()]));
 }
 }
 return (solutionFound) ? currentData : new PathAndDistance(-1, null, -1);
 // Out of while loop, if there is a solution, the current Data stores the optimal path, and its
 // distance
 }
}