Fixed a pathfinding bug

Turns out i!=j needed to be !(i == 0 && j == 0)
This one was pretty dumb
Made distances a double for more realistic paths

Now there is a problem with an infinate loop