PathFinding almost done

Haven't started debugging yet, and it doesn't return a path yet, bu
everything else shold be in place