new architecture stable on non-replanning tasks
cleaned up, fixed replanning, fixed wrong starting position