package kb;

import java.util.PriorityQueue;
import java.util.HashSet;
import java.util.Set;
import java.util.List;
import java.util.Comparator;
import java.util.ArrayList;
import java.util.Collections;

public class KB {

    public static void main(String[] args) {
        ArrayList<Node> titik = new ArrayList<Node>();

        titik.add(new Node("A", 180));
        titik.add(new Node("B", 90));
        titik.add(new Node("C", 89));
        titik.add(new Node("D", 0));
        titik.add(new Node("E", 110));
        titik.add(new Node("F", 79));
        titik.add(new Node("G", 100));

        titik.get(0).tetangga = new Edge[]{
            new Edge(titik.get(1), 79),
            new Edge(titik.get(6), 63),
            new Edge(titik.get(5), 81)
        };
        titik.get(1).tetangga = new Edge[]{
            new Edge(titik.get(0), 79),
            new Edge(titik.get(6), 55),
            new Edge(titik.get(2), 131),
            new Edge(titik.get(3), 109)
        };
        titik.get(2).tetangga = new Edge[]{
            new Edge(titik.get(1), 131),
            new Edge(titik.get(3), 88),};

        titik.get(3).tetangga = new Edge[]{
            new Edge(titik.get(1), 109),
            new Edge(titik.get(5), 126),
            new Edge(titik.get(2), 88),
            new Edge(titik.get(4), 92),};

        titik.get(4).tetangga = new Edge[]{
            new Edge(titik.get(5), 148),
            new Edge(titik.get(3), 92),};

        titik.get(5).tetangga = new Edge[]{
            new Edge(titik.get(6), 60),
            new Edge(titik.get(0), 81),
            new Edge(titik.get(3), 126),};

        titik.get(6).tetangga = new Edge[]{
            new Edge(titik.get(0), 63),
            new Edge(titik.get(1), 55),
            new Edge(titik.get(5), 60),};

        AstarSearch(titik.get(0), titik.get(3));
        List<Node> path = printPath(titik.get(3));
        System.out.println("Perhitungan dengan algoritma A* dari titik A Ke D");
        System.out.println("path :" + path);

    }

    public static List<Node> printPath(Node target) {
        List<Node> path = new ArrayList<Node>();
        for (Node node = target; node != null; node = node.parent) {
            path.add(node);
        }
        Collections.reverse(path);
        return path;
    }

    public static void AstarSearch(Node source, Node goal) {

        Set<Node> explored = new HashSet<>();

        PriorityQueue<Node> queue = new PriorityQueue<>(20, (Node i, Node j) -> {
            if (i.f_scores > j.f_scores) {
                return 1;
            } else if (i.f_scores < j.f_scores) {
                return -1;
            } else {
                return 0;
            }
        }
        );

        source.g_scores = 0;

        queue.add(source);

        boolean found = false;

        while ((!queue.isEmpty()) && (!found)) {

            Node current = queue.poll();

            explored.add(current);

            if (current.value.equals(goal.value)) {
                found = true;
            }

            for (Edge e : current.tetangga) {
                Node child = e.target;
                double cost = e.cost;
                double temp_g_scores = current.g_scores + cost;
                double temp_f_scores = temp_g_scores + child.h_scores;

                if ((explored.contains(child))
                        && (temp_f_scores >= child.f_scores)) {
                    continue;
                }
                else if ((!queue.contains(child))
                        || (temp_f_scores < child.f_scores)) {

                    child.parent = current;
                    child.g_scores = temp_g_scores;
                    child.f_scores = temp_f_scores;

                    if (queue.contains(child)) {
                        queue.remove(child);
                    }
                    queue.add(child);

                }

            }

        }

    }

}

class Node {

    public final String value;
    public double g_scores;
    public double f_scores = 0;
    public Edge[] tetangga;
    public Node parent;
    public final double h_scores;

    public Node(String val, double hVal) {
        value = val;
        h_scores = hVal;
    }

    public String toString() {
        return value;
    }

}

class Edge {

    public final double cost;
    public final Node target;

    public Edge(Node targetNode, double costVal) {
        target = targetNode;
        cost = costVal;
    }
}