Pathfinding works, calculation doesn't