diff --git a/src/ei/engine/math/Vector2f.java b/src/ei/engine/math/Vector2f.java
index 93bda1a..353dd30 100644
--- a/src/ei/engine/math/Vector2f.java
+++ b/src/ei/engine/math/Vector2f.java
@@ -6,7 +6,7 @@ package ei.engine.math;
  * @author Ziver
  */
 
-public class Vector2f {
+public class Vector2f implements Comparable{
 	private float x;
 	private float y;
 	
@@ -67,4 +67,14 @@ public class Vector2f {
 	public String toString(){
 		return "Vector2f["+x+","+y+"]";
 	}
+	
+
+	public int compareTo(Object v) {
+		if(x+y > ((Vector2f) v).getX()+((Vector2f) v).getY())
+			return 1;
+		else if(x+y < ((Vector2f) v).getX()+((Vector2f) v).getY())
+			return -1;
+		else
+			return 0;
+	}
 }
diff --git a/src/ei/engine/math/Vector2i.java b/src/ei/engine/math/Vector2i.java
index 104bc3c..0131f8e 100644
--- a/src/ei/engine/math/Vector2i.java
+++ b/src/ei/engine/math/Vector2i.java
@@ -6,7 +6,7 @@ package ei.engine.math;
  * @author Ziver
  */
 
-public class Vector2i {
+public class Vector2i implements Comparable{
 	private int x;
 	private int y;
 	
@@ -55,7 +55,25 @@ public class Vector2i {
 		y += i;
 	}
 	
+	/**
+	 * Returns a copy of this vector
+	 * 
+	 * @return A copy of this vector
+	 */
+	public Vector2i getCopy(){
+		return new Vector2i(x,y);
+	}
+	
 	public String toString(){
 		return "Vector2i["+x+","+y+"]";
 	}
+
+	public int compareTo(Object v) {
+		if(x+y > ((Vector2i) v).getX()+((Vector2i) v).getY())
+			return 1;
+		else if(x+y < ((Vector2i) v).getX()+((Vector2i) v).getY())
+			return -1;
+		else
+			return 0;
+	}
 }
diff --git a/src/ei/game/algo/AStar.java b/src/ei/game/algo/AStar.java
new file mode 100644
index 0000000..d5fd510
--- /dev/null
+++ b/src/ei/game/algo/AStar.java
@@ -0,0 +1,86 @@
+package ei.game.algo;
+
+import java.util.LinkedList;
+import java.util.List;
+import java.util.Random;
+
+import ei.engine.math.Vector2i;
+import ei.game.algo.AStarNode2D.Heuristic;
+import ei.game.gamestate.InGameState;
+
+public class AStar{
+	private static final long serialVersionUID = 1L;
+	private AStarPathfinder pathfinder;
+	
+	private int width;
+	private int hight;
+	private AStarNode[][] map;
+	
+	protected boolean randomizeMap = true;
+	protected Random RAND = new Random(0);
+	
+	public AStar() {
+		width = InGameState.getMap().getSize().getX();
+		hight = InGameState.getMap().getSize().getY();
+		initializePathfinder();
+		initializeMap();
+	}
+
+	public List<AStarNode> startSearch(Vector2i start, Vector2i goal){
+		return pathfinder.findPath(map[start.getX()][start.getY()], map[goal.getX()][goal.getY()]);
+	}
+	
+	protected void initializePathfinder() {
+		System.out.println("Initializing pathfinder");
+
+		AStarNode2D.heuristic = Heuristic.Euclidean;
+		
+		// Create the pathfinder.
+		pathfinder = new AStarPathfinder();
+	}
+	
+	
+	protected void initializeMap() {
+		System.out.println("Initializing map");
+		// Create the map.
+		map = new AStarNode2D[width][hight];
+		for(int x = 0, nodeId = 0; width > x; x++) {
+			for(int y = 0; hight > y; y++, nodeId++) {
+				map[x][y] = new AStarNode2D(x, y, nodeId);
+				
+				if(!InGameState.getMap().isPosEmpty(x, y)) {
+					map[x][y].setBlocked(true);
+				}
+
+			}
+		}
+		
+		// Create the neighbours.
+		for(int x = 0; width > x; x++) {
+			for(int y = 0; hight > y; y++) {
+				
+				List<AStarNeighbour> neighbours = new LinkedList<AStarNeighbour>();
+				
+				if(y-1 >= 0) 
+					neighbours.add(new AStarNeighbour(map[x][y-1], AStarNeighbour.Location.Adjacent));	// North.
+				if(x+1 < width && y-1 > 0) 
+					neighbours.add(new AStarNeighbour(map[x+1][y-1], AStarNeighbour.Location.Diagonal));	// North-East.
+				if(x+1 < width) 
+					neighbours.add(new AStarNeighbour(map[x+1][y], AStarNeighbour.Location.Adjacent));	// East.	
+				if(x+1 < width && y+1 < hight) 
+					neighbours.add(new AStarNeighbour(map[x+1][y+1], AStarNeighbour.Location.Diagonal));	// South-East.
+				if(y+1 < hight) 
+					neighbours.add(new AStarNeighbour(map[x][y+1], AStarNeighbour.Location.Adjacent));	// South.					
+				if(x-1 >= 0 && y+1 < hight) 
+					neighbours.add(new AStarNeighbour(map[x-1][y+1], AStarNeighbour.Location.Diagonal));	// South-West.
+				if(x-1 >= 0) 
+					neighbours.add(new AStarNeighbour(map[x-1][y], AStarNeighbour.Location.Adjacent));	// West.
+
+				if(x-1 >= 0 && y-1 > 0) neighbours.add(new AStarNeighbour(map[x-1][y-1], AStarNeighbour.Location.Diagonal));	// North-West.
+
+				map[x][y].setNeighbours(neighbours);
+				
+			}
+		}
+	}
+}
diff --git a/src/ei/game/algo/AStarNeighbour.java b/src/ei/game/algo/AStarNeighbour.java
new file mode 100644
index 0000000..aab10b3
--- /dev/null
+++ b/src/ei/game/algo/AStarNeighbour.java
@@ -0,0 +1,26 @@
+package ei.game.algo;
+
+public class AStarNeighbour {
+	
+	private Location location;
+	private AStarNode node;
+	
+	public AStarNeighbour(AStarNode node, Location location) {
+		this.node = node;
+		this.location = location;
+	}
+	
+	public AStarNode getNode() {
+		return node;
+	}
+	
+	public Location getLocation() {
+		return location;
+	}
+		
+	public enum Location {
+		Adjacent,
+		Diagonal
+	}
+
+}
diff --git a/src/ei/game/algo/AStarNode.java b/src/ei/game/algo/AStarNode.java
new file mode 100644
index 0000000..79e9cb9
--- /dev/null
+++ b/src/ei/game/algo/AStarNode.java
@@ -0,0 +1,167 @@
+package ei.game.algo;
+
+import java.util.List;
+
+import ei.game.algo.AStarNeighbour.Location;
+
+/**
+ * This Node class defines the Node that make up grids, paths etc.
+ * 
+ * @author Árni Arent
+ *
+ */
+public abstract class AStarNode implements Comparable {
+
+	protected int nodeId;
+	protected List<AStarNeighbour> neighbours;
+	protected AStarNode parent;
+	protected boolean blocked;
+	protected boolean visited;
+	protected int visitOrder;
+	protected boolean partOfPath;
+	public float costFromStart;
+	protected float estimatedCostToGoal;
+	
+	/**
+	 * 
+	 * @param nodeId
+	 */
+	public AStarNode(int nodeId) {
+		this.nodeId = nodeId;
+	}
+	
+	/**
+	 * 
+	 * @return
+	 */
+	public AStarNode getParent() {
+		return parent;
+	}
+
+	/**
+	 * 
+	 * @param parent
+	 */
+	public void setParent(AStarNode parent) {
+		this.parent = parent;
+	}
+
+	/**
+	 * Returns all the neighbours of this node.
+	 * Usually in grid maps the neighbours are positioned in north,
+	 * north-east, east, south-east, south, south-west, west and north-west.
+	 * 
+	 * @return the neighbours of this node.
+	 */
+	public List<AStarNeighbour> getNeighbours() {
+		return neighbours;
+	}
+
+	/**
+	 * Set the neighbours of this Node.
+	 * Usually in grid maps the neighbours are positioned in north,
+	 * north-east, east, south-east, south, south-west, west and north-west.
+	 * 
+	 * @param neighbours the neighbours of this Node.
+	 */
+	public void setNeighbours(List<AStarNeighbour> neighbours) {
+		this.neighbours = neighbours;
+	}
+
+	/**
+	 * Gives the id of the Node. The id is a unique integer number
+	 * defining the number of the node.
+	 * Grid maps of size 64x64 have 4096 nodes, so each node has a
+	 * id ranging from 0 to 4095.
+	 * This can usually be thought of as a auto incremental unique
+	 * identifier number, each Node created gets a incremented unique
+	 * id.
+	 * 
+	 * @return the id of this Node.
+	 */
+	public int getNodeId() {
+		return nodeId;
+	}
+	
+	
+	/**
+	 * Tells if this Node is blocked or not.
+	 * 
+	 * @return true if the node is blocked, false if not.
+	 */
+	public boolean isBlocked() {
+		return blocked;
+	}
+
+	/**
+	 * Defined if this Node should be blocked or not.
+	 * 
+	 * @param blocked true if it should be blocked, false if not.
+	 */
+	public void setBlocked(boolean blocked) {
+		this.blocked = blocked;
+	}
+	
+	public boolean isVisited() {
+		return visited;
+	}
+
+	public void setVisited(boolean visited) {
+		this.visited = visited;
+	}
+	
+	public void setVisitOrder(int order) {
+		this.visitOrder = order;
+	}
+	
+	public int getVisitOrder() {
+		return visitOrder;
+	}
+	
+	
+	
+	
+	public boolean isPartOfPath() {
+		return partOfPath;
+	}
+
+	public void setPartOfPath(boolean partOfPath) {
+		this.partOfPath = partOfPath;
+	}
+
+	public void reset() {
+		visited = false;
+		visitOrder = 0;
+		parent = null;
+		partOfPath = false;
+		estimatedCostToGoal = 0;
+	}
+
+	@Override
+	public String toString() {
+		return "[Node " + nodeId + " ("+(blocked?"X":" ")+")]";
+	}
+
+	public float getEstimatedCostFromStartToGoal() {
+		return costFromStart+estimatedCostToGoal;
+	}
+
+	public abstract float getEstimatedCostTo(AStarNode node, AStarNode startNode);
+	
+	public abstract float getCost(AStarNode node, Location location);
+	
+	public int compareTo(Object n) {
+		float cost = getEstimatedCostFromStartToGoal() - ((AStarNode)n).getEstimatedCostFromStartToGoal();
+		
+		if(cost > 0)
+			return 1;
+		else if(cost < 0)
+			return -1;
+		else
+			return 0;
+	}
+	
+	public abstract int getX();
+
+	public abstract int getY();
+}
diff --git a/src/ei/game/algo/AStarNode2D.java b/src/ei/game/algo/AStarNode2D.java
new file mode 100644
index 0000000..0928c05
--- /dev/null
+++ b/src/ei/game/algo/AStarNode2D.java
@@ -0,0 +1,117 @@
+package ei.game.algo;
+
+import ei.game.algo.AStarNeighbour.Location;
+
+/**
+ * 
+ * @author Árni Arent
+ *
+ */
+public class AStarNode2D extends AStarNode {
+	protected final static float adjacentCost = 1;
+	protected final static float diagonalCost = (float)Math.sqrt(2)*adjacentCost;
+	protected final static float tieBreaker = adjacentCost/(1024f/1024f); 
+	 
+	public enum Heuristic {
+		Manhattan,
+		Euclidean,
+		Diagonal,
+		DiagonalWithTieBreaking,
+		DiagonalWithTieBreakingCrossProduct,
+	}
+	
+	public static Heuristic heuristic = Heuristic.Manhattan;
+	
+	protected int x, y;
+	
+	public AStarNode2D(int x, int y, int nodeId) {
+		super(nodeId);
+		
+		this.x = x;
+		this.y = y;
+	}
+	
+	public int getX() {
+		return x;
+	}
+
+	public int getY() {
+		return y;
+	}
+
+
+	@Override
+	public float getEstimatedCostTo(AStarNode node, AStarNode startNode) {
+		AStarNode2D goal = (AStarNode2D)node;
+		AStarNode2D start = (AStarNode2D)startNode;
+		
+		float h = 0;
+		
+		/*
+		 * Manhattan distance heuristics. (no diagonal treatment) 
+		 */
+		if(heuristic == Heuristic.Manhattan) {
+			h = adjacentCost * (Math.abs(x-goal.x) + Math.abs(y-goal.y));
+		}
+		
+		/*
+		 * Euclidean distances (move at any angle, again no diagonal treatment).
+		 */
+		else if(heuristic == Heuristic.Euclidean) {
+			h = adjacentCost * (float)Math.sqrt(Math.pow(x-goal.x, 2) + Math.pow(y-goal.y,2));
+		}
+		
+		/*
+		 * Diagonal distance heuristic.
+		 */
+		else if(heuristic == Heuristic.Diagonal || 
+				heuristic == Heuristic.DiagonalWithTieBreaking || 
+				heuristic == Heuristic.DiagonalWithTieBreakingCrossProduct) {
+			float diagonal = Math.min(Math.abs(x-goal.x), Math.abs(y-goal.y));
+			float straight = (Math.abs(x-goal.x) + Math.abs(y-goal.y));
+			h = (diagonalCost * diagonal) + (adjacentCost * (straight - (2*diagonal)));
+			
+			
+			/*
+			 * Normal tie-breaking.
+			 */
+			if(heuristic == Heuristic.DiagonalWithTieBreaking) {
+				h *= (1.0 + tieBreaker);
+			}
+			
+			/*
+			 * Add tie-breaking cross-product to the heuristics.
+			 * (Produces nicer looking diagonal paths, but weird with obstacles)
+			 */
+			else if(heuristic == Heuristic.DiagonalWithTieBreakingCrossProduct) {
+				float dx1 = x - goal.x;
+				float dy1 = y - goal.y;
+				float dx2 = start.x - goal.x;
+				float dy2 = start.y - goal.y;
+				float cross = Math.abs(dx1*dy2 - dx2*dy1);
+				h += cross*0.001;
+			}
+		}
+
+		// Return the heuristic.
+		return h;
+	}
+
+	@Override
+	public float getCost(AStarNode node, Location location) {
+
+		if(heuristic == Heuristic.Manhattan || heuristic == Heuristic.Euclidean) {
+			return adjacentCost;
+		} else {
+			if(location == Location.Adjacent) {
+				return adjacentCost;
+			} else {
+				return diagonalCost;
+			}
+		}
+	}
+	
+
+	
+
+}
diff --git a/src/ei/game/algo/AStarPathfinder.java b/src/ei/game/algo/AStarPathfinder.java
new file mode 100644
index 0000000..24a9fe1
--- /dev/null
+++ b/src/ei/game/algo/AStarPathfinder.java
@@ -0,0 +1,139 @@
+package ei.game.algo;
+
+import java.util.Iterator;
+import java.util.LinkedList;
+import java.util.List;
+
+import ei.game.algo.AStarNeighbour.Location;
+
+
+/**
+ * 
+ * @author Árni Arent
+ *
+ */
+public class AStarPathfinder{
+	protected LinkedList<AStarNode> open;
+	protected LinkedList<AStarNode> closed;
+	private AStarNode start;
+	private AStarNode goal;
+
+	/**
+	 * Constructs a Pathfinder.
+	 */
+	public AStarPathfinder() {
+		this.open = new LinkedList<AStarNode>();
+		this.closed = new LinkedList<AStarNode>();
+	}
+	
+	/**
+	 * 
+	 * @param from the goal to go from, the start Node (origin).
+	 * @param to the goal to go to, the destination Node.
+	 * @return a List containing the path of Nodes to travel to reach goal.
+	 */
+	public List<AStarNode> findPath(AStarNode start, AStarNode goal) {
+		this.start = start;
+		this.goal = goal;
+		
+		return startSearch((AStarNode)start,(AStarNode)goal);
+			
+	}
+	
+	private List<AStarNode> startSearch(AStarNode start, AStarNode goal) {
+		// Add the start node to the open list, initialize it.
+		start.setParent(null); // make sure it does not have any parent defined.
+		start.estimatedCostToGoal = start.getEstimatedCostTo(goal,start);
+		start.costFromStart = 0;
+		open.add(start);
+
+		// Go through all the items in the open storage.
+		int order = 0; // defines the order of which the nodes were visited (used in gui visuals)
+		while (!open.isEmpty()) {
+			// Let's retrieve the first item from the storage.
+			AStarNode node = (AStarNode) open.removeFirst();
+			node.setVisited(true);
+			node.setVisitOrder(order++);
+
+			// Check if we found the goal.
+			if (node == goal) {
+				return constructPath(node);
+			} else {
+				// Let's go through all the neighbours of this node.
+				Iterator<AStarNeighbour> i = node.getNeighbours().iterator();
+				while (i.hasNext()) {
+					AStarNeighbour neighbour = (AStarNeighbour) i.next();
+					AStarNode neighbourNode = (AStarNode)neighbour.getNode();
+					
+					/*
+					 * We do not want to visit blocked neighbours, so we skip
+					 * them. Also, if the neighbour node is neither in the
+					 * closed and the open storage then add it to the open
+					 * storage, and set it's parent.
+					 */
+					if(!neighbourNode.isBlocked()) {
+						Location location = neighbour.getLocation();
+
+						float costFromStart = node.costFromStart + node.getCost(neighbourNode, location);
+						boolean inClosed = closed.contains(neighbourNode);
+						boolean inOpen = open.contains(neighbourNode);
+						
+						if ((!inOpen && !inClosed) || costFromStart < neighbourNode.costFromStart) {
+							neighbourNode.setParent(node);
+							neighbourNode.costFromStart = costFromStart;
+							neighbourNode.estimatedCostToGoal = neighbourNode.getEstimatedCostTo(goal,start);
+	
+							if (inClosed) {
+								closed.remove(neighbourNode);
+							}
+							if (!inOpen) {
+								open.add(neighbourNode);
+							}
+						}
+					}
+				}				
+				closed.add(node);
+			}
+		}
+
+		return null;
+	}
+
+
+	/**
+	 * Constructs a path from a Node through any number of 
+	 * Nodes to the goal Node.
+	 * 
+	 * @param node the Node that contains the path back from the goal to the start.
+	 * @return a List containing the path of Nodes to travel to reach goal.
+	 */
+	protected List<AStarNode> constructPath(AStarNode node) {
+		LinkedList<AStarNode> path = new LinkedList<AStarNode>();
+		
+		while (node.getParent() != null) {
+			node.setPartOfPath(true);
+			path.addFirst(node);
+			node = node.getParent();
+		}
+		
+		return path;
+	}
+	
+	/**
+	 * Resets all the nodes from the previous search.
+	 *
+	 */
+	protected void resetNodes() {
+		if(goal != null) goal.reset();
+		if(start != null) start.reset();
+				
+		if(open != null) {
+			open.clear();
+		}
+		// Go through all the items in the open storage.
+		if(closed != null) {
+			closed.clear();
+		}
+	}
+	
+}
diff --git a/src/ei/game/scene/GameEntity.java b/src/ei/game/scene/GameEntity.java
index b8d3c86..9ff3e64 100644
--- a/src/ei/game/scene/GameEntity.java
+++ b/src/ei/game/scene/GameEntity.java
@@ -1,6 +1,7 @@
 package ei.game.scene;
 
 import ei.engine.scene.Entity;
+import ei.engine.scene.Node;
 
 public abstract class GameEntity{
 	private int life;
@@ -47,4 +48,6 @@ public abstract class GameEntity{
 	}
 	
 	public abstract void update();
+	
+	public abstract Entity getNode();
 }
diff --git a/src/ei/game/scene/Map.java b/src/ei/game/scene/Map.java
index 3ad6f92..1becce1 100644
--- a/src/ei/game/scene/Map.java
+++ b/src/ei/game/scene/Map.java
@@ -132,7 +132,7 @@ public class Map {
 		for(int i=0; i<width ;i++){
 			for(int j=0; j<hight ;j++){
 				if(map[i][j] != null){
-					System.out.println("LOL: "+i+" "+j);
+					System.out.println("Unit("+map[i][j].getNode().getName()+"): "+i+" "+j);
 				}
 			}
 		}
diff --git a/src/ei/game/scene/units/Unit.java b/src/ei/game/scene/units/Unit.java
index fc1c53c..4fca1b7 100644
--- a/src/ei/game/scene/units/Unit.java
+++ b/src/ei/game/scene/units/Unit.java
@@ -7,7 +7,6 @@ import ei.engine.math.Vector2f;
 import ei.engine.math.Vector2i;
 import ei.engine.math.Vector3f;
 import ei.engine.scene.Node;
-import ei.engine.scene.Sprite;
 import ei.game.algo.AStar;
 import ei.game.algo.AStarNode;
 import ei.game.gamestate.InGameState;
@@ -18,6 +17,7 @@ import ei.game.scene.weapons.Weapon;
 /**
  * The Unit class, handles the units in the game.
  * @author Jesper Lundin
+ * @author Ziver koc
  *
  */
 public abstract class Unit extends GameEntity{
@@ -61,12 +61,15 @@ public abstract class Unit extends GameEntity{
 		return unitNode;
 	}
 	/**
-	 * Sets the pos of the unit whitout removing it from the old
+	 * Changes the pos of the unit in the map
 	 * 
 	 * @param x The x pos to move to
 	 * @param y The y pos to move to
 	 */
 	public void setPos(int x, int y) {
+		if(oldPos!=null) {
+			InGameState.getMap().removePos(oldPos.getX(), oldPos.getY());
+		}
 		oldPos = new Vector2i(x, y);
 		InGameState.getMap().setPos(this, x, y);		
 	}
@@ -78,24 +81,22 @@ public abstract class Unit extends GameEntity{
 	 * @param y The y pos to move to
 	 */
 	public void move(int x, int y) {
-		if(oldPos!=null) {
-			InGameState.getMap().removePos(oldPos.getX(), oldPos.getY());
+		path = (LinkedList<AStarNode>) new AStar().startSearch(oldPos,new Vector2i(x,y));
+
+		if(path != null && !path.isEmpty()){	
+			AStarNode temp = path.poll();
+			moveTo = Map.getPixelByPos(temp.getX(), temp.getY());
+			setPos(temp.getX(), temp.getY());
 		}
-		setPos(x, y);
-		path = (LinkedList) new AStar().findPath(new AStarNode(oldPos,null), new AStarNode(new Vector2i(x,y),null));
-		moveTo = Map.getPixelByPos((int)x, (int)y);
-		oldPos = new Vector2i(x, y);
 	}
 	
 	/**
 	 * Updating the unit
 	 */
 	public void update() {
-		if(moveTo!=null) {
+		if(moveTo != null) {
 			Vector2i moveVect = Map.getPosByPixel(moveTo.getX(), moveTo.getY());
 			Vector2i  currentVect = Map.getPosByPixel(unitNode.getLocation().getX(), unitNode.getLocation().getY());
-			System.out.println("going to: "+moveVect);
-			System.out.println("from: "+currentVect);
 			Vector3f lastRotation = new Vector3f(0, 0, 0);
 			
 			//The rotation animation is done here.
@@ -131,11 +132,17 @@ public abstract class Unit extends GameEntity{
 				unitNode.getLocation().add(0f, -1.5f, 0f);
 			}
 			
-			if(moveTo.getX() == unitNode.getLocation().getX() 
-					&& moveTo.getY() == unitNode.getLocation().getY()) {
-				if(!path.isEmpty()){
-					Vector2i temp = path.poll().getPos();
-					moveTo = Map.getPixelByPos(temp.getX(), temp.getY());
+			if(Math.abs(moveTo.getX() - unitNode.getLocation().getX()) < 2
+					&& Math.abs(moveTo.getY() - unitNode.getLocation().getY())< 2 ){
+				if(path != null && !path.isEmpty()){
+					AStarNode temp = path.poll();
+					if(InGameState.getMap().isPosEmpty(temp.getX(), temp.getY())){
+						moveTo = Map.getPixelByPos(temp.getX(), temp.getY());
+						setPos(temp.getX(), temp.getY());
+					}
+					else if(!path.isEmpty()){
+						move(path.getLast().getX(),	path.getLast().getY());
+					}
 				}
 				else{
 					moveTo = null;