Fix pathfinding and freeing memory