Pathfinding works, working on optimization