function [tt,xx,uu] = mpc_ExampleBGW(x0,N,T,myprint)
% motivating example from the paper
%    A. Boccia, L. Gruene, K. Worthmann,
%    Stability and feasibility of state constrained MPC 
%    without stabilizing terminal constraints
%    Preprint, University of Bayreuth, 2013
%
% needs the routine nmpc.m from 
% http://numerik.mathematik.uni-bayreuth.de/~lgruene/nmpc-book/matlab_nmpc.html
%
% example call: mpc_ExampleBGW([0.5 0.999],7,1,2);

    close all;
    %[0.5 0.09] --> 6
    %[0.5 0.9] --> 6
    %[0.5 0.99] --> 6
    %[0.5 0.999] --> 7
    %[0.5 0.9999] --> 10
    %[0.5 0.99999] --> 13
    %[0.5 0.999999] --> 16

    addpath('./nmpcroutine');

    mpciterations = 50;
%    N             = 5;
%    T             = 1;
    t0            = 0.0;
    %x0            = [0.5];
    u0            = zeros(2,N);
    tol_opt       = 1e-12;
    opt_option    = 0;
    iprint        = 5;
    type          = 'difference equation';
    atol_ode_real = 1e-12;
    rtol_ode_real = 1e-12;
    atol_ode_sim  = 1e-4;
    rtol_ode_sim  = 1e-4;

    global gconstr;
    gconstr = 1;
    
    global gN;
    gN = N;
    
    global gprint;
    gprint = myprint;
    
    global guu;
    guu = [];
    
    global gtt;
    gtt = [ t0 ];
    
    global gxx;
    gxx = [ x0 ];
    
    nmpc(@runningcosts, @terminalcosts, @constraints, ...
         @terminalconstraints, @linearconstraints, @system, ...
         mpciterations, N, T, t0, x0, u0, ...
         tol_opt, opt_option, ...
         type, atol_ode_real, rtol_ode_real, atol_ode_sim, rtol_ode_sim, ...
         iprint, @printHeader, @printClosedloopData, @plotTrajectories);

    tt = gtt;
    xx = gxx;
    uu = guu;
    
    rmpath('./nmpcroutine');
end







%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definition of the NMPC functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cost = runningcosts(t, x, u)
    lambda = 100;
    cost = lambda*x(1)^2 + x(2)^2 + u(1)^2 + lambda*u(2)^2;
end

function cost = terminalcosts(t, x)
    cost = 0.0;
end

function [c,ceq] = constraints(t, x, u)
    global gconstr;
    
    c(1) = -u(1)+u(2)-1;
    c(2) = +u(1)+u(2)-1;
    c(3) = -u(1)-u(2)-1;
    c(4) = +u(1)-u(2)-1;
    c(5) = x(1)-100;
    c(6) = -x(1)-100;
    c(7) = x(2)-1;
    c(8) = -x(2)-1;
    ceq = [];
end

function [c,ceq] = terminalconstraints(t, x)
    global gconstr;
    
    c(1) = x(1)-100;
    c(2) = -x(1)-100;
    c(3) = x(2)-1;
    c(4) = -x(2)-1;
    c(5) = 0;
    c(6) = 0;
    c(7) = 0;
    c(8) = 0;
    %c = [];
    ceq = [];
end

function [A, b, Aeq, beq, lb, ub] = linearconstraints(t, x, u)
    %A   = [1 0; -1 0; 0 1; 0 -1];
    %b   = [100; 100; 1; 1];
    A = [];
    b = [];
    Aeq = [];
    beq = [];
    lb = [];
    ub = [];
end

function y = system(t, x, u, T)
    y(1) = x(1)+x(2)+u(1);
    y(2) = 2*x(2)+u(2);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definition of output format
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function printHeader()
    fprintf('   k  |       u1(k)       u2(k)        x(1)        x(2)    Time\n');
    fprintf('--------------------------------------------------\n');
end

function printClosedloopData(mpciter, u, x, t_Elapsed)
    
    fprintf(' %3d  | %+11.6f %+11.6f %+11.6f %+11.6f %+7.3f', mpciter, ...
            u(1,1), u(2,1), x(1), x(2), t_Elapsed);
       
end

function plotTrajectories(dynamic, system, T, t0, x0, u, ...
                          atol_ode, rtol_ode, type)
    global gN;
    global gprint;
    global gtt;
    global gxx;
    global guu;
    
    gtt = [gtt; t0+T];
    
    if (gprint>=1)
        figure(1);
        hold on;  
    end;
        x = x0;
        for k=1:gN
            [x,t_intermediate, x_intermediate] = ...
                dynamic(system, T, t0+k-1, x, u(:,k), ...
                             atol_ode, rtol_ode, type);
            if (k==1)
                if (gprint>=1)
                    plot(t_intermediate,x_intermediate(:,1),'-ob');
                    plot(t_intermediate,x_intermediate(:,2),'-or');
                end;
                gxx = [gxx; x];
                guu = [guu, u(1,1)];
            else
                if (gprint>=2)
                    plot(t_intermediate,x_intermediate(:,1),'--b');
                    plot(t_intermediate,x_intermediate(:,2),'--r');
                end;
            end 
        end
        if (gprint>=1)
            if (gprint>=2)
                title(['open loop trajectories (dashed) and closed loop trajectory (solid)']);
            else
                title(['closed loop trajectory (solid)']);
            end;
            xlabel('n');
            ylabel('x(n)');
            grid on;
        end;
        hold off;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%