WIP

Working on path finding algorithm. Construction of search space and
helper classes for A* searching is done. Currently working on
constructing the path.