function predopt(block)
%PREDOPT   Executes the Predictive Controller Approximation based on Gauss Newton. 
    
%   Copyright 1992-2022 The MathWorks, Inc.
% Orlando De Jesus, Martin Hagan, 1-25-00

setup(block)
end

function setup(block)
block.NumDialogPrms = 22;
mdlInitializeSizes(block);
block.RegBlockMethod('InitializeConditions',@mdlInitConditions);
block.RegBlockMethod('Outputs', @mdlOutputs);
block.RegBlockMethod('Update', @mdlUpdate);
block.RegBlockMethod('PostPropagationSetup', @mdlSetWorkWidths);
block.RegBlockMethod('Terminate', @mdlTerminate);
end

function mdlSetWorkWidths(block)
% N2          = block.DialogPrm(1).Data;
% Ts          = block.DialogPrm(2).Data;
Nu          = block.DialogPrm(3).Data;
% maxiter     = block.DialogPrm(4).Data;
% csrchfun    = block.DialogPrm(5).Data;
% rho         = block.DialogPrm(6).Data;
% alpha       = block.DialogPrm(7).Data;
S1          = block.DialogPrm(8).Data;
% IW          = block.DialogPrm(9).Data;
% LW1_2       = block.DialogPrm(10).Data;
% LW2_1       = block.DialogPrm(11).Data;
% B1          = block.DialogPrm(12).Data;
% B2          = block.DialogPrm(13).Data;
Ni          = block.DialogPrm(14).Data;
Nj          = block.DialogPrm(15).Data;
% min_i       = block.DialogPrm(16).Data;
% max_i       = block.DialogPrm(17).Data;
% minp        = block.DialogPrm(18).Data;
% maxp        = block.DialogPrm(19).Data;
% mint        = block.DialogPrm(20).Data;
% maxt        = block.DialogPrm(21).Data;
% Normalize   = block.DialogPrm(22).Data;

block.NumDworks = 1;
block.Dwork(1).Name = 'x0';
block.Dwork(1).Dimensions = Ni+Nu-1+(S1+1)*(Nj-1);
block.Dwork(1).DatatypeID      = 0;
block.Dwork(1).Complexity      = 'Real';
block.Dwork(1).UsedAsDiscState = true;
end

function mdlInitializeSizes(block)
N2          = block.DialogPrm(1).Data;
Ts          = block.DialogPrm(2).Data;
Nu          = block.DialogPrm(3).Data;
% maxiter     = block.DialogPrm(4).Data;
% csrchfun    = block.DialogPrm(5).Data;
% rho         = block.DialogPrm(6).Data;
alpha       = block.DialogPrm(7).Data;
S1          = block.DialogPrm(8).Data;
IW          = block.DialogPrm(9).Data;
LW1_2       = block.DialogPrm(10).Data;
LW2_1       = block.DialogPrm(11).Data;
B1          = block.DialogPrm(12).Data;
B2          = block.DialogPrm(13).Data;
Ni          = block.DialogPrm(14).Data;
Nj          = block.DialogPrm(15).Data;
% min_i       = block.DialogPrm(16).Data;
% max_i       = block.DialogPrm(17).Data;
minp        = block.DialogPrm(18).Data;
maxp        = block.DialogPrm(19).Data;
mint        = block.DialogPrm(20).Data;
maxt        = block.DialogPrm(21).Data;
Normalize   = block.DialogPrm(22).Data;


block.NumInputPorts = 1;
block.NumOutputPorts = 1;

block.InputPort(1).Dimensions = -1;
block.InputPort(1).DirectFeedthrough = false;

block.OutputPort(1).DatatypeID = 0;
block.OutputPort(1).Dimensions = 1;
block.OutputPort(1).SamplingMode = 'Sample';

load_system('ptest3sim2');
if Normalize
    IW_gU=((maxt-mint)/(maxp-minp))*IW;
else
    IW_gU=IW;
end
set_param('ptest3sim2/Subsystem','B2',num2str(B2,20),'B1',mat2str(B1,20),'LW2_1',mat2str(LW2_1,20), ...
    'LW1_2',mat2str(LW1_2,20),'IW',mat2str(IW,20),'IW_gU',mat2str(IW_gU,20), ...
    'Ts',num2str(Ts),'S1',num2str(S1),'Ni',num2str(Ni), ...
    'Nj',num2str(Nj),'minp',num2str(minp,20),'maxp',num2str(maxp,20), ...
    'minp',num2str(minp,20),'mint',num2str(mint,20),'maxt',num2str(maxt,20), ...
    'Normalize',num2str(Normalize),'Nu',num2str(Nu));
assignin('base','t_init',cputime);
assignin('base','cont_u',0);

global tiu dUtilde_dU
global N1 d alpha2 upi uvi

block.SampleTimes  = [Ts 0]; % Inherited sample time

tiu=Ni;
dUtilde_dU = eye(Nu);
dUtilde_dU(1:Nu-1,2:Nu)=dUtilde_dU(1:Nu-1,2:Nu)-eye(Nu-1);
N1=1;
d=1;
alpha2     = alpha*alpha;
upi   = [1:Nu-1 Nu(ones(1,N2-d-Nu+2))];
uvi   = tiu:N2-N1+Ni;
end

function mdlInitConditions(block)
% N2          = block.DialogPrm(1).Data;
% Ts          = block.DialogPrm(2).Data;
Nu          = block.DialogPrm(3).Data;
% maxiter     = block.DialogPrm(4).Data;
% csrchfun    = block.DialogPrm(5).Data;
% rho         = block.DialogPrm(6).Data;
% alpha       = block.DialogPrm(7).Data;
S1          = block.DialogPrm(8).Data;
% IW          = block.DialogPrm(9).Data;
% LW1_2       = block.DialogPrm(10).Data;
% LW2_1       = block.DialogPrm(11).Data;
% B1          = block.DialogPrm(12).Data;
% B2          = block.DialogPrm(13).Data;
Ni          = block.DialogPrm(14).Data;
Nj          = block.DialogPrm(15).Data;
min_i       = block.DialogPrm(16).Data;
max_i       = block.DialogPrm(17).Data;
% minp        = block.DialogPrm(18).Data;
% maxp        = block.DialogPrm(19).Data;
% mint        = block.DialogPrm(20).Data;
% maxt        = block.DialogPrm(21).Data;
% Normalize   = block.DialogPrm(22).Data;

x0  = zeros(Ni+Nu-1+(S1+1)*(Nj-1),1);
x0(Ni:Nu+Ni-1) = (max_i-min_i)/2;

block.Dwork(1).Data = x0;
end

function mdlTerminate(~)
close_system('ptest3sim2',0);
assignin('base','t_end',cputime);
end

function mdlOutputs(block)
% N2          = block.DialogPrm(1).Data;
% Ts          = block.DialogPrm(2).Data;
% Nu          = block.DialogPrm(3).Data;
% maxiter     = block.DialogPrm(4).Data;
% csrchfun    = block.DialogPrm(5).Data;
% rho         = block.DialogPrm(6).Data;
% alpha       = block.DialogPrm(7).Data;
% S1          = block.DialogPrm(8).Data;
% IW          = block.DialogPrm(9).Data;
% LW1_2       = block.DialogPrm(10).Data;
% LW2_1       = block.DialogPrm(11).Data;
% B1          = block.DialogPrm(12).Data;
% B2          = block.DialogPrm(13).Data;
Ni          = block.DialogPrm(14).Data;
% Nj          = block.DialogPrm(15).Data;
% min_i       = block.DialogPrm(16).Data;
% max_i       = block.DialogPrm(17).Data;
% minp        = block.DialogPrm(18).Data;
% maxp        = block.DialogPrm(19).Data;
% mint        = block.DialogPrm(20).Data;
% maxt        = block.DialogPrm(21).Data;
% Normalize   = block.DialogPrm(22).Data;

x = block.Dwork(1).Data;
block.OutputPort(1).Data = x(Ni);
end

function mdlUpdate(block)

N2          = block.DialogPrm(1).Data;
Ts          = block.DialogPrm(2).Data;
Nu          = block.DialogPrm(3).Data;
maxiter     = block.DialogPrm(4).Data;
csrchfun    = block.DialogPrm(5).Data;
rho         = block.DialogPrm(6).Data;
alpha       = block.DialogPrm(7).Data;
S1          = block.DialogPrm(8).Data;
% IW          = block.DialogPrm(9).Data;
% LW1_2       = block.DialogPrm(10).Data;
% LW2_1       = block.DialogPrm(11).Data;
% B1          = block.DialogPrm(12).Data;
% B2          = block.DialogPrm(13).Data;
Ni          = block.DialogPrm(14).Data;
Nj          = block.DialogPrm(15).Data;
min_i       = block.DialogPrm(16).Data;
max_i       = block.DialogPrm(17).Data;
minp        = block.DialogPrm(18).Data;
maxp        = block.DialogPrm(19).Data;
mint        = block.DialogPrm(20).Data;
maxt        = block.DialogPrm(21).Data;
Normalize   = block.DialogPrm(22).Data;

x = block.Dwork(1).Data;
u = block.InputPort(1).Data;

global tiu dUtilde_dU
global N1 d alpha2 upi uvi

Ai=num2cell(zeros(2,Nj));
for k=1:Nj-1
    Ai{1,k}=x(Nu+Ni+1+(k-1)*(S1+1):Nu+Ni+S1+(k-1)*(S1+1));
    Ai{2,k}=x(Nu+Ni+(k-1)*(S1+1));                             % delayed plant output
end
Ai{1,Nj}=u(4:3+S1);

ref(1:N2,1)=u(1);
initval = '[upmin(Nu)]';

upmin=[x(Ni+1:Nu+Ni-1);x(Nu+Ni-1)];
u_vec(1:Ni-1,1)=x(2:Ni);
if Normalize
    ref=((ref-mint)*2/(maxt-mint)-1);
    Ai{2,Nj}=((u(3)-mint)*2/(maxt-mint)-1);           % Actual NN output
    upmin=((upmin-minp)*2/(maxp-minp)-1);
    u_vec=((u_vec-minp)*2/(maxp-minp)-1);
else
    Ai{2,Nj}=u(3);
end

upmin0   = upmin;
einitval = eval(initval);     % Evaluate 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,1) = 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 <<<<<<<<<<<<<<<<
    assignin('base','cont_u',evalin('base','cont_u')+1);

    set_param('ptest3sim2/Subsystem','u_init',mat2str(u_vec(Ni),20),'ud_init',mat2str(u_vec(Ni-1:-1:1),20), ...
        'y_init',mat2str(Ai{2,Nj},20),'yd_init',mat2str(cat(1,Ai{2,Nj-1:-1:1}),20));
    [~,~,Ac1,~,E,gU,gUd,dY_dU] = sim('ptest3sim2',[0 N2*Ts],[],[(0:Ts:(N2-2)*Ts)' u_vec(1:N2-1) ref(1:N2-1)]);

    yhat_vec=Ac1(1:N2+1,1)';

    E=E(2:N2+1,:);

    gU=gU(1:N2,:)';
    gUd=gUd(1:N2,:)';

    evec=E;

    if tiu==1
        duvec = [0; u_vec(tiu+1:tiu+Nu-1)-u_vec(tiu:tiu+Nu-2)];
    else
        duvec = u_vec(tiu:tiu+Nu-1)-u_vec(tiu-1:tiu+Nu-2);
    end

    JJ = evec'*evec + rho*(duvec'*duvec);

    % Forward Perturbation
    dY_dU=dY_dU(2:N2+1,:)';
    dJJ   = 2*(-dY_dU*evec + rho*(dUtilde_dU*duvec));
    if Normalize
        dJJ=dJJ/(maxp-minp);
    end

    %>>>>>>>>>>>>>>>>>>>>>>    EVALUATE CRITERION    <<<<<<<<<<<<<<<<<<<<<<
    J = JJ;


    %>>>>>>>>>>>>>>>>>>>>>>>>      DETERMINE dyhat/du       <<<<<<<<<<<<<<<<<<<<<<<<<

    %>>>>>>>>>>>>>>>>>>>>>>>>>>>>    DETERMINE dJ/du     <<<<<<<<<<<<<<<<<<<<<<<<<<<<
    dJdu   = dJJ;


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


    delta=1;
    tol=1/delta;
    ch_perf = J;      % for first iteration.
    %>>>>>>>>>>>>>>>>>>>>>>>     BEGIN SEARCH FOR MINIMUM      <<<<<<<<<<<<<<<<<<<<<<
    for m = 1:maxiter

        %>>>>>>>>>>>>>>>>>>>>>>>   DETERMINE SEARCH DIRECTION   <<<<<<<<<<<<<<<<<<<<<<<
        dX = -B*dJdu;

        if dX'*dJdu>0    % We reset the gradient if positive.
            %>>>>>>>>>>>>>>>>>>>>>>    DETERMINE INVERSE HESSIAN    <<<<<<<<<<<<<<<<<<<<<<<<<
            B = eye(Nu);                  % Initialize Hessian to I
            delta=1;
            tol=1/delta;
            ch_perf = J;      % for first iteration.
            %>>>>>>>>>>>>>>>>>>>>>>>   DETERMINE SEARCH DIRECTION   <<<<<<<<<<<<<<<<<<<<<<<
            dX = -B*dJdu;
        end

        if Normalize
            switch csrchfun
                case 1 %'csrchgol',
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=csrchgol(up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,-1,1,Normalize,minp,maxp);
                case 2  %'csrchbac',
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=csrchbac(up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,-1,1,Normalize,minp,maxp);
                case 3  %'csrchhyb'
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=csrchhyb(up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,-1,1,Normalize,minp,maxp);
                case 4  %'csrchbre'
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=csrchbre(up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,-1,1,Normalize,minp,maxp);
                case 5  %'csrchcha'
                    J_old=J;
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=csrchcha(up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,-1,1,Normalize,minp,maxp,ch_perf);
                    ch_perf = J - J_old;
                otherwise
                    J_old=J;
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=feval(csrchfun,up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,-1,1,Normalize,minp,maxp,ch_perf);
                    ch_perf = J - J_old;
            end
        else
            switch csrchfun
                case 1 %'csrchgol',
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=csrchgol(up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,min_i,max_i,Normalize,minp,maxp);
                case 2  %'csrchbac',
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=csrchbac(up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,min_i,max_i,Normalize,minp,maxp);
                case 3  %'csrchhyb'
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=csrchhyb(up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,min_i,max_i,Normalize,minp,maxp);
                case 4  %'csrchbre'
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=csrchbre(up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,min_i,max_i,Normalize,minp,maxp);
                case 5  %'csrchcha'
                    J_old=J;
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=csrchcha(up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,min_i,max_i,Normalize,minp,maxp,ch_perf);
                    ch_perf = J - J_old;
                otherwise
                    J_old=J;
                    [up_delta,J,dJdu_old,dJdu,~,delta,tol]=feval(csrchfun,up,u_vec,ref,Ai,Nu,N1,N2,d,Ni,Nj,dX,dJdu,J,dX'*dJdu,delta,rho,dUtilde_dU,alpha,tol,Ts,min_i,max_i,Normalize,minp,maxp,ch_perf);
                    ch_perf = J - J_old;
            end
        end


        %>>>>>>>>>>>>>>>>>>>>>>>>   UPDATE FUTURE CONTROLS   <<<<<<<<<<<<<<<<<<<<<<<<<
        up_old = up;
        up = up_delta;


        %>>>>>>>>>>>>>>>>>>>>>>>>     CHECK STOP CONDITION     <<<<<<<<<<<<<<<<<<<<<<<
        dup = up-up_old;
        if (dup'*dup < alpha2) || (ch_perf==0)
            break;
        end


        %>>>>>>>>>>>>>>>>>>>     BFGS UPDATE OF INVERSE HESSIAN    <<<<<<<<<<<<<<<<<<
        dG  = dJdu - dJdu_old;
        BdG = B*dG;
        dupdG = dup'*dG;
        fac = 1/dupdG;
        diff = dup - BdG;
        dupfac=dup*fac;
        diffdup = diff*(dupfac');
        B = B + diffdup + diffdup' - (diff'*dG)*(dupfac*dupfac');
    end


    %>>>>>>>>>>>>>>>>>>>>>>>     SELECT BEST MINIMUM     <<<<<<<<<<<<<<<<<<<<<<<<<
    if tr==1
        Jmin_old = J;
        upmin = up;
    else
        if J<Jmin_old
            upmin = up;
        end
    end
end

x(1:Ni-1)=x(2:Ni);           % State 1 to Nu = actual controls
if upmin(1)>1 || upmin(1)<-1
    upmin(1)=upmin(1);
end
if Normalize
    upmin=(upmin+1)*(maxp-minp)/2+minp;
end
x(Ni:Nu+Ni-1)=upmin;           % State 1 to Nu = actual controls
for k=1:Nj-2
    x(Nu+Ni+1+(k-1)*(S1+1):Nu+Ni+S1+(k-1)*(S1+1))=x(Nu+Ni+1+(k)*(S1+1):Nu+Ni+S1+(k)*(S1+1));
    x(Nu+Ni+(k-1)*(S1+1))=x(Nu+Ni+(k)*(S1+1));                             % delayed plant output
end
if Nj>=2
    if Normalize
        x(Nu+Ni+(Nj-2)*(S1+1))=((u(3)-mint)*2/(maxt-mint)-1);            % state Nu+1 = NN output
    else
        x(Nu+Ni+(Nj-2)*(S1+1))=u(2);
    end
    x(Nu+Ni+1+(Nj-2)*(S1+1):Nu+Ni+S1+(Nj-2)*(S1+1))=Ai{1,Nj};    % State Nu+2... = delayed layer 1 output.
end

block.Dwork(1).Data=x;
end