clear all;

EXPORT  = 1;

T  = 1.5;
N  = 15;
Ts = T/N;

%% Differential Equation (cf. R. Quirynen et al., Multiple Shooting in a Microsecond)
mu = 0.5;
DifferentialState x1 x2;
Control u;

f = [   dot(x1) == x2 + u*(mu+(1-mu)*x1  )  ; ...
        dot(x2) == x1 + u*(mu-4*(1-mu)*x2)  ];
    
nx = length(diffStates);
nu = length(controls);

%% SIMexport
acadoSet('problemname', 'sim');

sim = acado.SIMexport( Ts );
sim.setModel(f);
sim.set( 'INTEGRATOR_TYPE',             'INT_RK4'   );

if EXPORT
    sim.exportCode('export_SIM');
    cd export_SIM
    make_acado_integrator('../integrate_chen_allgoewer')
    cd ..
end

%% MPCexport
acadoSet('problemname', 'mpc');

ocp = acado.OCP( 0.0, T, N );

Q = 2*eye(2);
R = 0.1;
P = [10.605 -9.395; -9.395 10.605];
ocp.minimizeLSQ( blkdiag(Q,R), [x1; x2; u] );
ocp.minimizeLSQEndTerm( P, [x1; x2] );

ocp.subjectTo( -2 <= u <= 2 );
ocp.subjectTo( 'AT_END', [x1;x2].'*P*[x1;x2] <= 0.7 );
ocp.setModel(f);

mpc = acado.OCPexport( ocp );
mpc.set( 'HESSIAN_APPROXIMATION',       'GAUSS_NEWTON'         );
mpc.set( 'DISCRETIZATION_TYPE',         'MULTIPLE_SHOOTING'     );
mpc.set( 'SPARSE_QP_SOLUTION',          'FULL_CONDENSING_N2'    );
mpc.set( 'INTEGRATOR_TYPE',             'INT_RK4'               );
mpc.set( 'NUM_INTEGRATOR_STEPS',        N                     );

if EXPORT
    mpc.exportCode( 'export_MPC' );
    global ACADO_;
    copyfile([ACADO_.pwd '/../../external_packages/qpoases'], 'export_MPC/qpoases');
    
    cd export_MPC
    make_acado_solver('../acado_MPCstep')
    cd ..
end

%% PARAMETERS SIMULATION 
Tf = 6;
X0 = [-0.7 -0.85];
input.x = repmat(X0,N+1,1);
input.u = zeros(N,1);

input.y = zeros(N,3);
input.yN = zeros(1,2);

%% SIMULATION LOOP
display('------------------------------------------------------------------')
display('                      MPC Loop'                                    )
display('------------------------------------------------------------------')

iter = 0; 
time = 0; 
controls_MPC = [];  
state_sim = X0;

figure(1);clf;hold on
pred_plot = plot(0,0,'rx','MarkerSize',10);
state_plot = plot(state_sim(1:end-1,2),state_sim(1:end-1,1),'ro','MarkerSize',10);

while time(end) < Tf;
    
    % Solve NMPC OCP
    input.x0 = state_sim(end,:);
    output = acado_MPCstep(input);
    
    % Save the optimal first control
    controls_MPC = [controls_MPC; output.u(1,:)];
    
    % Shift the trajectories:
    input.x = [output.x(2:end,:); output.x(end,:)];
    input.u = [output.u(2:end,:); output.u(end,:)];
    
    % Simulate system
    sim_input.x = state_sim(end,:).';
    sim_input.u = output.u(1,:).';
    states = integrate_chen_allgoewer(sim_input);
    state_sim = [state_sim; states.value.'];
        
    % Time step
    iter = iter+1;
    nextTime = iter*Ts; 
    cpuTime = output.info.cpuTime;
    disp(['current simulation time: ' num2str(nextTime) '   ' char(9) ' (RTI step: ' num2str(round(cpuTime*1e6)) ' us)'])
    time = [time nextTime];
    
    hold on
    delete(pred_plot);
    ref_plot = plot(0,0,'rx','MarkerSize',10);
    pred_plot = plot(output.x(:,2),output.x(:,1),'k','LineWidth',3);
    state_plot = plot(state_sim(1:end-1,2),state_sim(1:end-1,1),'ro','MarkerSize',10);
    legend([ref_plot, pred_plot, state_plot],'reference','prediction','closed loop trajectory')
    xlabel('x2')
    ylabel('x1')

    xlim([-1 1])
    ylim([-1 1])
    grid on
    pause(Ts)
end


