% TITLE: INTEG4A.M
% DONE BY : RENO CIPOLLA
% DATE: JAN-MARCH 1994
% VERSION 2.0
% REVISED January 1995 
% Copyright 1995 T. Marlin and R. Cipolla
% This is a function program using discrete equations to solve a set of DE's.
% A fourth order lead/lag dead time model is represented. 
% This program is called by the SIMULATOR and OPTIMIZER.
% ***************************************************************************

function Q = integ4A(Y0) 

% Set all menu variables to global which allows them to be passed from 
% base workspace to function M-file.

global tau1 tau2 tau3 Kp set deltat dist vp time pv Dyn junk2 rho1 obj distg    
global tdist D change tset SP t usegrh maxit IAE ISE ITAE tmg boundg violates  
global cn_t scn_t bound mvtest betaa Kn A useopt td maxdev mindev rise rho chg 
global epsl p pp bdpos bdneg bd kc ti bgen Xmis vpmis bdmis Kpp ob_j op_ts lim_it  
global Qp mm theta taulead xi thetad tauleadd xid taud1 taud2 taud3 Kpd x_ ver_sion 
global c_1 c_2 c_i b_1 b_2 b_i int_kc int_ti o_func le_vel numlev 
global ss ss1  ss2 ss3 v v1 v2 v3 al_pha save_graph ct_count  



if useopt == 0 & pp== 0            % initialize vectors for model mismatch.
 postv = (1 + epsl/100); 
 negtv = (1 - epsl/100);
 for p=2:3
  if p==2
   Kp(p)=Kp(1)*postv; taulead(p)=taulead(1)*postv; tauleadd(p)=tauleadd(1)*postv;      
   tau1(p)=tau1(1)*postv; taud1(p)=taud1(1)*postv;
   tau2(p)=tau2(1)*postv; taud2(p)=taud2(1)*postv;
   tau3(p)=tau3(1)*postv; taud3(p)=taud3(1)*postv;
   theta(p)=theta(1)*postv; thetad(p)=thetad(1)*postv;
  else
   Kp(p)=Kp(1)*negtv; taulead(p)=taulead(1)*negtv; tauleadd(p)=tauleadd(1)*negtv;          
   tau1(p)=tau1(1)*negtv;  taud1(p)=taud1(1)*negtv;  
   tau2(p)=tau2(1)*negtv;  taud2(p)=taud2(1)*negtv;         
   tau3(p)=tau3(1)*negtv;  taud3(p)=taud3(1)*negtv;
   theta(p)=theta(1)*negtv; thetad(p)=thetad(1)*negtv;
  end
 end
end
Kpp=Kp;                           % variable used for MV bound.
maxit=ceil(time/deltat);
violates=zeros(1,3);
Xmis=zeros(maxit+1,3);
vpmis=Xmis; 

%if deltat < .1
%  Xmis = zeros(maxit + 2,3); vpmis=Xmis;
%end
pp = pp + 1;                      % Counts number of times function is called

for p=1:3                         % Beginning of MAIN LOOP for model mismatch
 if p > 1 
   if useopt > 0
    break, end
 end
 
 if p > 1 & pp < 2                % if only first call to simulation
   mvbound1                       % calculate MV Bound for p=2 & 3 
   mvtest=2;
 end
if op_ts == 0
 if useopt > 0 & mm > 0
  clc
  disp('')
  disp('')
  disp('      Simulation in Progress please wait ........      ')
  disp('')
  disp('')
  disp(' Go to PLOT RESULTS to see results (only last simulation will be recorded)')
 elseif p==1 & mm==0
  clc
  disp('')
  disp('')
  disp('     Simulating Model Mismatch Cases  please wait ........      ')
  disp('')
  disp('')
  disp(' Go to PLOT RESULTS to see results (only last set will be recorded)')
 end
end
                                   % Initialize Noise Seed with mean = 0 
 randn('seed',0);                  % and Std. deviation = 1

 vpmin = -50;                     % Range of MV 
 vpmax =  50;
 IAE=0;                           % initialize miscellaneous variables
 ISE=0;
 ITAE=0;
 
 gam_a= round(theta(p)/deltat);
 gam_ad= round(thetad(p)/deltat);

 t=zeros(1,maxit+1); 
 pv=zeros(1,maxit+1); vp=pv; pv1=vp;  set=pv1; Noise=set; distg=set;
 z=Noise; %size(z)
 dv1=zeros(1,maxit+gam_ad+1); dist=set;
 
 %if deltat < .1 
 %  dist= zeros(1,maxit + 2); distg=dist; set=distg; z=set; Noise=z;
 %end
 
 if theta(p) == 0
  n=1; 
  t(n)=0; Noise(n)=0; 
 else
  n=1:1:(gam_a);
  t(n)= (n-1)*deltat; 
  n=gam_a ; 
 end

 if thetad(p) == 0
  m=1;
 else
  m= gam_ad ;
 end

 if theta(p) > 1
  Noise(1)=0;
  for n=2:gam_a
   Noise(n)= A*Noise(n-1) + Kn*(1-A)*randn;  % Noise during dead time  
  end
 end
 if tau3(p) ~= 0              % calculates parameters for second order model
   a2=2*xi/tau3(p);
   a1=1/tau3(p)^2; 
   a0=a1;
   if xi >= 1 
     s_r=roots([tau3(p)^2 2*xi*tau3(p) 1]);
     s_r(1)= real(s_r(1)); s_r(2)= real(s_r(2));
     tau4= -1/s_r(1);
     tau5= -1/s_r(2);
   end
 end

 if taud3(p) ~= 0              
   ad2=2*xid/taud3(p);
   ad1=1/taud3(p)^2; 
   ad0=ad1;
   if xid >= 1 
     sd=roots([taud3(p)^2 2*xid*taud3(p) 1]);
     sd(1)= real(sd(1)); sd(2)= real(sd(2));
     taud4= -1/sd(1);
     taud5= -1/sd(2);
   end
 end


% Initialize dummy variables for intermediate ((n-1)) values

pv2b=0; pv3b=pv2b; pv4b=pv3b; pv5b=pv4b; pv6b=pv5b; rb=pv6b;
dv2b=0; dv3b=dv2b; dv4b=dv3b; dv5b=dv4b; dv6b=dv5b; db=dv6b;
der_p=0; der=0; del_der=0;

while n < maxit + 1 % t < time 

  n=n+1;     
  m=m+1;
  t(n)=t(n-1) + deltat;
%  if n <= maxit + 1

% Process algorithm using lead/lag terms.

pv1(n)=Kp(p)*vp(n-1);

 if betaa == 0 | theta(p) == betaa  
  pv2=pv1(n); 
 else
  pv2=pv1(n-gam_a);
 end


if (tau1(p)==0) & (taulead(p) == 0)
   pv3=pv2;
 else
   pv3=(tau1(p)*pv3b/deltat + (taulead(p)/deltat+1)*pv2 - taulead(p)*pv2b/deltat) / (tau1(p)/deltat+1);
 end

if tau2(p)==0
   pv4=pv3;
 else
   pv4= (1-exp(-deltat/tau2(p)))*pv3 + (exp(-deltat/tau2(p)))*pv4b;
 end

 if tau3(p)== 0
  pv5=pv4;
 else
  if xi < 1
     pv5=pv5b+deltat*rb;
     r=rb+deltat*(-a2*rb - a1*pv5b + a0*pv4b);
  elseif xi >= 1
     pv6= (1-exp(-deltat/tau4))*pv4 + (exp(-deltat/tau4))*pv6b;
     pv5= (1-exp(-deltat/tau5))*pv6 + (exp(-deltat/tau5))*pv5b;
  end
 end

% Disturbance Dynamics

  dv1(m)=Kpd*dist(n-1);
  
  dv2=dv1(m -gam_ad);

 if (taud1(p)==0) & (tauleadd(p) == 0)
   dv3=dv2;
 else
   dv3=(taud1(p)*dv3b/deltat + (tauleadd(p)/deltat+1)*dv2 - tauleadd(p)*dv2b/deltat) / (taud1(p)/deltat+1);
 end

 if taud2(p)==0
   dv4=dv3;
 else
   dv4= (1-exp(-deltat/taud2(p)))*dv3 + (exp(-deltat/taud2(p)))*dv4b;
 end

 if taud3(p)== 0
  dv5=dv4;
 else
  if xid < 1
     dv5=dv5b+deltat*db;
     d=db+deltat*(-ad2*db-ad1*dv5b+ad0*dv4b);
  elseif xid >= 1
     dv6= (1-exp(-deltat/taud4))*dv4 + (exp(-deltat/taud4))*dv6b;
     dv5= (1-exp(-deltat/taud5))*dv6 + (exp(-deltat/taud5))*dv5b;
  end
 end

 Noise(n)= A*Noise(n-1) + Kn*(1-A)*randn;  % Noise 
 pv(n)=pv5+dv5+Noise(n);      % process variable used for feedback
                              % Process + Disturbance + Noise
 if t < betaa
       vp(n)=vp(n-1);
       z(n)=set(n-1) - pv(n);          
       dist(n)=dist(n-1);
       distg(n)=dist(n);
       set(n)=set(n-1);
 else
       if change==1
        dist(n)=D;
       else
        set(n)=SP;
        dist(n)=dist(n-1);
       end
       z(n)= set(n) - pv(n);    
       distg(n)=dist(n);
       IAE=IAE+abs(z(n))*deltat;
       ISE=ISE+(abs(z(n))^2)*deltat;


% Controller Equations - Velocity Form (with filter on derivative mode)      
       
       if junk2 == 2
         deltavp=Y0(1)*(z(n)-z(n-1) + deltat*z(n)/Y0(2));
         der= Y0(1)*(( (td/deltat)*(al_pha*der_p + pv(n)-pv(n-1)) )/(1+al_pha*td/deltat));  
         del_der= der - der_p;
       elseif junk2 == 1 
         deltavp=Y0(1)*(z(n)-z(n-1) + deltat*z(n)/Y0(2));
         der= Y0(1)*(( (Y0(3)/deltat)*(al_pha*der_p + pv(n)-pv(n-1)) )/(1+al_pha*Y0(3)/deltat));  
         del_der= der - der_p;
       elseif junk2 == 3
         deltavp=Y0(1)*(z(n)-z(n-1) + deltat*z(n)/ti);
         der= Y0(1)*(( (Y0(2)/deltat)*(al_pha*der_p + pv(n)-pv(n-1)) )/(1+al_pha*Y0(2)/deltat));  
         del_der= der - der_p;
       end
       deltavp = deltavp - del_der;  % subtract derivative because using pv (not error)
       vp(n)= vp(n-1)+ deltavp;    
       
       vp(n)=max(vpmin,vp(n));   % Anti-reset windup
       vp(n)=min(vpmax,vp(n));

       ITAE=ITAE+(time-betaa)*abs(z(n))*deltat;
 end                       % end if betaa
      
  pv2b=pv2; pv3b=pv3; pv4b=pv4; pv5b=pv5; pv6b=pv6; rb=r;   
  dv2b=dv2; dv3b=dv3; dv4b=dv4; dv5b=dv5; dv6b=dv6; db=d;   
  der_p=der;

% end
end                 %end while

vpp = vp;
boundd = bgen;
pvv = pv;

if p == 1                       % size storage
    Xmis = zeros (length(t),3);
    vpmis= zeros (length(t),3);
end 

 if p== 1 & pp < 2
   Xmis(:,1)= pv';
   vpmis(:,1)=vp';
 elseif p==2                 % Save vectors for model mismatch plots
   Xmis(:,2)= pv'; 
   vpmis(:,2)=vp';
 elseif p==3  
   Xmis(:,3)= pv';
   vpmis(:,3)=vp';
 end

 if pp > 1
   bdd = bd;
   bdposs = bdpos;
   bdnegg = bdneg;
   if p == 1
     violate = MVV(bdd,vpp);
   elseif p == 2
     violate = MVV(bdposs,vpp);    % calculates violation from stored vector 
   elseif p == 3                   % bdd,bdpos or bdneg.
     violate = MVV(bdnegg,vpp);
   end
 else
   violate = MVV(boundd,vpp);     % calls function MVV to calc. MV violation
 end
 
 CVover = CVV(pvv);               % calculates amount of overshoot for CV.
 violates(p)=violate;
 CVovers(p) = CVover;
 
 Qp(p,1)= IAE;
 Qp(p,2)= ISE;
 Qp(p,3)= ITAE;
end                                    % end MAIN LOOP for model mismatch
% Objective Function sum(Errors (IAE,ISE,ITAE) ) + penalties on MV & CV.

Q = sum(Qp(:,obj)) + rho*sum(violates) + rho1*sum(CVovers);  

