Bunch of bug fixes and changes. Replaced A* with BFS