Pathfinding works

Pathfinding works, code needs cleaned up