Path finding

First implementation of A* path finding algorithm.