Implemented AStar pathfinding

But it isn't working yet ...