Graphs.java

import java.util.*;
import java.util.function.BiFunction;

public class Graphs {

	static class Node{
		public double g = 0;
		public double f = 0;
		public double h = 0;

		int id;
		static int lastid = 0;
		{
			this.id = lastid++;
		}
		Node parent = null;
		boolean visited = false;
		HashMap<Node, Double> connections = new HashMap<>();
		public void connect(Node n, double cost){
			if(cost == 0 || n == this)
				return;
			Double current = this.connections.getOrDefault(n, 0d);
			this.connections.put(n, current + cost);
		}


		@Override
		public String toString() {
			return Integer.toString(this.id);
		}

	}

	public double[] run(double[][] matrix){
		Node[] nodes = new Node[matrix.length];

		for (int i = 0; i < matrix.length; i++) {
			nodes[i] = new Node();
		}

		for (int x = 0; x < matrix.length; x++) {
			for (int y = 0; y < matrix[x].length; y++) {
				nodes[x].connect(nodes[y], matrix[x][y]);
			}
		}

		Node startNode = nodes[1];
		double[] traversed = findAStar(startNode, nodes[25]);
		System.out.println(Arrays.toString(traversed));
        return traversed;
	}

	public static double[] traverseDFS(Node startNode){

		Stack<Node> stack = new Stack<>();
		stack.push(startNode);

		HashSet<Node> nodeSet = new HashSet<>();

		while(!stack.empty()){
			Node v = stack.pop();

			if(v.visited) continue;

			v.visited = true;
			nodeSet.add(v);

			for(Node w : v.connections.keySet()) {
				if(w==v || v.connections.get(w) == 0 || w.parent != null || w.visited) continue;
				stack.push(w);
				w.parent = v;
			}
		}

		return nodeSet.stream().map(node -> node.id).mapToDouble(Integer::doubleValue).toArray();
	}

	public static double[] findAStar(Node startNode, Node endNode){
		List<Node> closedSet = new ArrayList<>();
		List<Node> openSet = new ArrayList<>();
		openSet.add(startNode);
		BiFunction<Node, Node, Float> heuristics = (n1, n2) -> (float) Math.abs(n1.id - n2.id);
		while(! openSet.isEmpty()){
			Node v = openSet.stream().min((o1, o2) -> (int) (o1.f - o2.f)).get();
			if(v == endNode)
				break;
			openSet.remove(v);
			closedSet.add(v);
			for (Node w : v.connections.keySet()) {
				if( closedSet.contains(w) )
					continue;
				double tmpG = v.g + heuristics.apply(w, v);
				boolean newPath = false;
				if( openSet.contains(w) ){
					if(tmpG < w.g){
						w.g = tmpG;
						newPath = true;
					}
				} else {
					w.g = tmpG;
					newPath = true;
					openSet.add(w);
				}
				if(newPath) {
					w.h = heuristics.apply(w, endNode);
					w.f = w.g + w.h;
					w.parent = v;
				}
			}
		}
		List<Node> path = new ArrayList<>();
		Node n = endNode;
		path.add(n);
		while((n = n.parent) != null)
			path.add(n);
		Collections.reverse(path);
		return path.stream().map(node -> node.id).mapToDouble(Integer::doubleValue).toArray();
	}

	public static void main (String[] args) {
		int NODES = 50;
		double[][] matrix = new double[NODES][NODES];
		Random r = new Random(System.currentTimeMillis());
		for (int x = 0; x < matrix.length; x++) {
			for (int y = 0; y < matrix[x].length; y++) {
				matrix[x][y] = r.nextDouble() <= 0.05 ? 1 : 0;
			}
		}
		new Graphs().run(matrix);
	}
}