Path finding now working

But the efficiency sucks donkey dick, gotta improve that