function [solver,init] = BuildScenMPC(par,f,p_case,nR)
import casadi.*


par.T = par.N*par.Ts;
par.TsMPC = par.Ts;

lbx = par.lbx;      ubx = par.ubx;
lbu = par.lbu(1);
ubu = par.ubu(1);
nx = par.nx;        nd = par.nd;        nu = par.nu;
dx0 = par.dx0;      u0 = par.u0(1);

M = numel(p_case);
nS = M^nR;

a = 1;
ac = 1;
for i = 1:par.N
    for j = 1:nS
        
        nonant(i,j) = a;
        
        if i == 1
            a=1;
            
        else if i<=nR
                ac = ac+1;
                if ac>M
                    a = a+1;
                    ac = 1;
                end
            else
                a =NaN;
            end
        end
        
        
    end
end
nonant = nonant';

%% Direct Collocation

% Degree of interpolating polynomial
d = 3;

% Get collocation points
tau_root = [0, collocation_points(d, 'radau')];

% Coefficients of the collocation equation
C = zeros(d+1,d+1);

% Coefficients of the continuity equation
D = zeros(d+1, 1);

% Coefficients of the quadrature function
B = zeros(d+1, 1);

% Construct polynomial basis
for j=1:d+1
    % Construct Lagrange polynomials to get the polynomial basis at the collocation point
    coeff = 1;
    for r=1:d+1
        if r ~= j
            coeff = conv(coeff, [1, -tau_root(r)]);
            coeff = coeff / (tau_root(j)-tau_root(r));
        end
    end
    % Evaluate the polynomial at the final time to get the coefficients of the continuity equation
    D(j) = polyval(coeff, 1.0);
    
    % Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
    pder = polyder(coeff);
    for r=1:d+1
        C(j,r) = polyval(pder, tau_root(r));
    end
    
    % Evaluate the integral of the polynomial to get the coefficients of the quadrature function
    pint = polyint(coeff);
    B(j) = polyval(pint, 1.0);
end

%% Build NLP solver

% empty nlp
w   = {};
w0  = [];
lbw = [];
ubw = [];
J   = 0;

g   = {};
lbg = [];
ubg = [];

for js = 1:nS
    
    % initial conditions for each scenario
    X0  = MX.sym('X0',nx);
    w   = {w{:}, X0};
    lbw = [lbw;lbx];
    ubw = [ubw;ubx];
    w0  = [w0; dx0];
    
    % Formulate NLP
    Xk  = X0;
    Xkj = {};
    
    
    
    switch js
        case 1
            p1      = MX.sym('p1',nd);
            U01     = MX.sym('U01',nu);
            X01_par = MX.sym('X01_par',nx);
            
            g   = {g{:},X0 - X01_par};
            lbg = [lbg;zeros(nx,1)];
            ubg = [ubg;zeros(nx,1)];
            
            Uk_prev = U01;
        case 2
            p2      = MX.sym('p2',nd);
            U02     = MX.sym('U02',nu);
            X02_par = MX.sym('X02_par',nx);
            
            g   = {g{:},X0 - X02_par};
            lbg = [lbg;zeros(nx,1)];
            ubg = [ubg;zeros(nx,1)];
            
            Uk_prev = U02;
            case 3
            p3      = MX.sym('p3',nd);
            U03     = MX.sym('U03',nu);
            X03_par = MX.sym('X03_par',nx);
            
            g   = {g{:},X0 - X03_par};
            lbg = [lbg;zeros(nx,1)];
            ubg = [ubg;zeros(nx,1)];
            
            Uk_prev = U03;
           
    end
    
    for k = 0:par.N-1
        
        Uk  = MX.sym(['U_' num2str(k) '_' num2str(js)],nu);
        w   = {w{:},Uk};
        lbw = [lbw;lbu];
        ubw = [ubw;ubu];
        w0  = [w0;u0];
        
        Xkj = {};
        
        for j = 1:d
            Xkj{j} = MX.sym(['X_' num2str(k) '_' num2str(j) '_' num2str(js)],nx);
            w   = {w{:},Xkj{j}};
            lbw = [lbw;lbx];
            ubw = [ubw;ubx];
            w0  = [w0; dx0];
        end
        
        % Loop over collocation points
        Xk_end  = D(1)*Xk;
        
        for j = 1:d
            % Expression for the state derivative at the collocation point
            xp  = C(1,j+1)*Xk;  % helper state
            for r = 1:d
                xp = xp + C(r+1,j+1)*Xkj{r};
            end
            switch js
                case 1
                    [fj,qj] =  f(Xkj{j},vertcat(Uk,p1));
                case 2
                    [fj,qj] =  f(Xkj{j},vertcat(Uk,p2));
                     case 3
                    [fj,qj] =  f(Xkj{j},vertcat(Uk,p3));
                   
            end
            
            g   = {g{:},par.TsMPC*fj-xp};  % dynamics and algebraic constraints
            lbg = [lbg;zeros(nx,1)];
            ubg = [ubg;zeros(nx,1)];
            
            % Add contribution to the end states
            Xk_end  = Xk_end + D(j+1)*Xkj{j};
            
            J   = J + (B(j+1)*qj*par.TsMPC ); %
        end
        J   = J  + 0.0.*sum((Uk_prev - Uk).^2) ;
        
        Uk_prev = MX.sym(['Uprev_' num2str(k+1) '_' num2str(js)],nu);
        Uk_prev = Uk;
        
        % New NLP variable for state at end of interval
        Xk      = MX.sym(['X_' num2str(k+1)  '_' num2str(js)], nx);
        w       = {w{:},Xk};
        lbw     = [lbw;lbx];
        ubw     = [ubw;ubx];
        w0      = [w0; dx0];
        
        % Shooting Gap constraint
        g   = {g{:},Xk_end-Xk};
        lbg = [lbg;zeros(nx,1)];
        ubg = [ubg;zeros(nx,1)];
        
        if k < nR
            u_ant{js,k+1} = MX.sym(['u_ant_' num2str(k) '_' num2str(js)],nu);
            u_ant{js,k+1} = Uk;
        end
    end
end

%Add Non-anticipativity constraints
for k = 1:nR
    for js = 1:nS-1
        if ~isnan(nonant(js,k))
            if nonant(js) == nonant(js+1)
                g = {g{:},(u_ant{js,k} - u_ant{js+1,k})};
                lbg = [lbg;zeros(nu,1)];
                ubg = [ubg;zeros(nu,1)];
            end
        end
    end
end

% create and solve NLP solver
% Definition of the options for casadi
opts = struct('warn_initial_bounds',false, ...
    'print_time',false, ...
    'ipopt',struct('print_level',1) ...
    );

nlp     = struct('x',vertcat(w{:}),'p',vertcat(p1,U01,X01_par,p2,U02,X02_par,p3,U03,X03_par),'f',J,'g',vertcat(g{:}));
solver  = nlpsol('solver','ipopt',nlp,opts);

init.w0 = w0;
init.lbw = lbw;
init.ubw = ubw;
init.lbg = lbg;
init.ubg = ubg;
