Astar.java

import java.util.ArrayList;
import java.util.Date;

public class Astar {

	private static int MAP1_WIDTH = 5;
	private static int MAP1_HEIGHT = 5;
	private static int MAP2_WIDTH = 5;
	private static int MAP2_HEIGHT = 5;
	private static int START_SQUARE = 3;
	private static int FINISH_SQUARE = 2;

	public static int[][] map1 = new int[][] { { 0, 1, 0, 0, 3 }, { 2, 0, 0, 1, 0 }, { 0, 0, 1, 0, 0 }, { 0, 1, 1, 0, 0 }, { 0, 0, 0, 0, 0 } };

	public static int[][] map2 = new int[][] { { 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0 }, { 0, 0, 2, 0, 0 }, { 0, 3, 0, 0, 0 }, { 0, 0, 1, 0, 0 } };

	// print tags
	private static String LEFT = "left";
	private static String RIGHT = "right";
	private static String UP = "up";
	private static String DOWN = "down";
	private static String START = "start";
	private static String END = "end";

	public static void main(String[] args) {

		Date startTime = new Date();

		int xStart1, xStart2, yStart1, yStart2;
		int xEnd1, xEnd2, yEnd1, yEnd2;

		// these need to be initialised to make Eclipse happy
		xStart1 = xStart2 = yStart1 = yStart2 = 0;
		xEnd1 = xEnd2 = yEnd1 = yEnd2 = 0;

		// get start and end coordinates for map 1
		for (int y = 0; y < MAP1_HEIGHT; y++ ) {
			for (int x = 0; x < MAP1_WIDTH; x++ ) {
				if (map1[y][x] == START_SQUARE) {
					xStart1 = x;
					yStart1 = y;
				} else if (map1[y][x] == FINISH_SQUARE) {
					xEnd1 = x;
					yEnd1 = y;
				}
			}
		}

		// get start and end coordinates for map 2
		for (int y = 0; y < MAP2_HEIGHT; y++ ) {
			for (int x = 0; x < MAP2_WIDTH; x++ ) {
				if (map2[y][x] == START_SQUARE) {
					xStart2 = x;
					yStart2 = y;
				} else if (map2[y][x] == FINISH_SQUARE) {
					xEnd2 = x;
					yEnd2 = y;
				}
			}
		}

		// initialise start and end node
		Node start = new Node(xStart1, xStart2, yStart1, yStart2, 0, START);
		Node end = new Node(xEnd1, xEnd2, yEnd1, yEnd2, 0, END);

		start.setCameFrom(null);
		start.setgScore(0);
		start.setfScore(start.getHeuristic(end));

		ArrayList<Node> openSet = new ArrayList<Node>();
		ArrayList<Node> closedSet = new ArrayList<Node>();

		openSet.add(start);
		Node current;
		ArrayList<Node> neighbours;

		while ( !openSet.isEmpty()) {

			// find node in openset with lowest fscore
			int index = 0;
			int minFscore = openSet.get(index).getfScore();
			for (int i = 1; i < openSet.size(); i++ ) {
				int tentativeMin = openSet.get(i).getfScore();
				if (tentativeMin < minFscore) {
					index = i;
					minFscore = tentativeMin;
				}
			}

			current = openSet.get(index);

			if (current.equals(end)) {
				// current.movement = END;
				ArrayList<Node> path = current.reconstructPath();
				System.out.println("Shortest path is " + path.size() + " moves"); // subtract
																					// 2
																					// for
																					// the
																					// start
																					// and
																					// finish
																					// nodes
				for (Node p : path) {
					System.out.println(p.movement);
				}
				break;
			}

			openSet.remove(index);
			closedSet.add(current);

			neighbours = current.getNeighbours(map1, map2);

			for (Node n : neighbours) {
				int tentativeGScore = current.getgScore() + n.edge;
				int tentativeFScore = tentativeGScore + n.getHeuristic(end);
				boolean neitherSet = true;

				// if in open set and lower gscore
				for (int i = 0; i < openSet.size(); i++ ) {
					if (openSet.get(i).equals(n) && tentativeGScore < openSet.get(i).getgScore()) {
						openSet.remove(i);
						neitherSet = false;
						break;
					}
				}

				// if in closedset and lower gscore
				for (int i = 0; i < closedSet.size(); i++ ) {
					if (closedSet.get(i).equals(n) && tentativeGScore < closedSet.get(i).getgScore()) {
						closedSet.remove(i);
						neitherSet = false;
						break;
					}
				}

				if (neitherSet) {
					n.cameFrom = current;
					n.setgScore(tentativeGScore);
					n.setfScore(tentativeFScore);
					openSet.add(n);
				}

			}

		}

		Date finishTime = new Date();
		System.out.println("DONE took " + ( (finishTime.getTime() - startTime.getTime())) + " milliseconds");

	}

	private static class Node {
		Node cameFrom;
		int gScore;
		int fScore;
		int edge;
		String movement;

		int x1, x2, y1, y2;

		public Node(int x_1, int x_2, int y_1, int y_2, int e, String t) {
			x1 = x_1;
			x2 = x_2;
			y1 = y_1;
			y2 = y_2;
			edge = e;
			movement = t;

		}

		public boolean equals(Node n) {
			if (x1 == n.x1 && x2 == n.x2 && y1 == n.y1 && y2 == n.y2) {
				return true;
			}
			return false;
		}

		public Node getCameFrom() {
			return cameFrom;
		}

		public void setCameFrom(Node cameFrom) {
			this.cameFrom = cameFrom;
		}

		public int getgScore() {
			return gScore;
		}

		public void setgScore(int gScore) {
			this.gScore = gScore;
		}

		public int getfScore() {
			return fScore;
		}

		public void setfScore(int fScore) {
			this.fScore = fScore;
		}

		public int getHeuristic(Node n) {
			int heuristic = (Math.abs(x1 - n.x1) + Math.abs(x2 - n.x2) + Math.abs(y1 - n.y1) + Math.abs(y2 - n.y2));
			return heuristic;
		}

		public ArrayList<Node> getNeighbours(int[][] m1, int[][] m2) {
			ArrayList<Node> neighbours = new ArrayList<Node>();
			int nx1, nx2, ny1, ny2;
			int nEdge;

			// left
			nx1 = (x1 > 0 && m1[y1][x1 - 1] != 1) ? (x1 - 1) : x1;
			nx2 = (x2 > 0 && m2[y2][x2 - 1] != 1) ? (x2 - 1) : x2;
			nEdge = x1 - nx1 + x2 - nx2;
			if (nEdge != 0)
				neighbours.add(new Node(nx1, nx2, y1, y2, 1, LEFT));

			// right
			nx1 = (x1 < MAP1_WIDTH - 1 && m1[y1][x1 + 1] != 1) ? (x1 + 1) : x1;
			nx2 = (x2 < MAP2_WIDTH - 1 && m2[y2][x2 + 1] != 1) ? (x2 + 1) : x2;
			nEdge = nx1 - x1 + nx2 - x2;
			if (nEdge != 0)
				neighbours.add(new Node(nx1, nx2, y1, y2, 1, RIGHT));

			// up
			ny1 = (y1 > 0 && m1[y1 - 1][x1] != 1) ? (y1 - 1) : y1;
			ny2 = (y2 > 0 && m2[y2 - 1][x2] != 1) ? (y2 - 1) : y2;
			nEdge = y1 - ny1 + y2 - ny2;
			if (nEdge != 0)
				neighbours.add(new Node(x1, x2, ny1, ny2, 1, UP));

			// down
			ny1 = (y1 < MAP1_HEIGHT - 1 && m1[y1 + 1][x1] != 1) ? (y1 + 1) : y1;
			ny2 = (y2 < MAP2_HEIGHT - 1 && m2[y2 + 1][x2] != 1) ? (y2 + 1) : y2;
			nEdge = ny1 - y1 + ny2 - y2;
			if (nEdge != 0)
				neighbours.add(new Node(x1, x2, ny1, ny2, 1, DOWN));

			return neighbours;
		}

		public ArrayList<Node> reconstructPath() {
			ArrayList<Node> path = new ArrayList<Node>();

			if (cameFrom != null) {
				path.addAll(cameFrom.reconstructPath());
				path.add(this);
			}
			return path;
		}

	}
}