First commit, pathfinding works