function m = arx_with_regularization_special_code(Data,nn,kernel)
%function m = arx_with_regularization_special_code(Data,nn,kernel)
% Estimating Regularized ARX models using special code.
%
% Data: The input-output data (an iddata object)
% nn: nn=[na nb nk] The orders of the ARX model. na, nb, nk are scalars,
% being expanded to the size of Data. In this code, na is either 0
% (estimating Regularaized FIR models) or na=nb. nk is restricted to nk=1.
% Kernel: The regularization kernel, one of 'TC', 'SS' or 'DC'

[N,Ny,Nu]=size(Data);
na=nn(1);
nb=nn(2)*ones(1,Nu);
nk=nn(3)*ones(1,Nu);
Dataext = Data;
 
u=Data.u;
y=Data.y;
Dataext = iddata(y,[y u]);

marx = rfir(Dataext,[na nb],kernel);
Nu = size(u,2);

B = cell(1,Nu);
for k=1:Nu
B{k} = [0 marx(k+1).par'];
F{k}=1;
end

m = idpoly([1 -marx(1).par'],B,1,1,F);

function  [M, estinfo] = rfir(varargin)
%rfir	Construct an FIR model using regularization method.
%
%   M = new_rfir(z,nb,kernel) or
%   M = new_rfir(z,[nb,nk],kernel) or
%   M = new_rfir(z,{nb,nk},kernel)
%
%   estimate an FIR model represented by:
%   y(t) = B(q) u(t-nk) +  e(t)
%   where:
%       nb = order of B polynomial + 1
%       nk = input delay (in number of samples)
%       Ny = number of output
%       Nu = number of inputs
%
%   The estimated model is delivered as an IDPOLY object. Type "help
%   idpoly" for more information on IDPOLY objects.
%
%   Output:
%       M : IDPOLY model containing estimated values for B
%       polynomials along with their structure information. When Ny = 1,
%       M is a multi-input idpoly model. When Ny > 1, Jl is temporarily
%       a Ny by 1 cell array with each element as a multi-input idpoly model.
%       In the future version, Jl will be a multi-input-multi-ouput idpoly model.
%
%   Inputs:
%       z : The estimation data as an IDDATA. Use IDDATA object for input-output
%       time domain signals. Type "help iddata" for more information.
%
%       nb and nk: Orders and delays of the FIR model. nb must be specified
%       but nk can be omitted and the default value of nk is 1. Both nb and
%       nk can be scalar, which assumes all FIR models will have the same
%       order and input delay. nb can also be a Ny by 1 column vector, each
%       element of which indicates the order of all FIR models associated
%       with that ouput. nk can also be a Ny by Nu matrix, through which the
%       delays of each FIR model can be set individually.
%
%       kernel: the structure of the regularization matrix. The following
%       kernels are supported:
%            CS: qubic spline kernel
%            SE: squared exponential kernel
%            SS: stable spline kernel
%            HF: high frequency stable spline kernel
%            DI: diagonal kernel
%            TC: tuned and correlated kernel
%            DC: diagonal and correlated kernel
%       kernel can be a char (e.g., kernel = 'DC'), which assumes all FIR models
%       use the same kernel. kernel can also be a Ny by Nu cell array,
%       through which the kernels of each FIR model can be set individually.
%       More information about the structure of the regularization matrix and
%       the regulariation method can be found in literature.


%  Author: Tianshi Chen
%  All rights reserved.
% check No. of inputs
narginchk(3,3); % data, [nb nk], kernel

% front matter
% decomopse the inputs and check applicability
for j = 1:length(varargin)
    if isa(varargin{j},'iddata')
        data = varargin{j};
    else if isscalar(varargin{j}) || isnumeric(varargin{j})
            nbnk = varargin{j};
        else if ischar(varargin{j})
                kernel = upper(varargin{j});
            else if iscell(varargin{j})
                    tmp = varargin{j};
                    if isnumeric(tmp{1})
                        nbnk = tmp;
                    else
                        kernel = upper(tmp);
                    end
                end
            end
        end
    end
end

if realdata(data)
    [Nd, Ny, Nu, Ne] = size(data);
    dom = pvget(data,'Domain');
else
    error('Error in the data: only real-valued signals supported.');
end



if isscalar(nbnk)
    nb = nbnk*ones(Ny,Nu); nk = ones(Ny,Nu);
end

if isnumeric(nbnk) && ~isscalar(nbnk)
    if isequal(size(nbnk),[1 2])
        nb = nbnk(1)*ones(Ny,Nu); nk = nbnk(2)*ones(Ny,Nu);
    else if isequal(size(nbnk),[Ny Nu])
            nb = nbnk; nk = ones(Ny,Nu);
        else
            error('Error in the order (nb) and delay (nk) parameter matrix: Dimension mismatch with that of the MIMO system.')
        end
    end
end
nb = nbnk(1:Nu);nk=ones(1,Nu); %% added
%nk = nbnk(Nu+1:end)

if iscell(nbnk)
    if isequal(size(nbnk{1}),[Ny Nu]) && isequal(size(nbnk{2}),[Ny Nu])
        nb = nbnk{1}; nk = nbnk{2};
    else
        error('Error in the order (nb) and delay (nk) parameter matrix: Dimension mismatch with that of the MIMO system.')
    end
end


if ischar(kernel)
    tmp = kernel;
    kernel = cell(Ny,Nu);
    for i = 1:Nu
        for j = 1:Ny
            kernel{j,i} = tmp;
        end
    end
else if iscell(kernel)
        if ~isequal(size(kernel),[Ny Nu])
            error('Error in the kernel allocation matrix: Dimension mismatch with that of the MIMO system.')
        end
    end
end

indth = zeros(Ny,Nu+1);
for j = 1:Ny
    ind = cumsum(nb(j,:)+1-nk(j,:));  % ind: index of the estimated FIR parameters for each of Nu blocks
    indth(j,:) = [0 ind];
    
    for i = 1:Nu
        if ~strcmp(kernel{j,i},{'CS' 'SE' 'DI' 'SS' 'HF' 'TC' 'DC' 'NEW1' 'NEW2' 'NEW3'})
            error(['Error in output channel No.' num2str(j) ': No such kernel ' kernel{j,i} ' supported.']);
        end
    end
end



% main body

th = cell(Ny,1);
hp = cell(Ny,1);
hpini = cell(Ny,1);
obj = zeros(Ny,1);
exflg = zeros(Ny,1);
indhp = zeros(Ny,Nu+1);
gradient = cell(Ny,1);
hessian = cell(Ny,1);
otpt = cell(Ny,1);
condNb = zeros(Ny,1);
noi = zeros(Ny,1);

alg = {'trust-region-reflective','active-set', 'interior-point','sqp'};
options = optimset('GradObj','on','Hessian','on','Hessian','user-supplied','Algorithm',alg{1},'Display','off');

for j = 1:Ny
    z = data(:,j,:);
    [hpini{j}, Rt, Ney, indhp(j,:),lb,ub] = ini_rfir_miso(z,nb(j,:),nk(j,:),kernel(j,:),options);
    ff = @(x)nglglklhd(x,Rt, Ney, kernel(j,:),indth(j,:), indhp(j,:));
    [hp{j}, obj(j), exflg(j), otpt{j}, ~, gradient{j}, hessian{j}] = fmincon(ff,hpini{j},[],[],[],[],lb,ub,[],options);
    %      ff = @(x)nglglklhd_simp(x,Rt, Ney, kernel(j,:),indth(j,:), indhp(j,:));
    %      [hp{j} obj(j) exflg(j) otpt{j}] = fminsearch(ff,hpini{j});
    [~, condNb(j), th{j}, noi(j)] = nglglklhd_simp(hp{j},Rt, Ney, kernel(j,:),indth(j,:), indhp(j,:));
end


% output the estimated model and associated structure information
if Ny == 1
    M =  idpoly();
else
    M = idarx();
end

for j = 1:Ny
    
    thtmp = th{j};
    mtmp = idpoly();
    
    for i = 1:Nu
        md = idpoly(1,[zeros(1,nk(j,i)) thtmp(indth(j,i)+1:indth(j,i+1))']);
        mtmp = [mtmp md];
    end
    
    estinfo = mtmp.estimationinfo;
    estinfo.Status =  ['estimated ' 'model'];
    estinfo.Method =  'new_rfir';
    estinfo.DataLength =  Nd;
    estinfo.DataDomain =  'Time';
    estinfo.InitialState ='Zero';
    estinfo.IniHyperparameter = hpini{j}';
    estinfo.Hyperparameter = hp{j}';
    estinfo.Hyperpartition = indhp(j,:)';
    estinfo.Kernel = [kernel{j,:}];
    estinfo.Optimizationinfo = struct('Minimizer','fmincon', 'options', options, 'Gradient',gradient{j},'Hessian', hessian{j},'Exitflag', exflg(j),'Optinfo', otpt{j});
    estinfo.obj = obj(j);
    estinfo.cond =  condNb;
    estinfo.datalength = Nd;
    mtmp.noi = noi(j);
    try
        mtmp.est = estinfo;
    catch
    end
    if Ny == 1
        M = mtmp;
    else
        if j == 1
            M = idarx(mtmp);
        else
            M = [M;idarx(mtmp)];
        end
    end
end



function [hpini, Rt, Ney, ind2, lb, ub] = ini_rfir_miso(z,nb,nk,kernel,options)

Nu = size(z,3);

% construct the regression matrix Phi for sys-id
[Rt, Ney] = qrfactor_Phi(z,nb,nk);


% Initialize the prior hyperparameter
ind2 = zeros(1,Nu);
hpini = [];
lb = [];
ub = [];
tol = sqrt(eps);
for ni = 1:Nu
beta =(0.5:0.01:0.9)';
hpr = [-2:0.5:10];
    
    if strcmp(kernel{ni}, 'CS')
        beta = 1;
        lbi = -inf;
        ubi = +inf;
    end
    if strcmp(kernel{ni}, 'SE')
        lbi = [tol  -inf]';
        ubi = [+inf +inf]';
    end
    if sum(strcmp(kernel{ni}, {'SS' 'HF' 'DI' 'TC' 'NEW1'}))
        lbi = [tol -inf]';
        ubi = [1-tol +inf]';
    end
    if strcmp(kernel{ni}, 'DC')
        lbi = [tol -1+tol -inf]';
        ubi = [1-tol 1-tol +inf]';
    end
    if strcmp(kernel{ni}, 'NEW2')
        lbi = [tol tol -inf]';
        ubi = [1-tol 1-tol +inf]';
    end
    
    if strcmp(kernel{ni}, 'NEW3')
        lbi = [tol -5 -inf]';
        ubi = [1-tol +10 +inf]';
    end
    
    if Nu == 1
        Rti = Rt;
    else
        Rti = qrfactor_Phi(z(:,:,ni),nb(ni),nk(ni));
    end
    
    if ~sum(strcmp(kernel{ni},{'NEW2' 'NEW3'}))
        obj = zeros(size(beta,1),size(hpr,2));
        for nj = 1:size(beta,1)
            for nm = 1:size(hpr,2)
                hpstart = [beta(nj)*ones(1,~strcmp(kernel{ni}, 'CS')) 0.99*ones(1,strcmp(kernel{ni}, 'DC')) hpr(nm)]';
                obj(nj,nm) = nglglklhd_simp(hpstart,Rti,Ney,kernel(ni),[0 nb(ni)+1-nk(ni)], [0 length(hpstart)]);
            end
        end
        [~, ind_bbeta] = min(obj(:));
        [indr, indc] = ind2sub([nj nm],ind_bbeta);
        hptmp = [beta(indr)*ones(1,~strcmp(kernel{ni}, 'CS')) 0.99*ones(1,strcmp(kernel{ni}, 'DC')) hpr(indc)]';
    else
        if strcmp(kernel{ni},'NEW2')
            betahpr = beta;
        else
            betahpr = hpr;
        end
        obj = zeros(size(beta,1),size(betahpr,1),size(hpr,2));
        for nj = 1:size(beta,1)
            for nr = 1:size(betahpr,1)
                for nm = 1:size(hpr,2)
                    hpstart = [beta(nj) betahpr(nr) hpr(nm)]';
                    obj(nj,nr,nm) = nglglklhd_simp(hpstart,Rti,Ney,kernel(ni),[0 nb(ni)+1-nk(ni)], [0 length(hpstart)]);
                end
            end
        end
        [~, ind_bbeta] = min(obj(:));
        [indj, indr, indc] = ind2sub([nj nr nm],ind_bbeta);
        hptmp = [beta(indj) betahpr(indr) hpr(indc)]';
    end
    
    
    if Nu > 1
        hpstart = hptmp;
        ff = @(x)nglglklhd(x,Rti,Ney,kernel(ni),[0 nb(ni)+1-nk(ni)], [0 length(hpstart)]);
        hptmp = fmincon(ff,hpstart,[],[],[],[],lbi,ubi,[],options);
    end
    
    hpini = [hpini; hptmp];
    lb = [lb; lbi];
    ub = [ub; ubi];
    ind2(ni) = length(hpstart);
    
end
ind2 = cumsum(ind2); ind2=[0 ind2];



function [Rt, Ney] = qrfactor_Phi(z,nb,nk)

ind = cumsum(nb+1-nk); ind = [0 ind]; % index of the estimated FIR parameters for each of Nu blocks
Nth = ind(end);
Rt = zeros(0,Nth+1);


dom = pvget(z,'Domain');

if lower(dom(1)) == 't'
    
    y = pvget(z,'OutputData');
    u = pvget(z,'InputData');

    [Nd, ~, Nu, Ne] = size(z);
    Ney = sum(Nd-max(nb));
    yt = zeros(Ney,1);
    Phi = zeros(Nth,Ney);
    nt = cumsum(Nd-max(nb)); nt = [0 nt];
    
    for ni = 1:Nu
        for ne = 1:Ne
            for nj = 1:Nd(ne)-max(nb)
                Phi(ind(ni)+1:ind(ni+1),nj+nt(ne)) = flipud(u{ne}(nj+max(nb)-nb(ni):(nj+max(nb)-nk(ni)),ni));
            end
        end
    end
    
    for ne = 1:Ne
        yt(nt(ne)+1:nt(ne+1)) = y{ne}(max(nb)+1:end);
    end
    
    % Calculate the QR factor of Phi and yt
    Rt = triu(qr([Phi' yt]));
    try
        Rt = Rt(1:Nth+1,:);
    catch
        Rt = [Phi' yt];
    end
    
end

if lower(dom(1)) == 'f'
    
    if min(z.Frequency) == 0
        zt = ifft(z);
        z = fft(zt,size(zt,1),'compl');
    end
    nz = nyqcut(complex(z));
    y = pvget(nz,'OutputData');
    u = pvget(nz,'InputData');
    w = pvget(nz,'Radfreqs');
    Ts = pvget(nz,'Ts');
    
    [Nd, ~, Nu, Ne] = size(nz);
    Ney = sum(Nd);
    nt = cumsum(Nd); nt = [0 nt];   
    yt = zeros(Ney,1);
    Phi = zeros(Nth,Ney);
    
    for ne = 1:Ne
        for ni = 1:Nu
            OM = exp(-1i*(nk(ni):nb(ni))'*w{ne}'*Ts{ne});
            tempM = (OM.').*(u{ne}(:,ni)*ones(1,nb(ni)-nk(ni)+1));
            Phi(ind(ni)+1:ind(ni+1),nt(ne)+1:nt(ne+1)) = tempM.';
        end
        yt(nt(ne)+1:nt(ne+1)) = y{ne};
    end
    
    % Calculate the QR factor of Phi and yt
    if norm(imag(triu(qr([Phi.' yt])))) > 1e-4
        warning('The frequency domain data processing is not handled appropriately!')
    end
    Rt = real(triu(qr([Phi.' yt])));
    try
        Rt = Rt(1:Nth+1,:);
    catch
        Rt = [Phi.' yt];
    end
    
end


function [obj, condNb, xest, noi] = nglglklhd_simp(hp,Rt,Ney, kernel,ind,ind2)

warning('off', 'MATLAB:nearlySingularMatrix');

Nth = size(Rt,2) -1;
Nu = size(kernel,2);
P_chol = zeros(Nth,Nth);


chol_flag = 1;
cond_flag = 1;

for di = 1:Nu
    
    hyper = hp(ind2(di)+1:ind2(di+1));
    nPi = ind(di+1)-ind(di);
    Pi = zeros(nPi,nPi);
    
    switch kernel{di}
        
        case 'CS'
            for dk=1:nPi
                for dj=1:nPi
                    Pi(dk,dj) = dk*dj*min(dk,dj)/2-min(dk,dj)^3/6;
                end
            end
            
        case 'SE'
            for dk=1:nPi
                for dj=1:nPi
                    Pi(dk,dj) = exp(-(dk-dj)^2/(2*hyper(1)^2));
                end
            end
            
            
        case 'DI'
            t=abs(hyper(1)).^(1:nPi);%exp(-beta*[1:nb])
            Pi=diag(t);
            
        case 'SS'
            t=abs(hyper(1)).^(1:nPi);%exp(-beta*[1:nb])
            for dk=1:nPi
                for dj=1:nPi
                    Pi(dk,dj) = t(dk)*t(dj)*min(t(dk),t(dj))/2-min(t(dk),t(dj))^3/6;
                end
            end
            
        case 'HF'
            t=abs(hyper(1)).^(1:nPi);%exp(-beta*[1:nb])
            for dk=1:nPi
                for dj=1:nPi
                    if mod(dk+dj,2) == 0
                        Pi(dk,dj) = min(t(dk),t(dj));
                    else
                        Pi(dk,dj) = -min(t(dk),t(dj));
                    end
                end
            end
            
        case 'TC'
            t=abs(hyper(1)).^(1:nPi); %exp(-beta*[1:nb])
            for dk=1:nPi
                for dj=1:nPi
                    Pi(dk,dj) =min(t(dk),t(dj));
                end
            end
            
        case 'DC'
            for dk = 1:nPi
                for dj = 1:nPi
                    Pi(dk,dj) = abs(hyper(1))^((dk+dj)/2)*hyper(2)^(abs(dk-dj));
                end
            end
        case 'NEW1'
            %             for dk = 1:nPi
            %                 for dj = 1:nPi
            %                     %                 Pi(dk,dj) = min(abs(hyper(1))^dk,abs(hyper(1))^dj)-abs(hyper(1))^(dk+dj);
            %                     Pi(dk,dj) = min(dk,dj)*abs(hyper(1))^(dk+dj);
            %                 end
            %             end
                        for dk = 1:nPi
                            for dj = 1:nPi
                                Pi(dk,dj) = (hyper(1))^max(dk,dj)+(hyper(1))^(dk+dj);
                            end
                        end
%             for dk = 1:nPi
%                 for dj = 1:nPi
%                     Pi(dk,dj) = (1-(min(dk,dj)+1)^(-1))*(hyper(1))^(dk+dj);
%                 end
%             end
        case 'NEW2'
            for dk = 1:nPi
                for dj = 1:nPi
                    if hyper(1) == hyper(2)
                        %                 Pi(dk,dj) = min(abs(hyper(1))^dk,abs(hyper(1))^dj)-abs(hyper(1))^(dk+dj);
                        Pi(dk,dj) = min(dk,dj)*(hyper(1))^(dk+dj);
                    else
                        Pi(dk,dj) = sign(hyper(2)-hyper(1))*((hyper(1))^(max(dk,dj)-min(dk,dj))*(hyper(2))^(2*min(dk,dj)) - (hyper(1))^(dk+dj));
                    end
                end
            end
        case 'NEW3'
            for dk = 1:nPi
                for dj = 1:nPi
%                     Pi(dk,dj) = (hyper(1))^(max(dk,dj)) + (exp(hyper(2))-1)*(hyper(1))^(dk+dj);
                    Pi(dk,dj) = (1+exp(hyper(2))-(min(dk,dj)+1)^(-1))*(hyper(1))^(dk+dj);
                end
            end
            
    end
    
    if cond(Pi) > 1e+100
        cond_flag = 0;
    end
    
    try
        U = chol(Pi)'*sqrt(exp(hyper(end)));
    catch
        chol_flag = 0;
        U=chol(Pi+1e-4*eye(nPi))'*sqrt(exp(hyper(end)));
    end
    P_chol(ind(di)+1:ind(di+1),ind(di)+1:ind(di+1)) = U;
    
end


R1 = real(triu(qr([Rt(:,1:Nth)*P_chol Rt(:,Nth+1); eye(Nth) zeros(Nth,1)])));
R1 = R1(1:Nth+1,:);

if cond_flag ~= 1 || chol_flag ~=1
    obj = 1/eps;
else
    obj = Ney*log(R1(end,Nth+1)^2) + 2*sum(log(abs(diag(R1(1:Nth,1:Nth)))))  - Ney*log(Ney) + Ney;
end

if nargout >1
    condNb = cond((R1(1:Nth,1:Nth)'*R1(1:Nth,1:Nth)));
    xest = P_chol/R1(1:Nth,1:Nth)*R1(1:Nth,Nth+1);
    noi = R1(end,Nth+1)^2/Ney;
end


function [obj, grad, hesn] = nglglklhd(hp,Rt, Ney, kernel,ind,ind2)

warning('off', 'PiATLAB:nearlySingularPiatrix');

Nth = size(Rt,2)-1;
Nu = size(kernel,2);
P = zeros(Nth,Nth);
P_chol = zeros(Nth,Nth);
P_invsqr = zeros(Nth,Nth); % square root of the inverse of P
Grd = cell(Nu,1);
Hsn = cell(Nu,1);
grad = zeros(length(hp),1);
hesn = zeros(length(hp),length(hp));

chol_flag = 1;
cond_flag = 1;

for di = 1:Nu
    
    hyper = hp(ind2(di)+1:ind2(di+1));
    nPi = ind(di+1)-ind(di);
    Pi = zeros(nPi,nPi);
    J1 = Pi; J2 = Pi; H11 = Pi; H12 = Pi; H22 = Pi;
    
    switch kernel{di}
        
        case 'CS'
            for dk=1:nPi
                for dj=1:nPi
                    Pi(dk,dj) = dk*dj*min(dk,dj)/2-min(dk,dj)^3/6;
                end
            end
            
        case 'SE'
            for dk=1:nPi
                for dj=1:nPi
                    Pi(dk,dj) = exp(-(dk-dj)^2/(2*hyper(1)^2));
                    J1(dk,dj) = exp(-(dk-dj)^2/(2*hyper(1)^2))*(dk-dj)^2/hyper(1)^3;
                    H11(dk,dj) = exp(-(dk-dj)^2/(2*hyper(1)^2))*(dk-dj)^2/hyper(1)^4*((dk-dj)^2/hyper(1)^2 - 3);
                end
            end
            Grd{di} = exp(hyper(end))*J1;
            Hsn{di} = exp(hyper(end))*H11;
            
        case 'DI'
            t = hyper(1).^(1:nPi);%exp(-beta*[1:nb])
            td = [1 (2:nPi).*hyper(1).^(1:nPi-1)];
            tdd = [0 2 (2:nPi-1).*((3:nPi).*hyper(1).^(1:nPi-2))];
            Pi = diag(t);
            J1 = diag(td);
            H11 = diag(tdd);
            Grd{di} = exp(hyper(end))*J1;
            Hsn{di} = exp(hyper(end))*H11;
            
        case 'SS'
            for dk=1:nPi
                for dj=1:nPi
                    if dk >= dj
                        Pi(dk,dj) = hyper(1)^(2*dk+dj)/2 - hyper(1)^(3*dk)/6;
                        J1(dk,dj) = (2*dk+dj)*hyper(1)^(2*dk+dj-1)/2 - dk*hyper(1)^(3*dk-1)/2;
                        H11(dk,dj) = (2*dk+dj-1)*(2*dk+dj)*hyper(1)^(2*dk+dj-2)/2 - (3*dk-1)*dk*hyper(1)^(3*dk-2)/2;
                    else
                        Pi(dk,dj) = hyper(1)^(2*dj+dk)/2 - hyper(1)^(3*dj)/6;
                        J1(dk,dj) = (2*dj+dk)*hyper(1)^(2*dj+dk-1)/2 - dj*hyper(1)^(3*dj-1)/2;
                        H11(dk,dj) = (2*dj+dk-1)*(2*dj+dk)*hyper(1)^(2*dj+dk-2)/2 - (3*dj-1)*dj*hyper(1)^(3*dj-2)/2;
                    end
                end
            end
            Grd{di} = exp(hyper(end))*J1;
            Hsn{di} = exp(hyper(end))*H11;
            
        case 'HF'
            t = hyper(1).^(1:nPi);%exp(-beta*[1:nb])
            td = [1 (2:nPi).*hyper(1).^(1:nPi-1)];
            tdd = [0 2 (2:nPi-1).*((3:nPi).*hyper(1).^(1:nPi-2))];
            for dk=1:nPi
                for dj=1:nPi
                    if mod(dk+dj,2) == 0
                        Pi(dk,dj) = min(t(dk),t(dj));
                        J1(dk,dj) = min(td(dk),td(dj));
                        H11(dk,dj) = min(tdd(dk),tdd(dj));
                    else
                        Pi(dk,dj) = -min(t(dk),t(dj));
                        J1(dk,dj) = -min(td(dk),td(dj));
                        H11(dk,dj) = -min(tdd(dk),tdd(dj));
                    end
                end
            end
            Grd{di} = exp(hyper(end))*J1;
            Hsn{di} = exp(hyper(end))*H11;
            
        case 'TC'
            t = hyper(1).^(1:nPi);%exp(-beta*[1:nb])
            td = [1 (2:nPi).*hyper(1).^(1:nPi-1)];
            tdd = [0 2 (2:nPi-1).*((3:nPi).*hyper(1).^(1:nPi-2))];
            for dk=1:nPi
                for dj=1:nPi
                    Pi(dk,dj) = min(t(dk),t(dj));
                    J1(dk,dj) = min(td(dk),td(dj));
                    H11(dk,dj) = min(tdd(dk),tdd(dj));
                end
            end
            Grd{di} = exp(hyper(end))*J1;
            Hsn{di} = exp(hyper(end))*H11;
            
        case 'DC'
            for dk = 1:nPi
                for dj = 1:nPi
                    Pi(dk,dj) = hyper(1)^((dk+dj)/2)*hyper(2)^(abs(dk-dj));
                    J1(dk,dj) = (dk+dj)/2*hyper(1)^((dk+dj)/2-1)*hyper(2)^(abs(dk-dj));
                    if dk == dj
                        J2(dk,dj) = 0;
                        H12(dk,dj) = 0;
                    else
                        J2(dk,dj) = abs(dk-dj)*hyper(1)^((dk+dj)/2)*hyper(2)^(abs(dk-dj)-1);
                        H12(dk,dj) = abs(dk-dj)*(dk+dj)/2*hyper(1)^((dk+dj)/2-2)*hyper(2)^(abs(dk-dj)-1);
                    end
                    H11(dk,dj) = ((dk+dj)/2-1)*(dk+dj)/2*hyper(1)^((dk+dj)/2-2)*hyper(2)^(abs(dk-dj));
                    if dk == dj || abs(dk-dj) == 1
                        H22(dk,dj) =0;
                    else
                        H22(dk,dj) = (abs(dk-dj)-1)*abs(dk-dj)*hyper(1)^((dk+dj)/2)*hyper(2)^(abs(dk-dj)-2);
                    end
                end
            end
            Grd{di} = exp(hyper(end))*[J1 J2];
            Hsn{di} = exp(hyper(end))*[H11 H12 H22];
            
        case 'NEW1'
            %             for dk = 1:nPi
            %                 for dj = 1:nPi
            %                     % Pi(dk,dj) = min(abs(hyper(1))^dk,abs(hyper(1))^dj)-abs(hyper(1))^(dk+dj);
            %                     Pi(dk,dj) = min(dk,dj)*abs(hyper(1))^(dk+dj);
            %                     J1(dk,dj) = (dk+dj)*min(dk,dj)*abs(hyper(1))^(dk+dj-1);
            %                     H11(dk,dj) = (dk+dj)*(dk+dj-1)*min(dk,dj)*abs(hyper(1))^(dk+dj-2);
            %                 end
            %             end
                        for dk = 1:nPi
                            for dj = 1:nPi
                                Pi(dk,dj) = (hyper(1))^max(dk,dj)+(hyper(1))^(dk+dj);
                                J1(dk,dj) = max(dk,dj)*((hyper(1))^(max(dk,dj)-1)) + (dk+dj)*(hyper(1))^(dk+dj-1);
                                H11(dk,dj) = (max(dk,dj)-1)*max(dk,dj)*((hyper(1))^(max(dk,dj)-2)) + (dk+dj-1)*(dk+dj)*(hyper(1))^(dk+dj-2);
                            end
                        end
%             for dk = 1:nPi
%                 for dj = 1:nPi
%                     Pi(dk,dj) = (1-(min(dk,dj)+1)^(-1))*(hyper(1))^(dk+dj);
%                     J1(dk,dj) = (dk+dj)*(1-(min(dk,dj)+1)^(-1))*(hyper(1))^(dk+dj-1);
%                     H11(dk,dj) = (dk+dj-1)*(dk+dj)*(1-(min(dk,dj)+1)^(-1))*(hyper(1))^(dk+dj-2);
%                 end
%             end
            Grd{di} = exp(hyper(end))*J1;
            Hsn{di} = exp(hyper(end))*H11;
            
        case 'NEW2'
            for dk = 1:nPi
                for dj = 1:nPi
                    if hyper(1) == hyper(2)
                        % Pi(dk,dj) = min(abs(hyper(1))^dk,abs(hyper(1))^dj)-abs(hyper(1))^(dk+dj);
                        Pi(dk,dj) = min(dk,dj)*abs(hyper(1))^(dk+dj);
                        J1(dk,dj) = (dk+dj)*min(dk,dj)*abs(hyper(1))^(dk+dj-1);
                        H11(dk,dj) = (dk+dj)*(dk+dj-1)*min(dk,dj)*abs(hyper(1))^(dk+dj-2);
                        J2(dk,dj) = J1 (dk,dj);
                        H12(dk,dj) = H11(dk,dj);
                        H22(dk,dj) = H11(dk,dj);
                    else
                        Pi(dk,dj) = sign(hyper(2)-hyper(1))*((hyper(1))^(max(dk,dj)-min(dk,dj))*(hyper(2))^(2*min(dk,dj)) - (hyper(1))^(dk+dj));
                        J1(dk,dj) = sign(hyper(2)-hyper(1))*((max(dk,dj)-min(dk,dj))*(hyper(1))^(max(dk,dj)-min(dk,dj)-1)*(hyper(2))^(2*min(dk,dj)) - (dk+dj)*(hyper(1))^(dk+dj-1));
                        J2(dk,dj) = sign(hyper(2)-hyper(1))*2*min(dk,dj)*(hyper(1))^(max(dk,dj)-min(dk,dj))*(hyper(2))^(2*min(dk,dj)-1);
                        H11(dk,dj) = sign(hyper(2)-hyper(1))*((max(dk,dj)-min(dk,dj)-1)*(max(dk,dj)-min(dk,dj))*(hyper(1))^(max(dk,dj)-min(dk,dj)-2)*(hyper(2))^(2*min(dk,dj)) ...
                            - (dk+dj-1)*(dk+dj)*(hyper(1))^(dk+dj-2));
                        H12(dk,dj) = sign(hyper(2)-hyper(1))*2*min(dk,dj)*(max(dk,dj)-min(dk,dj))*(hyper(1))^(max(dk,dj)-min(dk,dj)-1)*(hyper(2))^(2*min(dk,dj)-1);
                        H22(dk,dj) = sign(hyper(2)-hyper(1))*(2*min(dk,dj)-1)*(2*min(dk,dj))*(hyper(1))^(max(dk,dj)-min(dk,dj))*(hyper(2))^(2*min(dk,dj)-2);
                    end
                end
            end
            Grd{di} = exp(hyper(end))*[J1 J2];
            Hsn{di} = exp(hyper(end))*[H11 H12 H22];
        case 'NEW3'
            for dk = 1:nPi
                for dj = 1:nPi
%                     Pi(dk,dj) = (hyper(1))^(max(dk,dj)) + (exp(hyper(2))-1)*(hyper(1))^(dk+dj);
%                     J1(dk,dj) = max(dk,dj)*((hyper(1))^(max(dk,dj)-1)) + (dk+dj)*(exp(hyper(2))-1)*(hyper(1))^(dk+dj-1);
%                     H11(dk,dj) = (max(dk,dj)-1)*max(dk,dj)*((hyper(1))^(max(dk,dj)-2)) + (dk+dj-1)*(dk+dj)*(exp(hyper(2))-1)*(hyper(1))^(dk+dj-2);
%                     H12(dk,dj) = (dk+dj)*exp(hyper(2))*(hyper(1))^(dk+dj-1);
%                     J2(dk,dj) = exp(hyper(2))*(hyper(1))^(dk+dj);
%                     H22(dk,dj) = J2(dk,dj);
                    Pi(dk,dj) = (1+exp(hyper(2))-(min(dk,dj)+1)^(-1))*(hyper(1))^(dk+dj);
                    J1(dk,dj) = (dk+dj)*(1+exp(hyper(2))-(min(dk,dj)+1)^(-1))*(hyper(1))^(dk+dj-1);
                    H11(dk,dj) = (dk+dj-1)*(dk+dj)*(1+exp(hyper(2))-(min(dk,dj)+1)^(-1))*(hyper(1))^(dk+dj-2);
                    H12(dk,dj) = (dk+dj)*exp(hyper(2))*(hyper(1))^(dk+dj-1);
                    J2(dk,dj) = exp(hyper(2))*(hyper(1))^(dk+dj);
                    H22(dk,dj) = J2(dk,dj);
                end
            end
            Grd{di} = exp(hyper(end))*[J1 J2];
            Hsn{di} = exp(hyper(end))*[H11 H12 H22];
    end
    
    P(ind(di)+1:ind(di+1),ind(di)+1:ind(di+1)) = exp(hyper(end))*Pi;
    
    % Terminite if the condition number of the regularization marix is too large
    if cond(Pi) > 1e+100
        cond_flag = 0;
    end
    
    try
        U = chol(Pi)';
        P_invsqr(ind(di)+1:ind(di+1),ind(di)+1:ind(di+1)) = sqrt(1/exp(hyper(end)))*eye(nPi)/U;
        U = U*sqrt(exp(hyper(end)));
    catch
        % Terminite if the cholesky decomposition fails
        chol_flag = 0;
        U = chol(Pi+1e-4*eye(nPi))';
        P_invsqr(ind(di)+1:ind(di+1),ind(di)+1:ind(di+1)) = sqrt(1/exp(hyper(end)))*eye(nPi)/U;
        U = U*sqrt(exp(hyper(end)));
    end
    P_chol(ind(di)+1:ind(di+1),ind(di)+1:ind(di+1)) = U;
end


R1 = real(triu(qr([Rt(:,1:Nth)*P_chol Rt(:,Nth+1); eye(Nth) zeros(Nth,1)])));
R1 = R1(1:Nth+1,:);

if  cond_flag ~= 1 || chol_flag ~=1
    obj = 1/eps;
else
    obj = Ney*log(R1(end,Nth+1)^2) + 2*sum(log(abs(diag(R1(1:Nth,1:Nth))))) - Ney*log(Ney) + Ney;
end




%  calculate gradient and hessian
tmp = eye(Nth)/R1(1:Nth,1:Nth)'; % R1^{-T}
for di = 1:Nu
    
    nh = ind2(di+1) - ind2(di);
    Pi = P(ind(di)+1:ind(di+1),ind(di)+1:ind(di+1));
    Pi_inv = P_invsqr(ind(di)+1:ind(di+1),ind(di)+1:ind(di+1));
    
    tmp1 = tmp(1:Nth,ind(di)+1:ind(di+1))*Pi_inv; % R1^{-T}*L^{-1}
    tmp2 = (tmp1'*tmp1); % simga* L^{-T}R1^{-1}R1^{-T}*L^{-1}
    
    X1 = (Pi_inv'*Pi_inv) - tmp2; % L^{-T}L^{-1} - simga* L^{-T}R1^{-1}R1^{-T}*L^{-1}
    tmp3 = R1(1:Nth,end)'*tmp1; % R2^T R1^{-T}*L^{-1}
    X2 = (tmp3'*tmp3); % L^{-1} R1^{-1} R2 * R2^T R1^{-T}*L^{-1}
    Xd = X1 - X2*Ney/R1(end,Nth+1)^2;
    
    if nh == 1
        grad(ind2(di)+1) = Xd(:)'*Pi(:);
        
        Htmplogdet = - X1'*Pi*X1; % Hessian for the log det term
        Xtmp = X1*Pi*X2;
        Htmpmf = Xtmp + Xtmp'; % Hessian for the model fit part Y^T[...]^{-1}Y
        Htmp = Htmplogdet + Htmpmf*Ney/R1(end,Nth+1)^2;
        Htmpmfextra = -(grad(ind2(di)+1))^2*Ney/R1(end,Nth+1)^4;
        hesn(ind2(di)+1,ind2(di)+1) = grad(ind2(di)+1) + Htmp(:)'*Pi(:) + Htmpmfextra;
        
    else if nh == 2
            J1 = Grd{di};
            grad(ind2(di)+1) = Xd(:)'*J1(:);
            grad(ind2(di)+2) = Xd(:)'*Pi(:);
            
            H11 = Hsn{di};
            Htmplogdet = - X1'*J1*X1;
            Xtmp = X1*J1*X2;
            Htmpmf = Xtmp + Xtmp';
            Htmp = Htmplogdet + Htmpmf*Ney/R1(end,Nth+1)^2;
            Htmpmfextra = -(grad(ind2(di)+1))^2*Ney/R1(end,Nth+1)^4;
            hesn(ind2(di)+1,ind2(di)+1) = Xd(:)'* H11(:) + Htmp(:)'*J1(:)  + Htmpmfextra;
            
            Htmpmfextra = -grad(ind2(di)+1)*grad(ind2(di)+2)*Ney/R1(end,Nth+1)^4;
            hesn(ind2(di)+1,ind2(di)+2) = grad(ind2(di)+1) + Htmp(:)'*Pi(:) + Htmpmfextra;
            hesn(ind2(di)+2,ind2(di)+1) = hesn(ind2(di)+1,ind2(di)+2);
            
            Htmplogdet = - X1'*Pi*X1;
            Xtmp = X1*Pi*X2;
            Htmpmf = Xtmp + Xtmp';
            Htmp = Htmplogdet + Htmpmf*Ney/R1(end,Nth+1)^2;
            Htmpmfextra = -(grad(ind2(di)+2))^2*Ney/R1(end,Nth+1)^4;
            hesn(ind2(di)+2,ind2(di)+2) = grad(ind2(di)+2) + Htmp(:)'*Pi(:) + Htmpmfextra;
            
        else if nh == 3
                J = Grd{di};
                J1 = J(:,1:ind(di+1)-ind(di));
                J2 = J(:,ind(di+1)-ind(di)+1:end);
                grad(ind2(di)+1) = Xd(:)'*J1(:);
                grad(ind2(di)+2) = Xd(:)'*J2(:);
                grad(ind2(di)+3) = Xd(:)'*Pi(:);
                H = Hsn{di};
                H11 = H(:,1:ind(di+1)-ind(di)); H12 = H(:,ind(di+1)-ind(di)+1:2*(ind(di+1)-ind(di))); H22 = H(:,2*(ind(di+1)-ind(di))+1:end);
                
                Htmplogdet = - X1'*J1*X1;
                Xtmp = X1*J1*X2;
                Htmpmf = Xtmp + Xtmp';
                Htmp = Htmplogdet + Htmpmf*Ney/R1(end,Nth+1)^2;
                Htmpmfextra = -(grad(ind2(di)+1))^2*Ney/R1(end,Nth+1)^4;
                hesn(ind2(di)+1,ind2(di)+1) = Xd(:)'* H11(:) + Htmp(:)'*J1(:)  + Htmpmfextra;
                
                Htmpmfextra = -grad(ind2(di)+1)*grad(ind2(di)+2)*Ney/R1(end,Nth+1)^4;
                hesn(ind2(di)+1,ind2(di)+2) = Xd(:)'* H12(:) + Htmp(:)'*J2(:) + Htmpmfextra;
                hesn(ind2(di)+2,ind2(di)+1) = hesn(ind2(di)+1,ind2(di)+2);
                
                Htmpmfextra = -grad(ind2(di)+1)*grad(ind2(di)+3)*Ney/R1(end,Nth+1)^4;
                hesn(ind2(di)+1,ind2(di)+3) = grad(ind2(di)+1) + Htmp(:)'*Pi(:) + Htmpmfextra;
                hesn(ind2(di)+3,ind2(di)+1) = hesn(ind2(di)+1,ind2(di)+3);
                
                Htmplogdet = - X1'*J2*X1;
                Xtmp = X1*J2*X2;
                Htmpmf = Xtmp + Xtmp';
                Htmp = Htmplogdet + Htmpmf*Ney/R1(end,Nth+1)^2;
                Htmpmfextra = -(grad(ind2(di)+2))^2*Ney/R1(end,Nth+1)^4;
                hesn(ind2(di)+2,ind2(di)+2) = Xd(:)'* H22(:) + Htmp(:)'*J2(:)  + Htmpmfextra;
                
                Htmpmfextra = -grad(ind2(di)+2)*grad(ind2(di)+3)*Ney/R1(end,Nth+1)^4;
                hesn(ind2(di)+2,ind2(di)+3) = grad(ind2(di)+2) + Htmp(:)'*Pi(:) + Htmpmfextra;
                hesn(ind2(di)+3,ind2(di)+2) = hesn(ind2(di)+2,ind2(di)+3);
                
                
                Htmplogdet = - X1'*Pi*X1;
                Xtmp = X1*Pi*X2;
                Htmpmf = Xtmp + Xtmp';
                Htmp = Htmplogdet + Htmpmf*Ney/R1(end,Nth+1)^2;
                Htmpmfextra = -(grad(ind2(di)+3))^2*Ney/R1(end,Nth+1)^4;
                hesn(ind2(di)+3,ind2(di)+3) = grad(ind2(di)+3) + Htmp(:)'*Pi(:) + Htmpmfextra;
            end
        end
    end
    
end