Got Pathfinding going on using [A*](https://en.wikipedia.org/wiki/A*_search_algorithm)
Also added a Stack and minor refactoring