% ***************************************************************************
% COPYRIGHT BY DAVE JEWELL AND T.E. MARLIN 1994
% FILNAME : SIM_2BY2.M
% Version 2.0
% LAST UPDATED : DECEMBER 1994
% DISCRIPTION OF FILE :
% THIS M-FILE IS ACCESSED FROM "twobytwo.m" AND IS THE MAIN BODY OF THE 
% SIMULATOR.  IT CALLS THE "initial.m','perform.m' IN ORDER TO 
% RUN A COMPLETE SIMULATION.
% ***************************************************************************
%clg;
% Indicates to the user that the simulation is running (on the screen)

disp('  ')
disp('*****  SIMULATION IN PROGRESS *****');

% ***************************************************************************
% ************* INITIALIZATION OF MATRICES AND PLANT COEFFICIENTS ***********
% ***************************************************************************
initial;                % CALLS SCRIPT M-FILE FOR PLANT AND CONTROLLER
% ***************************************************************************
% ****************************  TIME LOOP  **********************************
% ***************************************************************************
% TIME LOOP BEGINS HERE! A steady-state time is added to the simulation 
% to protect against dead times that will want to call previous values that are out of the range 
% of the matrix (zero or negative index).
% It is assumed that steady-state values are their initial values provided in the base case.

EXECUTION=1;       % INDICATES THAT A SIMULATION HAS BEEN RUN, 
                   % THEREFORE PLOTTING ALLOWED

if NOISEYN == 1    % CHECKS FOR MEASUREMENT NOISE ON SIGNAL
    for n=nstart:1:nmax             % time loop
       noise1(n)=BETA(1)*noise1(n-1)+Kn(1)*randn;
       noise2(n)=BETA(2)*noise2(n-1)+Kn(2)*randn;
    end                 % DISTYN CHECK
end                     % ITERATION LOOP

%****************************************************************************        
%*****************************    PLANT   *********************************
%****************************************************************************        
% these calc. are done in deviation variables with the inital values of the
% process variable being added to each output after the its calculation is 
% made. NOTE : the initial value must be subtracted from the final element 
% (mv) prior to calculation since we are using deviation variables.  This 
% eliminates the necessity of storing past values of the final control 
% element in deviation variables to be retrieved

for n=nstart:1:nmax             % time loop  

% UPDATES MANIPULATED VARIABLE AFFECTS
MV1=[mv1(n-1-gamma(1,1))-initmv(1) mv2(n-1-gamma(1,2))-initmv(2); ...
     mv1(n-1-gamma(2,1))-initmv(1) mv2(n-1-gamma(2,2))-initmv(2)];

MV2=[mv1(n-2-gamma(1,1))-initmv(1) mv2(n-2-gamma(1,2))-initmv(2); ...
     mv1(n-2-gamma(2,1))-initmv(1) mv2(n-2-gamma(2,2))-initmv(2)];

cvp=-a1.*cvp1-a2.*cvp2+b1.*MV1+b2.*MV2;
cvp2=cvp1;        % holds and updates the n-2 value
cvp1=cvp;         % holds and updates the n-1 value
       
% **************** DISTURBANCE EFFECTS ON OUTPUTS **********************

if DISTYN == 1    % CHECKS IF PROCESS HAS DISTURBANCE
       DIST1=[dist(n-1-Dgamma(1));dist(n-1-Dgamma(2))];
       DIST2=[dist(n-2-Dgamma(1));dist(n-2-Dgamma(2))];
       cvd=-Da1.*cvd1-Da2.*cvd2+Db1.*DIST1+Db2.*DIST2;
       cvd2=cvd1;      % holds and updates the n-2 value
       cvd1=cvd;       % holds and updates the n-1 value
end                     % DISTURBANCE CHECK

% ***************************************************************************
% **************** SUMMATION OF ALL EFFECTS ON OUTPUTS **********************
% ***************************************************************************

cv1(n)=sum(cvp(1,:))+cvd(1)+noise1(n)+initcv(1); 
cv2(n)=sum(cvp(2,:))+cvd(2)+noise2(n)+initcv(2); 

%****************************************************************************        
%   the plant simulation has been completed - now the controller is selected
%****************************************************************************
%  DETERMINE THE SET POINT FOR THIS TIME STEP

if n>=(round(config(6,1)/delt)+nstart-1)        % SETPOINT IMPLEMENTAION TIME
    sp1(n)=config(5,1);                         % UPDATES SETPOINT
else                                            % ELSE - USE INITIAL SET POINT
    sp1(n)=initsp(1);                           % UPDATES SETPOINT
end                                             % END - SETPOINT UPDATE
if n>=(round(config(6,2)/delt)+nstart-1)        % SETPOINT IMPLEMENTAION TIME
    sp2(n)=config(5,2);                         % UPDATES SETPOINT
else                                            % ELSE - USE INITIAL SET POINT
    sp2(n)=initsp(2);                           % UPDATES SETPOINT
end                                             % END - SETPOINT UPDATE

% CALCULATE THE ERRORS FOR THE CONTROLLED VARIABLES
error1(n)=sp1(n)-cv1(n);                        % ERROR CALCULATION
error2(n)=sp2(n)-cv2(n);                        % ERROR CALCULATION


%*****************************   CONTROL   **********************************
%  SELECT THE CONTROLLER - PID OR DMC

if conalgo == 1               %  SELECT PID CONTROL

%*********************  PID CONTROL SYSTEM HAS BEEN SELECTED  ***************
%****************************************************************************        
% ************************* FIRST CONTROLLER ********************************


if (config(2,1)==0)&(config(7,1)~=0)&(n==nSTEP-1)
    deltacontout1(n)=config(7,1);               % CHECKS FOR A PRC
else
    deltacontout1(n)=config(2,1)*(error1(n)-error1(n-1)+delt/config(3,1)* ...
    error1(n)-config(4,1)/delt*(cv1(n)-2*cv1(n-1)+cv1(n-2)));
end                                             % PRC CHECK

contout1(n)=deltacontout1(n)+contout1(n-1);     % CALCULATES CONTROLLER OUTPUT
%contout1(n)=min(contout1(n),contoutmax(1));     % this would be bound on delta
%contout1(n)=max(contout1(n),contoutmin(1));     % controller output, not desired

% ************************* SECOND CONTROLLER *******************************

if (config(2,2)==0)&(config(7,2)~=0)&(n==nSTEP-1) 
    deltacontout2(n)=config(7,2);               % CHECKS FOR A PRC
else
    deltacontout2(n)=config(2,2)*(error2(n)-error2(n-1)+delt/config(3,2)* ...
    error2(n)-config(4,2)/delt*(cv2(n)-2*cv2(n-1)+cv2(n-2)));
end                                             % PRC CHECK

contout2(n)=deltacontout2(n)+contout2(n-1);     % CALCULATES CONTROLLER OUTPUT
%contout2(n)=min(contout2(n),contoutmax(2));    % this would be bound on delta
%contout2(n)=max(contout2(n),contoutmin(2));    % controller output, not desired


%  Note that the bounds are allocated to the manipulated variable, not 
%  controller output.  The menu correctly represents the situation.
%  The variable name (e.g., contoutmax(i)) is misleading; it represents
%  the upper bound on manipulated variable i.

if (config(1,1)==1)
    mv1(n)=contout1(n)+initmv(1);
    mv2(n)=contout2(n)+initmv(2);
    mv1(n) = min(mv1(n),contoutmax(1));         % bound the controller
    mv1(n) = max(mv1(n),contoutmin(1));         % outputs
    mv2(n) = min(mv2(n),contoutmax(2));         % 
    mv2(n) = max(mv2(n),contoutmin(2));         % 
    contout1(n) = mv1(n) - initmv(1);           % reset contoutj to represent
    contout2(n) = mv2(n) - initmv(2);           % bounded value
  
elseif (config(1,1)==2)
    mv1(n)=contout2(n)+initmv(1);
    mv2(n)=contout1(n)+initmv(2);
    mv1(n) = min(mv1(n),contoutmax(1));         % bound the controller
    mv1(n) = max(mv1(n),contoutmin(1));         % outputs
    mv2(n) = min(mv2(n),contoutmax(2));         % 
    mv2(n) = max(mv2(n),contoutmin(2));         % 
    contout2(n) = mv1(n) - initmv(1);           % reset contoutj to represent
    contout1(n) = mv2(n) - initmv(2);           % bounded value


end
% ********************  END OF PID CONTROLLER CALCULATIONS  ****************
% **************************************************************************

elseif conalgo ==2               % SELECT DMC CONTROL

% ********************  DMC CONTROLLER HAS BEEN SELECTED  *******************
%****************************************************************************        
%****************** MODEL CALCULATED TO COMPARE WITH PLANT OUTPUT  **********
%****************************************************************************        
% these calc. are done in deviation variables with the inital values of the
% process variable being added to each output after the its calculation is 
% made. NOTE : the initial value must be subtracted from the final element 
% (mv) prior to calculation since we are using deviation variables.  This 
% eliminates the necessity of storing past values of the final control 
% element in deviation variables to be retrieved

% UPDATES MANIPULATED VARIABLE AFFECTS
MV1_sim=[mv1(n-1-gamma_m(1,1))-initmv(1) mv2(n-1-gamma_m(1,2))-initmv(2); ...
     mv1(n-1-gamma_m(2,1))-initmv(1) mv2(n-1-gamma_m(2,2))-initmv(2)];

MV2_sim=[mv1(n-2-gamma_m(1,1))-initmv(1) mv2(n-2-gamma_m(1,2))-initmv(2); ...
     mv1(n-2-gamma_m(2,1))-initmv(1) mv2(n-2-gamma_m(2,2))-initmv(2)];

cv_sim=-a1_m.*cv1_sim-a2_m.*cv2_sim+b1_m.*MV1_sim+b2_m.*MV2_sim;

%  store the last two values for the future prediction cacluation
cv1_pre = cv1_sim ;
cv2_pre = cv2_sim ;
%  now, step forward one time increment, storing previous values
cv2_sim=cv1_sim;        % holds and updates the n-2 value
cv1_sim=cv_sim;         % holds and updates the n-1 value

% **************** SUMMATION OF DELTA AND INITIAL     **********************
% ***************************************************************************

cvv1_sim(n) = sum(cv_sim(1,:)) + initcv(1);
cvv2_sim(n) = sum(cv_sim(2,:)) + initcv(2); 

%  ******************  CALCULATE CURRENT THE MODEL ERROR    ******************
%  do not store the model in history

ERRM1(n) = cv1(n) - cvv1_sim(n) ;
ERRM2(n) = cv2(n) - cvv2_sim(n) ;

%end

%  ******************  CALCULATE THE FUTURE PREDICTED ERROR  ******************
%  ******************  WITH NO FUTURE MV CHANGES             ******************
%  1) assume that the set point at this time is valid for the entire future horizon
%  2) recognize that the model has been solved up to the present for comparison with
%     current feedback measurement, these are cvv1_sim and cvv2_sim
%  3) in the simulation, set all future MV values to the current value
%  *********************************************************************

%  first, prepare vectors of the manipulated, input variables
%  set the previous and all future (in horizon) to previous value
%  which would be valid if controller were not executed

%   for ijk=1:n-1                 % this summation not required
%       mv1_pre(ijk) = mv1(ijk);  % need to update only last value
%       mv2_pre(ijk) = mv2(ijk);
%   end

   mv1_pre(n-1) = mv1(n-1) ; mv2_pre(n-1)=mv2(n-1);

   for kji=n:NN+n                 % these are future and are fixed
      mv1_pre(kji) = mv1(n-1);
      mv2_pre(kji) = mv2(n-1);
   end
%  *********************************************************************
%  set the prediction equal to the model simulation at the current point
%  which was done in the DMC simulation section above

%   cv1_pre = cv1_sim;
%   cv2_pre = cv2_sim;

%  simulation for NN steps into the future

for  nn=n:n+NN+1

MV1_pre=[mv1_pre(nn-1-gamma_m(1,1))-initmv(1) mv2_pre(nn-1-gamma_m(1,2))-initmv(2); ...
         mv1_pre(nn-1-gamma_m(2,1))-initmv(1) mv2_pre(nn-1-gamma_m(2,2))-initmv(2)];

MV2_pre=[mv1_pre(nn-2-gamma_m(1,1))-initmv(1) mv2_pre(nn-2-gamma_m(1,2))-initmv(2); ...
         mv1_pre(nn-2-gamma_m(2,1))-initmv(1) mv2_pre(nn-2-gamma_m(2,2))-initmv(2)];

cv_pre=-a1_m.*cv1_pre-a2_m.*cv2_pre+b1_m.*MV1_pre+b2_m.*MV2_pre;
cv2_pre = cv1_pre;        % holds and updates the n-2 value
cv1_pre = cv_pre;         % holds and updates the n-1 value

% **************** SUMMATION OF DEVIATION AND INITIAL     **********************
% ***************************************************************************

cvv1_pre(nn) = sum(cv_pre(1,:)) + initcv(1); 
cvv2_pre(nn) = sum(cv_pre(2,:)) + initcv(2); 

end  % NN steps in prediction of future

sp1_pre(1:NN) = sp1(n) * ones(1,NN);
sp2_pre(1:NN) = sp2(n) * ones(1,NN);

err1=zeros(1,NN);
err2=zeros(1,NN);

for klm = 1:NN
    err1(klm) = sp1_pre(klm) - cvv1_pre(n + klm ) - ERRM1(n);
    err2(klm) = sp2_pre(klm) - cvv2_pre(n + klm ) - ERRM2(n);
end  % klm

err  = [ err1 err2];

%  ******************  CALCULATE THE NEXT CHANGE IN THE MV   ******************

deltacontout1(n) = DMC_V1 * err';
deltacontout2(n) = DMC_V2 * err';

mv1(n) = mv1(n-1)  + deltacontout1(n) ;
mv2(n) = mv2(n-1)  + deltacontout2(n) ;


end   %  conalgo    ENDS THE ALGORITHM CALCULATIONS FOR THIS TIME STEP

% ***************************************************************************
% ********************** END CONTROLLER CALCULATIONS*************************
% ***************************************************************************


% ***************************************************************************
% *************************  END OF TIME LOOP  ******************************
% ***************************************************************************
end % LOOP
% ***************************************************************************
% **********************  PERFORMACE CALCULATIONS  **************************
% ***************************************************************************

perform;        % CALLS M-FILE WHICH EVALUATES CONTROLLER PERFORMANCE

%clg;            % INDICATES THE COMPLETION OF THE SIMULATION

disp('  ')
disp('***** THE SIMULATION IS COMPLETE *****');

pause(1);       % PAUSES FOR 1 SECOND
% ***************************************************************************
% *****************************  END SIMULATION  ****************************
% ***************************************************************************
