Added unit tests for the AStar code to make sure we can refactor safely and that it produces correct paths