Refactored find path, total cost and time into
functions. Yes I wrote fucking tests for them!
initial commit of binary, removed more
debugging from lib