Not pathing right. Needs debugging and tweaking. Currently finds a path, but not the optimal path.