reformulated optimal control problem, started on implementation