# c.load_covariance_prior()
# c.set_state_covariance()
# c.regen_objective_fun()
# Update prior-state
# c.set_prior_state_from_prior_mhe()
#
# c.print_r_mhe()
# Compute the controls
# c.initialize_olnmpc(c.d1, "real")
# c.load_init_state_nmpc(src_kind="state_dict", state_dict="real") # for good measure
# # if i == 5:
# # with open("somefilc.txt", "w") as f:
# # c.olnmpc.R_nmpc.display(ostream=f)
# # c.olnmpc.Q_nmpc.display(ostream=f)
# # f.close()
#
# stat_nmpc = c.solve_d(c.olnmpc, skip_update=False)
# if stat_nmpc != 0:
# stat_nmpc = c.solve_d(c.olnmpc, stop_if_nopt=True, skip_update=False, iter_max=300)
# c.olnmpc.write_nl()
# c.update_u(c.olnmpc)
c.print_r_nmpc()
# c.shift_mhe()
# c.shift_measurement_input_mhe()
c.curr_u["u1"] = value(c.d1.u1[1]) * 1.5
print(value(c.d1.Gb[1, 1, 1, 1]), value(c.d1.Tgb[1, 1, 1, 1]))
c.cycle_ics(plant_step=True)
c.plant_input_gen(c.d1, src_kind="dict")
# c.d1.u1[1] = value(c.d1.u1[1]) * 0.99
# c.solve_k_aug_nmpc()
# c.olnmpc.write_nl()
i = 1
k = 1
while k != 0:
if i == 1:
k = c.solve_d(c.olnmpc, max_cpu_time=60 * i)
else:
k = c.solve_d(c.olnmpc, max_cpu_time=60 * i, warm_start=True)
c.olnmpc.ipopt_zL_in.update(c.olnmpc.ipopt_zL_out)
c.olnmpc.ipopt_zU_in.update(c.olnmpc.ipopt_zU_out)
i += 1
c.initialize_olnmpc(c.d1)
c.olnmpc.display(filename="somefile1.txt")
c.solve_d(c.olnmpc, max_cpu_time=60 * 60)
c.update_u(src=c.olnmpc)
c.cycle_ics()
c.plant_input_gen(c.d1, src_kind="dict")
c.solve_d(c.d1)
c.update_state_real()
c.initialize_olnmpc(c.olnmpc, fe=5)
c.solve_d(c.olnmpc, max_cpu_time=60 * 60)
c.create_predictor()
c.predictor_step(c.d1)
c.update_state_predicted()
c.update_u(src=c.olnmpc)
c.update_soi_sp_nmpc()
# c.print_r_nmpc()