Forgot to reverse arguments for some functions
Add path cost function and organize hasPath