Improve algorithm to determine forced neighbors

* Cleaned up code and refactored
* TODO: refactor more
* TODO: debug how forced neighbors are chosen... still not quite right
* TODO: expand final path in JPS