function u=NMPC_with_cons(x,u0,w,par,const)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                         %
%               function u=NMPC_with_cons(x,u0,w,par)                     %
%                                                                         %
%   This function is a Nonlinear Predictive Controller based on           %
% decomposition principle. The controller considers requirements on       % 
% system output deviation penalization, system input time differential    %
% penalization and also system input deviation from optimal values. The   %
% script returns a column vector of system inputs on horizon.             %
%                                                                         %
%   Input:  x   - state vector at time instant k                          %                            
%           u0  - presumptive system inputs on horizon                    %
%           w   - vector of desired outputs at time k+1 to k+N            %
%           par - parametrs and etc. (structure)                          %
%                                                                         %
%           par.M   - matrix of controller                                %
%           par.H   - forced output prediction matrix                     %          
%           par.Rv  - weighting matrix (prediction error)                 %
%           par.Q2  - weighting matrix (inputs deviation from optimum)    %
%           par.Q   - weighting matrix (time difference)                  %
%           par.Ev  - weighting matrix (time difference)                  %
%           par.Ev2 - weighting matrix (time difference)                  %
%           par.Ts  - samplingtime                                        %
%           par.mni - number of iterations                                %
%           par.nam - nonlinear math. model file name                     %
%           par.C   - state matrix                                        %
%           par.uopt- vector of optimal inputs (nu*N x 1)                 %
%           par.ulin- inputs linearization points (vector)                %
%           par.xlin- states linearization points (vector)                %
%           par.S  - constraints - left side                              %
%           par.s  - constraints - right side                             %
%           par.xc - filtering of states occuring in constraints          %
%                    matrix [1 0 ... 0;0 1 ... 0;...]                     %
%                    1 occuring, 0 not occuring                           %
%           const  - Parameters of the mathematical model                 %
%                                                                         %
%   Output: u - vector of system inputs on horizon                        %
%                                                                         %
% Author: Pavel Skrabanek                         Last update: 10.01.2011 %
% Version: 2.0                                                            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ny=size(par.C,1);                   % Number of system outputs
hor=length(w)/ny;                   % Length of horizon
nu=length(u0)/hor;                  % Number of system inputs  
nxc=size(par.xc,1);                 % Number of states which occure in constraints

u0=[u0(nu+1:end);u0(end-nu+1:end)];

t=[0 par.Ts/2 par.Ts];              % Time vector
options = odeset('RelTol',1e-5);    % Simulation setting

xxc=zeros(nxc*hor,1);
yfr=zeros(ny*hor,1);
dy=zeros(9,1);

for z=1:par.mni
    xx=x;
    for i=1:hor                       % Creating of free response vector
        y0=[xx;(u0(1+(i-1)*nu:i*nu))];% Initial condition (for free respons)
        [~,Y]=ode45(@(t,y0) par.nam(t,y0,const,dy),t,y0,options);%Free respons at time t+n*Ts
        xx=Y(3,1:end-nu)';
        xxc(1+nxc*(i-1):i*nxc,1)=par.xc*xx;
        yfr(1+(i-1)*ny:i*ny,1)=par.C*xx;        
    end  
    s=par.s+[xxc;-xxc;u0;-u0];
    [u0f,Ac,ac,Nc,nc]=initial_condition(par.S,s,u0*0);       
    m=par.H'*par.Rv*(yfr-w)+par.Q2*(u0-par.uopt)+par.Ev'*par.Q*par.Ev2*u0;
    deltau=qpas(u0f,par.M,m,Ac,ac,Nc,nc);
    u0=u0+deltau;                 % Corected system input      
end

u=u0;