% ----------------------------------   NPCCON1.M    -------------------------------
%
%  This program simulates a predictive controller applied to a process
%  modelled by a nonlinear neural network model. 
%
%  Minimization of the predictive control criterion:
%
%                     N2                              Nu
%                    ---         ^        2           ---                    2
%       J(t,U)  =    >   (r(t+k)-y(t+k|t))   +  rho * >   (u(t+k-1)-u(t+k-2))
%                    ---                              --- 
%                    k=N1                             k=1
%
%  is done with a Quasi-Newton method using the BFGS-algorithm for
%  approximating the inverse Hessian. The algorithm is implemented with
%  the "soft" line search described in:
%                   K. Madsen: "Optimering uden bibetingelser." (in Dansih)
%                   IMM, Technical University of Denmark, 1984
%
%  The proposed algorithm is inspired by the considerations in:
%    J.E. Dennis & J.J More: "Quasi-Newton Methods, Motivation and Theory"
%    SIAM Review 19, pp. 46-89, 1977
%
%  All design parameters must be defined in the file 'npcinit.m'
%
%  Programmed by Magnus Norgaard, IAU/EI/IMM, Technical Univ. of Denmark
%  LastEditDate: Feb. 12, 1996


%----------------------------------------------------------------------------------
%-------------------         >>>  INITIALIZATIONS  <<<        ---------------------
%----------------------------------------------------------------------------------

%>>>>>>>>>>>>>>>>>>>>>>      READ VARIABLES FROM FILE       <<<<<<<<<<<<<<<<<<<<<<<
clear plot_a plot_b
global ugl
npcinit
eval(['load ' nnfile]);                % Load neural network


% >>>>>>>>>>>>>>>>>>>>>>>>   DETERMINE REGRESSOR STRUCTURE   <<<<<<<<<<<<<<<<<<<<<<   
na      = NN(1);                       % # of past y's to be used in TDL
nb      = NN(2);                       % # of past u's to be used in TDL
nk      = NN(3);                       % Time delay in system
d       = NN(3);                       % Time delay in addition to the usual 1 
N1      = d;                           % N1<>d not fully implemented yet. 
inputs  = na+sum(nb);                  % Total number of inputs to network
outputs = 1;                           % # of outputs is 1 (SISO system)
phi     = zeros(inputs,1);             % Initialize regressor vector


% >>>>>>>>>>>>>>>>>    DETERMINE STRUCTURE OF NETWORK MODEL     <<<<<<<<<<<<<<<<<<<
hidden   = length(NetDef(1,:));        % Number of hidden neurons
L_hidden = find(NetDef(1,:)=='L')';    % Location of linear hidden neurons
H_hidden = find(NetDef(1,:)=='H')';    % Location of tanh hidden neurons
y1       = zeros(hidden,N2-N1+1);      % Hidden layer outputs
yhat     = zeros(outputs,1);           % Network output


%>>>>>>>>>>>>>>>>>>>>>>>        INITIALIZE VARIABLES        <<<<<<<<<<<<<<<<<<<<<<
% Determine length of reference filter polynomials
nam = length(Am);
nbm = length(Bm);

% Initialization of past signals
maxlen = 5;
ref_old    = zeros(maxlen,1);
y_old      = zeros(N2,1);
yhat_old   = zeros(N2,1);
u_old      = zeros(maxlen,1);

% Initialization of constant gain PID parameters
if strcmp(regty,'pid'),
  B1 = K*(1+Ts*Wi/2);
  A1 = Ts*Wi;
  B2 = (2*Td+Ts)/(2*alf*Td+Ts);
  A2 = 2*Ts/(2*alf*Td+Ts);
  I1 = 0;
  I2 = 0;
  uimin = -10; uimax = 10;
end

% Miscellaneous initializations
df  = ones(hidden,1);
dUtilde_dU = eye(Nu);
dUtilde_dU(1:Nu-1,2:Nu)=dUtilde_dU(1:Nu-1,2:Nu)-eye(Nu-1);
dY_dU      = zeros(Nu,N2-N1+1);  % Initialize matrix of partial derivatives
y0         = 0;                  % The output up to time t<=0
u          = 0;                  % The controls up to time t<=0
up         = u(ones(Nu,1));      % Initial future controls
upmin      = up;
beta0      = 0.9;                % Associated with the line search
t = -Ts;
delta2     = delta*delta;
fighandle=progress;

% Initialization of Simulink system
if strcmp(simul,'simulink')
  eval(['[sizes,x0] = ' sim_model '([],[],[],0);']);
end


%>>>>>>>>>>>>>>>>    CALCULATE REFERENCE SIGNAL & FILTER IT     <<<<<<<<<<<<<<<<<<
if strcmp(refty,'siggener'),
  ref = zeros(samples+N2,1);
  for i = 1:samples+N2,
    ref(i) = siggener(Ts*(i-1),sq_amp,sq_freq,sin_amp,sin_freq,dc,sqrt(Nvar));
  end
elseif strcmp(refty,'none'),
  ref = zeros(samples+N2,1);
else
  eval(['ref = ' refty ';']);
  ref=ref(:);
  i=length(ref);
  if i>samples+N2,
    ref=ref(1:samples+N2);
  else
    ref=[ref;ref(i)*ones(samples+N2-i,1)];
  end
end
ref=filter(Bm,Am,ref);


% Initializations of vectors used for storing old data
ref_data    = ref(1:samples);
u_data      = zeros(samples,1);
y_data      = zeros(samples,1);
yhat_data   = zeros(samples,1);
t_data      = zeros(samples,1);
e_data      = zeros(samples,1);


% Data vectors used in control algorithm
% u_vec : First element is u(t-d+N1-nb+1), last element is u(t-d+N2)
% Index to time t is thus: tiu=d-N1+nb ("t index u_vec")
u_vec = u(ones(N2-N1+nb,1));
tiu   = d-N1+nb;
upi   = [1:Nu-1 Nu(ones(1,N2-d-Nu+2))];     % [1 2 ... Nu Nu ... Nu] 
uvi   = [tiu:N2-N1+nb];

% y_vec: First element is y(t-na), last element is y(t-1)
% Index to time t is: tiy=na+1
y_vec = y0(ones(na,1));
tiy   = na+1;

% yhat_vec: First element is yhat(t), last element is yhat(t+N2)
% Index to time t is: tiyh=1
yhat_vec = y0(ones(N2+1,1));
tiyh = 1;


%------------------------------------------------------------------------------
%-------------------         >>>   MAIN LOOP   <<<           ------------------
%------------------------------------------------------------------------------
for i=1:samples,
  t = t + Ts;
  
%>>>>>>>>>>>>>>>>>>>>>>>>    PREDICT OUTPUT FROM PLANT  <<<<<<<<<<<<<<<<<<<<<<<
  phi            = [y_vec(na:-1:1);u_old(d:d+nb-1)];
  h1             = W1(:,1:inputs)*phi + W1(:,inputs+1);  
  y1(H_hidden,1) = pmntanh(h1(H_hidden)); 
  y1(L_hidden,1) = h1(L_hidden);
  yhat           = W2(:,1:hidden)*y1(:,1) + W2(:,hidden+1);
  yhat_vec(tiyh) = yhat;


%>>>>>>>>>>>>>>>>>>>>>>>>    READ OUTPUT FROM PLANT     <<<<<<<<<<<<<<<<<<<<<<<
  if strcmp(simul,'simulink')
    utmp=[t-Ts,u_old(1);t,u_old(1)];
    [time,x0,y] = rk45(sim_model,[t-Ts t],x0,[1e-3 Ts/2000 Ts/50 0 0 2],utmp);
    x0 = x0(length(x0),:)';
    y  = y(length(y),:)';
  elseif strcmp(simul,'matlab')
    ugl = u_old(1);
    [time,x] = ode45(mat_model,t-Ts,t,x0);
    x0 = x(length(time),:)';
    eval(['y = ' model_out '(x0);']);
  elseif strcmp(simul,'nnet')
    y = yhat;
  end

 
  %--------------------------------------------------------------------------------
  %----------     >>>  FIND NEW CONTROL SIGNAL BY OPTIMIZATION    <<<    ----------
  %--------------------------------------------------------------------------------
  upmin0   = upmin([2:Nu Nu]);
  einitval = eval(initval);     % Evaulate inival string
  for tr=1:length(einitval),
    up=upmin0;                  % Initial value for numerical search for a new u  
    up(Nu)=einitval(tr);
    u_vec(uvi) = up(upi);  
    dw = 1;                     % Flag specifying that up is new
    lambda = 0.1;               % Initialize Levenberg-Marquardt parameter
  
  
  %>>>>>>>>>>>>>>> COMPUTE PREDICTIONS FROM TIME t+N1 TO t+N2 <<<<<<<<<<<<<<<<
    for k=N1:N2,
      %----- Determine prediction yhat(t+k) -----
      phi              = [yhat_vec(tiyh+k-1:-1:tiyh+k-min(k,na)) ; ...
               y_vec(tiy-1:-1:tiy-max(na-k,0)) ; u_vec(tiu-d+k:-1:tiu-d+1-nb+k)];
      h1               = W1(:,1:inputs)*phi + W1(:,inputs+1);  
      y1(H_hidden,k-N1+1) = pmntanh(h1(H_hidden)); 
      y1(L_hidden,k-N1+1) = h1(L_hidden);
      yhat_vec(tiyh+k) = W2(:,1:hidden)*y1(:,k-N1+1) + W2(:,hidden+1);
    end
  
  
  %>>>>>>>>>>>>>>>>>>>>>>    EVALUATE CRITERION    <<<<<<<<<<<<<<<<<<<<<<
    duvec = u_vec(tiu:tiu+Nu-1)-u_vec(tiu-1:tiu+Nu-2);
    evec  = ref(i+N1:i+N2) - yhat_vec(tiyh+N1:tiyh+N2);
    J = evec'*evec + rho*(duvec'*duvec);
    
    
  %>>>>>>>>>>>>>>>>>>>>>>>>      DETERMINE dyhat/du       <<<<<<<<<<<<<<<<<<<<<<<<<
    for k=N1:N2
      % tanh'(x)
      df(H_hidden)  = (1-y1(H_hidden,k-N1+1).*y1(H_hidden,k-N1+1));
      for l=0:min(k-d,Nu-2)
         %                                 min(k-d-l,na)
         %                                     ---        dy(t+k-1)
         % h(k,l,j) =    w              +      >   w      --------
         %                j,na+k-d-l+1         ---   j,i   du(t+l)
         %                                     i=1
         % 
         imax1 = min(k-d-l,na);
         if l>=k-d-nb+1,
          if imax1>=1, 
            hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)'...
                          + W1(:,na+k-d-l+1);;
          else
            hj_vec=W1(:,na+k-d-l+1);
          end
         else
           hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)';
         end
    
         %          hidden     
         % dy(t+k)   ---
         % ------- =  >  W  * f'(y1(j)) * h(k,l,j)
         % du(t+l)   ---  j
         %           j=1
         dY_dU(l+1,k-N1+1)  = W2(1:hidden)*(df.*hj_vec);
      end
    
      if k>=Nu
         l=Nu-1;
         %       min(k-d-Nu+2,nb)          min(k-d-l,na)
         %             ---                    ---        dy(t+k-1)
         % h(k,l,j) =  >   w           +      >   w      --------
         %             ---  j,na+i            ---   j,i   du(t+l)
         %             i=1                    i=1
         % 
         imax1 = min(k-d-l,na);
         imax2 = min(k-d-Nu+2,nb);
         if imax2>1,
           if k==Nu,
             hj_vec = sum(W1(:,na+1:na+imax2)')';
           else
             hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)'...
                                                    + sum(W1(:,na+1:na+imax2)')';
           end
         else
           if k==Nu,
             hj_vec = W1(:,na+1:na+imax2);
           else
             hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)'...
                                                    + W1(:,na+1:na+imax2);
           end
         end
                                                
                                                    
         %          hidden     
         % dy(t+k)   ---
         % ------- =  >  W  * f'(y1(j)) * h(k,l,j)
         % du(t+l)   ---  j
         %           j=1     
         dY_dU(l+1,k-N1+1)  = W2(1:hidden)*(df.*hj_vec);
       end 
     end

  %>>>>>>>>>>>>>>>>>>>>>>>>>>>>    DETERMINE dJ/du     <<<<<<<<<<<<<<<<<<<<<<<<<<<<
  dJdu   = 2*(-dY_dU*evec + rho*(dUtilde_dU*duvec));


  %>>>>>>>>>>>>>>>>>>>>>>    DETERMINE INVERSE HESSIAN    <<<<<<<<<<<<<<<<<<<<<<<<<
  B = eye(Nu);                  % Initialize Hessian to I


  %>>>>>>>>>>>>>>>>>>>>>>>     BEGIN SEARCH FOR MINIMUM      <<<<<<<<<<<<<<<<<<<<<<    
  for m = 1:maxiter,            % Max. number of iteration
  
  
    %>>>>>>>>>>>>>>>>>>>>>>>   DETERMINE SEARCH DIRECTION   <<<<<<<<<<<<<<<<<<<<<<<
    f = -B*dJdu;

     
     %===============  DETERMINE STEP SIZE BY SIMPLE LINE SEARCH  =================
     mu = 1; b1 = 0; b2 = 1; beta = beta0; Jb1 = J; Gb1 = dJdu;

  
     while 2>1   
       %==============  COMPUTE PREDICTIONS FROM TIME t+N1 TO t+N2  ===============
       up_mu = up + mu*f;                % A priori iteration
       u_vec(uvi) = up_mu(upi);          % Insert updated controls
     
       %----- Determine prediction yhat(t+k,up_mu) -----
       for k=N1:N2,
         phi              = [yhat_vec(tiyh+k-1:-1:tiyh+k-min(k,na)) ; ...
                  y_vec(tiy-1:-1:tiy-max(na-k,0)) ; u_vec(tiu-d+k:-1:tiu-d+1-nb+k)];