Cleaned up path-finding process
Completed all the TODO tasks I marked earlier. i.e. you could call this
version 0.1