function  [Reg,A2,FLAG]= bo_d_gen02(rety,Vs,nump,denop,Ttot,app,X0,OPT)
%function [Reg(,A2,FLAG)]=bo_d_gen02(rety,Vs,nump,denop(,Ttot,app,X0,OPT))
%Optimization of continuous controllers: Magnitude Optimum - delayed inputs
%UNLIKE bo_d_gen: higher order controllers PID3, PID4 available;
%                 revised, generalized equation system 
% Plant:        Vs=gain; nump  = numerator polynomial;
%                        denop = denominator polynomial
% Controllers:  rety={2,3,4,5,6}={PI,PID,PID2,PID3,PID4}
%               Reg=    [VR,TN,TR0,TV,TQ,TT,TU]
%        parameter meaning: GC(p)=VR*[1+1/(p*TN)+p*TV+p^2*TQ+p^3*TT+p^4*TU]
%        VR=TN/TR0, p: Laplace variable, thus alternatively:
%        GC(p)=[1+p*TN+p^2*TV*TN+p^3*TQ*TN+p^4*TT*TN+p^5*TU*TN]/(p*TR0)
% Pre-filter:   pre-filter denominator = controller numerator (ex gain);
%               GFi(p)=1/[1+p*TN+p^2*TV*TN+p^3*TQ*TN+p^4*TT*TN+p^5*TU*TN]
% Ttot:    optional dead-time
% app:     optional approximation code for Ttot: if absent app=23
%          11 ... 19 = Strejc approximation of order 01 ... 09
%          21 ... 23 = Pade         "        "   "   01 ... 03
%          31 ... 39 = Polynomial   "        "   "   01 ... 09
% X0:      optional initial vales for equation solver "fsolve"
% OPT:     optional OPTIONS for equation solver "fsolve"
% A2,FLAG: optional outputs A2 and EXITFLAG of equation solver "fsolve"
%          meaningful results: A2 as small as possible, Flag=1
% For deactivation of display commands use hidden mode "rety+10" as type
%                                        - useful for nested function calls

%Check of inputs:
if round(rety)~=rety|~isscalar(rety) 
                                        disp('Input rety improper'); end
if rety>10 rety=rety-10; d_off=1;     %realizes "disp" hidden mode
                                  else d_off=0; end
if ~isreal(Vs)||~isscalar(Vs)  disp('Input Vs improper'); end

for j=1:length(nump) if ~isreal(nump(j))||~isscalar(nump(j))
                  disp(['Input 3 nump(' num2str(j) ') improper']), end, end
for j=1:length(denop) if ~isreal(denop(j))||~isscalar(denop(j))
                 disp(['Input 4 denop(' num2str(j) ') improper']), end, end
Knz=find(denop,1,'last');            %Last vector element unequal zero
DeltaInt=length(denop)-Knz;          %plant integral terms
NumInt=DeltaInt+1;                   %controller features one integral term
%%% Previously designed for controllers with just one integral part !!!
% --------- should not be applied -------------------------------------------
%if NumInt>1             %hence, n0=1 NOT valid, but n0=...=n_(NumInt-2)==0
%    Vs=Vs/denop(Knz);       %hence, Vs contains fictive units
%    denop=denop(1:Knz);     %hence, denominator must be shortened
%    denop=denop/denop(Knz); %hence, now fictive n0==1
%end,                        n0=1; %thus, settig is general valid
% -------------------------------------------------------------------------
n0=denop(end);
if     nargin<5  NO_Tt=1;            %no dead time
elseif nargin==5 
       if isempty(Ttot) NO_Tt=1;    %[] == no dead time, no more arguments
       else NO_Tt=0; app=23; end,   %dead time, but no approximation rule
elseif nargin>5 
       if isempty(Ttot) NO_Tt=1;    %[] == no dead time, but more arguments
       else NO_Tt=0; end,      %dead time and rule, more arguments possible
elseif nargin>8 disp('Wrong number of input arguments'); %handle X0 and OPT
end, 
   if ~NO_Tt    %% plant features dead time - start
if ~isreal(Ttot)||~isscalar(Ttot)||Ttot<0  disp('Input Ttot improper'); end
if round(app)~=app|~isscalar(app)|app<0 disp('Input app improper'); end
%set chosen Ttot approximation:
if app>10&&app<20                           %approximation 1: Strejc
   ProxOrder=app-10; Tt_num=1; Tt_den=1;
   for i=1:ProxOrder Tt_den=conv(Tt_den,[Ttot/ProxOrder 1]); end,
elseif app>30&&app<40                       %approximation 3: Polynomial
   ProxOrder3=app-30; Tt_num=1; Tt_den(1,ProxOrder3+1)=1;
   for i=1:ProxOrder3 Tt_den(ProxOrder3-(i-1))=Ttot^i/factorial(i); end,
elseif app==21  Tt_num=[-Ttot  1];          %approximation 2: Pade
                Tt_den=[+Ttot  1];          %reconfiguration p^0: Koeff=1
elseif app==22  Tt_num=[(1/12)*Ttot^2  -0.5*Ttot  1];   
                Tt_den=[(1/12)*Ttot^2   0.5*Ttot  1];
elseif app==23  Tt_num=[-(1/120)*Ttot^3  0.1*Ttot^2  -0.5*Ttot  1];
                Tt_den=[+(1/120)*Ttot^3  0.1*Ttot^2   0.5*Ttot  1];
else   disp('Input app improper'); end 
%Firstly polynomial multiplication thereafter reversal of order
%Tt_num, Tt_den,  % for check ONLY
nump=conv(nump,Tt_num); denop=conv(denop,Tt_den);
   end          %% plant features dead time - end
%set polynomials in ASCENDING order now:
zf=flip(nump); nf=flip(denop);
maxVL=max(length(zf),length(nf));
zf(maxVL+1)=0; nf(maxVL+1)=0;  %Vector extension is irrelevant

% MINIMUM polynomial order for PI:  4!!! check nesssary
% rety depended necessary orders::   PS:3,5,7,9,11;    DZ and DP:1,2,3,4,5
if     rety==2 xP=3;  xD=1; %PI
elseif rety==3 xP=5;  xD=2; %PID
elseif rety==4 xP=7;  xD=3; %PID2
elseif rety==5 xP=9;  xD=4; %PID3
elseif rety==6 xP=11; xD=5; %PID4
else   error('Error 1 in bo_d_gen02'); end
if maxVL<xP zf(xP+1)=0; nf(xP+1)=0; end      %adequate vector length
% auffüllen vermeiden: höchsten Wert zuerst Null setzen 
PSV(11)=0;for i=1:xP PSV(i)=PSX(i,zf,nf); end %irrespective zeros and poles
DZV(5)=0; for i=1:xD DZV(i)=DZX(i,zf); end    %if maxVL>=xP length=maxVL+1
DPV(5)=0; for i=1:xD DPV(i)=DPX(i,nf); end

XX=[];
if exist('X0','var')&&~isempty(X0) AW=X0;
                              else AW=rand(1,rety); end, %initial values
if exist('OPT','var') optim=OPT; else optim=[];   end,
  %StepToleranz=TolX;   FiniteDifferenceStepSize=FinDiffRelStep
  %if rety==2 %equation system may be too easy for 'trust-region-dogleg'
  %FSdefault=optimset('fsolve');
  %optim=optimset(FSdefault,'TolFun',1e-40,'FinDiffRelStep',sqrt(eps),...
  %                 'TolX',1.0e-06,...
  %                 'Algorithm','levenberg-marquardt');
  %end
PS0=n0;       DZ0=0.5;        DP0=0.5*n0^2;
VK(1,1)=DP0; VK(2:6,1)=DPV(1:5);   % DPV ist oben schon aufgefüllt
                                   % VK noch nicht manipuliert
VDE=[1/2  -1/4  1/8  -1/16  1/32]; % Ergebnis der Manipulation für Mqu, Mpr
Mqu=diag(VDE); Mqu(6,5)=0;         % schon manipuliert, DZ0 integriert
Mpr=Mqu(:,1:4);                    % schon manipuliert, DZ0 integriert
ML=[ PSV(1)   n0         0       0       0      0;
    -PSV(3)  -PSV(2)  -PSV(1)  -n0       0      0;
     PSV(5)   PSV(4)   PSV(3)  PSV(2)  PSV(1)   n0;
    -PSV(7)  -PSV(6)  -PSV(5) -PSV(4) -PSV(3) -PSV(2);
     PSV(9)   PSV(8)   PSV(7)  PSV(6)  PSV(5)  PSV(4);
    -PSV(11) -PSV(10) -PSV(9) -PSV(8) -PSV(7) -PSV(6)];  %alle PSx direkt
DZ1=DZV(1); DZ2=DZV(2); DZ3=DZV(3); DZ4=DZV(4); DZ5=DZV(5);
F31=DZ1^2-DZ2/2;                                                 F64=F31/8;
                                                 F42=F31/2;  F53=F31/4;
F41=DZ1^3-DZ1*DZ2+DZ3/4;                         F52=F41/2;  F63=F41/4;
F51=DZ1^4-1.5*DZ1^2*DZ2+DZ1*DZ3/2+DZ2^2/4-DZ4/8; F62=F51/2;
F61=DZ1^5-2*DZ1^3*DZ2+3*DZ1^2*DZ3/4-DZ1*DZ4/4+3*DZ2^2*DZ1/4-DZ2*DZ3/4-...
                                                                    DZ5/16;
Vko(1,1)  =VK(1);   %Manipulation von Vko und MLiMa
MLiMa(1,:)=ML(1,:);
Vko(2,1)  =VK(1)  *DZ1-0.5*VK(2);
MLiMa(2,:)=ML(1,:)*DZ1-0.5*ML(2,:);
Vko(3,1)  =VK(1)  *F31-0.5*DZ1*VK(2)  +0.25*VK(3);
MLiMa(3,:)=ML(1,:)*F31-0.5*DZ1*ML(2,:)+0.25*ML(3,:);
Vko(4,1)  =VK(1)  *F41-VK(2)  *F42+0.25*DZ1*VK(3)  -VK(4)/8;
MLiMa(4,:)=ML(1,:)*F41-ML(2,:)*F42+0.25*DZ1*ML(3,:)-ML(4,:)/8;
Vko(5,1)  =VK(1)  *F51-VK(2)  *F52+VK(3)  *F53-VK(4)  *DZ1/8+VK(5)/16;
MLiMa(5,:)=ML(1,:)*F51-ML(2,:)*F52+ML(3,:)*F53-ML(4,:)*DZ1/8+ML(5,:)/16;
Vko(6,1)  =VK(1)  *F61-VK(2)  *F62+VK(3)  *F63-...
                                     VK(4)  *F64+VK(5)  *DZ1/16-VK(6)/32;
MLiMa(6,:)=ML(1,:)*F61-ML(2,:)*F62+ML(3,:)*F63-...
                                     ML(4,:)*F64+ML(5,:)*DZ1/16-ML(6,:)/32; 
[XX,A2,FLAG]=fsolve(@(XX) bo_d_gen_eq(XX,rety,Vko,MLiMa,Mqu,Mpr),AW,optim);
%Definitionen für Regler mit einem I-Anteil
VR=XX(2)/Vs;          % Vr   == XX(2)  is identical to variable v
T0=1/XX(1);           % 1/T0 == XX(1)  is identical to variable y
TR0=T0*Vs;            % Algorithm is based on T0 rated by Vs 
TN=VR*TR0;            % ==Vr*T0: here rating must cancel itself out
Reg=[VR,TN,TR0];         %Reg=[VR,TN,TR0,TV,TQ]
if rety>2 Tpr=XX(3); %Tp/T0 is identical to variable x
             Tp=Tpr*T0; TV=Tp/TN; Reg(4)=TV; end
if rety>3 Tqr=XX(4); %Tq/T0 is identical to variable w
             Tq=Tqr*T0; TQ=Tq/TN; Reg(5)=TQ; end
if rety>4 Ttr=XX(5); %Tt/T0 is identical to variable t
             Tt=Ttr*T0; TT=Tt/TN; Reg(6)=TT; end
if rety>5 Tur=XX(6); %Tu/T0 is identical to variable u
             Tu=Tur*T0; TU=Tu/TN; Reg(7)=TU; end
   if ~d_off   %switch off disp commands - useful for nested function calls         
disp(['Sequence of the parameters: ' 'VR, TN, TR0, TV, TQ, TT, TU']);
   end
end

function q=bo_d_gen_eq(XX,rety,Vko,MLiMa,Mqu,Mpr)
y=XX(1);  v=XX(2);
if rety>2 x=XX(3); else x=0; end
if rety>3 w=XX(4); else w=0; end
if rety>4 t=XX(5); else t=0; end
if rety>5 u=XX(6); else u=0; end
Vli=[ y v x w t u ]';  Vqu=Vli(2:6).^2;
Vpr=-2*[ y*x  v*w-y*t  x*t-v*u  w*u]';
erg = Vko + MLiMa*Vli + Mqu*Vqu + Mpr*Vpr;
q=erg(1:rety);
end 
function PS=PSX(x,nump,denop)
%assumption 1: equal length of nump and denop
%assumption 2: x and length are harmonated
PS=0;
for i=0:x
    PS=PS+(-1)^i*denop(i+1)*nump(x+1-i);
end
end
function DZ=DZX(x,nump)
%assumption: x and length of nump are harmonated
DZ=0.5*nump(x+1)^2;
for i=0:x-1
    DZ=DZ+(-1)^(i+1)*nump(x-i)*nump(x+i+2);
end
end
function DP=DPX(x,denop);
%assumption: x and length of denop are harmonated
DP=0.5*denop(x+1)^2;
for i=0:x-1
    DP=DP+(-1)^(i+1)*denop(x-i)*denop(x+i+2);
end
end