Added A* algorithm with Manhattan heuristic

Check if counting cost is ok.
Fix compare function on map so it finds correct elements...