function tree(objectorder,heights)

%TREE creates a tree in which the leaves represent
%   objects.  The vertical coordinate of the junction
%   of two branches is the dissimilarity between the
%   corresponding clusters (maximal 30 objects allowed).
%
% The algorithm is fully described in:
%   Kaufman, L. and Rousseeuw, P.J. (1990),
%   "Finding groups in data: An introduction to cluster analysis",
%   Wiley-Interscience: New York (Series in Applied Probability and
%   Statistics), ISBN 0-471-87876-6.
%
% Required input arguments:
%   objectorder :  order of objects
%   heights     : diameter of cluster before dividing it
%                 (=length of banner)
%
% I/O:
%   tree(objectorder,heights)
%
% Example (subtracted from the referenced book)
%   load agricul.mat
%   result = diana(agricul,[4 4],0,0,1);
%   tree(result.objectorder,result.heights)
%
% The output of TREE is a figure containing the
%   agglomerative (agnes) or divise (diana) tree.
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by Wai Yan Kong (May 2006)
% Last Revision: 28/09/2006

clf reset
whitebg([1 1 1]);

if (nargin<2)
    error('Two input arguments required')
elseif (nargin>2)
    error('Too many input arguments')
end

if(size(objectorder,2)~=size(heights,2)+1)
    error('Missing values in objectorder or heights')
end

Heights=H(heights);

number=size(objectorder,2);

if (number>30)
    error('Only 30 objects allowed')
end

%midden=[];
middle(1)=0;
Maxi=0;
Mini=0;
PrevMID=middle(1);
lengt=[]; %length
high=[]; %height
indices=[];
L=[];

[maxim,index]=max(Heights);
if(1<=index-1)
    [Prevmax,Previndex]=max(Heights(1:(index-1)));
else
    Prevmax=maxim;
    Previndex=index;
end
if(index+1<=number-1)
    [Postmax,Postindex]=max(Heights((index+1):(number-1)));
    Postindex=Postindex+index+1-1;
else
    Postmax=maxim;
    Postindex=index;
end

if(Postindex+1<=number-1)
    [Post2max,Post2index]=max(Heights(Postindex+1:number-1));
    Post2index=Post2index+Postindex+1-1;
else
    Post2max=Postmax;
    Post2index=Postindex;
end

if(Postindex+1<=Post2index-1)
    [Betweenmax,Betweenindex]=max(Heights(index+1:Postindex-1));
    Betweenindex=Betweenindex+index+1-1;
else
    Betweenmax=Postmax;
    Betweenindex=Postindex;
end

L=cat(2,L,maxim);

high=cat(2,high,[Post2max,Postmax,Betweenmax,maxim,Prevmax]);
indices=cat(2,indices,[Post2index,Postindex,Betweenindex,index,Previndex]);

M=0;
extra=number/2;
lengt(1)=Postindex-Previndex+extra+2;
PrevLEN=lengt(1);
rectangle('Position',[middle(1)-(lengt(1)/2),maxim,lengt(1),0.0001]);

NbanFirst=1;
k=1;
branch=0;
over=0;
Element=0;
Special=0;
Spec=0;
Sp=0;
S=0;
ww=0;
if(maxim<20)
    extrawaystick=0.8;
    extrawaytext=1.5;
else
    extrawaytext=0;
    extrawaystick=0;
end

right=0;
direct11=0;
direct111=0;
righttree=0;
if(index==1)
    direct1=1;
else
    direct1=0;
end

Last=size(lengt,2);
LastM=size(middle,2);
LastL=size(lengt,2);

while(k<=number)
    w=0;
    LastH=size(high,2);
    if((high(LastH)~=high(LastH-1)) & Element==0)
        while(branch==0)
            S=0;
            w=w+1;
            ww=ww+1;

            if(over==0)
                middle=cat(2,middle,middle(LastM)-(lengt(LastL)/2));
            end

            if(righttree==1)
                middle=cat(2,middle,middle(LastM)-(lengt(LastL)/2));
                righttree=0;
            end

            if(Sp==1)
                LastM=size(middle,2);
                LastL=size(lengt,2);
                middle(LastM)=middle(LastM)+lengt(LastL)/2;
                Sp=0;
                S=1;
            end
            LastM=size(middle,2);

            Output=maxim-Prevmax;
            rectangle('Position',[middle(LastM),Prevmax,0.0001,Output]);

            if(Betweenmax==Postmax | over==0)
                Post2max=Postmax;
                Post2index=Postindex;
            end

            if((Betweenmax==Postmax | over==0) & (right~=1 | over==0))
                Postmax=maxim;
                Postindex=index;
            end

            maxim=Prevmax;
            index=Previndex;

            if(NbanFirst<=(index-1))
                [Prevmax,Previndex]=max(Heights(NbanFirst:(index-1)));
                Previndex=Previndex+NbanFirst-1;
            else
                Prevmax=maxim;
                Previndex=index;
            end

            if(index+1<=Postindex-1)
                [Betweenmax,Betweenindex]=max(Heights(index+1:Postindex-1));
                Betweenindex=Betweenindex+index+1-1;
            else
                Betweenmax=Postmax;
                Betweenindex=Postindex;
            end

            if(right==1 & over~=0)
                high=cat(2,high,[Prevmax Prevmax]);
                indices=cat(2,indices,[Previndex Previndex]);
            else
                high=cat(2,high,Prevmax);
                indices=cat(2,indices,Previndex);
            end

            if(Postindex-Previndex<=0)
                lengt=cat(2,lengt,1);
            else
                if(Postindex-Previndex-direct111>0)
                    if(ww==1)
                        lengt=cat(2,lengt,Postindex-Previndex-direct111+2);
                    else
                        lengt=cat(2,lengt,Postindex-Previndex-direct111);
                    end
                else
                    if(ww==1)
                        lengt=cat(2,lengt,Postindex-Previndex+2);
                    else
                        lengt=cat(2,lengt,Postindex-Previndex);
                    end
                end
            end
            L=cat(2,L,maxim);

            if(direct111~=0)
                direct111=0;
            end

            LastH=size(high,2);
            LastL=size(lengt,2);
            LastM=size(middle,2);

            rectangle('Position',[middle(LastM)-(lengt(LastL)/2),maxim,lengt(LastL),0.0001]);

            if(over~=0)
                over=0;
            end

            if(right~=0)
                right=0;
            end

            if(NbanFirst==index)
                branch=1;
            end
        end

        rectangle('Position',[middle(LastM)-(lengt(LastL)/2),maxim-1+extrawaystick,0.0001,1-extrawaystick]);
        if(objectorder(k)>10)

            text(middle(LastM)-(lengt(LastL)/2)-0.25,maxim-2.0+extrawaytext,num2str(double(objectorder(k))));
            T=middle(LastM)-(lengt(LastL)/2-0.25);
        else

            text(middle(LastM)-(lengt(LastL)/2)-0.1,maxim-2.0+extrawaytext,num2str(double(objectorder(k))));
            T=middle(LastM)-(lengt(LastL)/2-0.1);
        end

        Maxi=max(Maxi,T);
        Mini=min(Mini,T);

        if(Betweenmax~=Postmax)
            high=high(1:LastH-2);
            indices=indices(1:LastH-2);

            Prevmax=Betweenmax;
            Previndex=Betweenindex;

            high=cat(2,high,Prevmax);
            indices=cat(2,indices,Previndex);
            LastH=size(high,2);

            LastL=size(lengt,2);
            middle(LastM)=middle(LastM)+lengt(LastL)/2;
            over=1;
        end
        direct1=0;
        Special=0;
        Spec=0;
    else
        if(Special==1)
            Special=0;
            Spec=1;
        end

        if(Sp==1)
            Sp=0;
        end

        if(S==1)
            S=2;
        end

        direct111=0;
        LastH=size(high,2);
        LastM=size(middle,2);
        LastL=size(lengt,2);

        if(direct1==0)

            rectangle('Position',[middle(LastM)+(lengt(LastL)/2),high(LastH)-1+extrawaystick,0.0001,1-extrawaystick]);

            text(middle(LastM)+lengt(LastL)/2-0.1,high(LastH)-2.0+extrawaytext,num2str(double(objectorder(k))));

            T=middle(LastM)+lengt(LastL)/2-0.1;
            Maxi=max(Maxi,T);
            Mini=min(Mini,T);
        else

            rectangle('Position',[middle(LastM)-(lengt(LastL)/2),high(LastH)-1+extrawaystick,0.0001,1-extrawaystick]);

            text(middle(LastM)-lengt(LastL)/2-0.1,high(LastH)-2.0+extrawaytext,num2str(double(objectorder(k))));

            T=middle(LastM)-lengt(LastL)/2-0.1;
            Maxi=max(Maxi,T);
            Mini=min(Mini,T);
            direct1=0;
            direct11=1;
        end

        if(high(LastH-1)==high(LastH))
            high=high(1:LastH-2);
            indices=indices(1:LastH-2);
        else
            high=high(1:LastH-1);
            indices=indices(1:LastH-1);
        end
        LastH=size(high,2);

        maxim=high(LastH);
        index=indices(LastH);

        if(LastH-1>=1)
            Postmax=high(LastH-1);
            Postindex=indices(LastH-1);
        end

        if(LastH-2>=1)
            Post2max=high(LastH-2);
            Post2index=indices(LastH-2);
        end

        if(index+1<=Postindex-1)
            [Betweenmax,Betweenindex]=max(Heights(index+1:Postindex-1));
            Betweenindex=Betweenindex+index+1-1;
        else
            Betweenmax=Postmax;
            Betweenindex=Postindex;
        end

        if(Betweenmax~=Postmax)
            Prevmax=Betweenmax;
            Previndex=Betweenindex;
        else
            Prevmax=maxim;
            Previndex=index;
        end

        if(high(LastH)>high(LastH-1))
            righttree=1;
        end

        high(LastH)=Prevmax;
        indices(LastH)=Previndex;

        LastM=size(middle,2);
        LastL=size(lengt,2);
        Last=size(L,2);

        if(LastM-1>=1)
            middle=middle(1:LastM-1);
        end

        if(LastL-1>=1)
            lengt=lengt(1:LastL-1);
        end

        if(Last-1>=1)
            L=L(1:Last-1);
        end

        LastM=size(middle,2);
        LastL=size(lengt,2);
        Last=size(L,2);

        if(Last>0)
            while(maxim>L(Last))
                lengt=lengt(1:Last-1);
                L=L(1:Last-1);
                Last=size(L,2);
            end
        end
        LastL=size(lengt,2);

        middle(LastM)=middle(LastM)+(lengt(LastL)/2);

        if(Betweenmax~=Postmax)
            Element=0;
        else
            Element=1;
            middle(LastM)=middle(LastM)-(lengt(LastL)/2);
        end

        if(righttree==1 | direct11==1)
            if(Spec==1)
                Sp=1;
            end

            PrevMAX=L(1);
            [maxim,index]=max(Heights(NbanFirst+1:number-1));
            index=index+NbanFirst+1-1;

            if(NbanFirst+1<=index-1)
                [Prevmax,Previndex]=max(Heights(NbanFirst+1:index-1));
                Previndex=Previndex+NbanFirst+1-1;
            else
                Prevmax=maxim;
                Previndex=index;
            end

            if(index+1<=number-1)
                [Postmax,Postindex]=max(Heights(index+1:number-1));
                Postindex=Postindex+index+1-1;
            else
                Postmax=maxim;
                Postindex=index;
            end

            if(index+1<=Postindex-1)
                [Betweenmax,Betweenindex]=max(Heights(index+1:Postindex-1));
                Betweenindex=Betweenindex+index+1-1;
            else
                Betweenmax=Postmax;
                Betweenindex=Postindex;
            end

            if(Postindex+1<=number-1)
                [Post2max,Post2index]=max(Heights(Postindex+1:number-1));
                Post2index=Post2index+Postindex+1-1;
            else
                Post2max=Postmax;
                Post2index=Postindex;
            end

            rectangle('Position',[PrevMID+(PrevLEN/2),maxim,0.0001,PrevMAX-maxim]);

            LastM=size(middle,2);
            LastMID=PrevMID+(PrevLEN/2);
            PrevMID=LastMID;
            middle=[];
            middle(1)=LastMID;
            LastM=size(middle,2);

            high=[];
            indices=[];
            high=cat(2,high,[Post2max,Postmax,Betweenmax,maxim,Prevmax]);

            indices=cat(2,indices,[Post2index,Postindex,Betweenindex,index,Previndex]);
            LastH=size(high,2);

            L=[];
            L=cat(2,L,maxim);
            Last=size(L,2);
            lengt=[];
            if(Postindex>Previndex)
                if(M<extra)
                    M=M+1;
                end
                lengt(1)=Postindex-Previndex+(extra-M);

                rectangle('Position',[LastMID-lengt(1)/2,maxim,lengt(1),0.0001]);
                direct1=1;
            elseif(Postindex<=Previndex & Betweenmax==Postmax)
                lengt(1)=1;
                rectangle('Position',[LastMID-1/2,maxim,1,0.0001]);
                %%%%%%%

                rectangle('Position',[LastMID-1/2,maxim-1+extrawaystick,0.0001,1-extrawaystick]);
                if(objectorder(k+1)>10)

                    text(LastMID-1/2-0.25,maxim-2.0+extrawaytext,num2str(double(objectorder(k+1))));
                    Mini=min(Mini,LastMID-1/2-0.25);
                else

                    text(LastMID-1/2-0.1,maxim-2.0+extrawaytext,num2str(double(objectorder(k+1))));
                    Mini=min(Mini,LastMID-1/2-0.1);
                end

                rectangle('Position',[LastMID+1/2,maxim-1+extrawaystick,0.0001,1-extrawaystick]);

                text(LastMID+1/2-0.1,maxim-2.0+extrawaytext,num2str(double(objectorder(k+2))));
                Maxi=max(Maxi,LastMID+1/2-0.1);

                k=k+2;
            end
            PrevLEN=lengt(1);
            LastL=size(lengt,2);

            if(Element==1)
                Element=0;
            end

            if(direct11==1)
                if(NbanFirst+1<index)
                    middle=cat(2,middle,middle(LastM)-lengt(LastL)/2);
                    LastM=size(middle,2);
                    direct111=1;
                    over=1;
                end
                direct11=0;
            end

            Special=1;
            PrevMAX=maxim;
        end
        if(S==2 && k+1==number)

            rectangle('Position',[LastMID+lengt(1)/2,PrevMAX-1+extrawaystick,0.0001,1-extrawaystick]);

            text(LastMID+lengt(1)/2-0.1,PrevMAX-2.0+extrawaytext,num2str(double(objectorder(k+1))));
            Maxi=max(Maxi,LastMID+lengt(1)/2-0.1);
            Mini=min(Mini,LastMID+lengt(1)/2-0.1);
            k=k+1;
        end
        over=1;
    end
    branch=0;

    k=k+1;
    NbanFirst=NbanFirst+1;

end
axis([Mini-1,Maxi+1,min(Heights)-5,max(Heights)+1]);
XT=[];
set(gca,'XTick',XT);
set(gca,'XTickLabel',[]);

%---
function res=H(vector)

lengt=size(vector,2);
Nvector=vector;
i=1;
for i=1:lengt
    for j=i+1:lengt
        if (vector(i)==vector(j))
            Nvector(j)=vector(j)+i*0.0001;
            i=i+1;
        end
    end
end
res=Nvector;


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function phiJump = cf_lognormjump(u,a,b,lambda,T)
% log characteristic function for lognormal jumps
    phiJump = lambda*T*(-a*u*1i + (exp(u*1i*log(1.0+a) ...
        +0.5*b*b*u*1i.*(u*1i-1.0))-1.0));
end


function [Im_DN,D_] = Denoise(Im_N,Sigma,K,n,Algo)
% INPUT ARGUMENTS : Im_N - the noisy image (gray-level scale)
%                   Sigma - the s.d. of the noise (assume to be white Gaussian).
%                   K - the number of atoms in the representing dictionary.
%                   n - Block Size n x n
%% Data Stuff
Reduce_DC = 1;
[N1,N2] = size(Im_N);
C = 1.15;                       % Taken from the Paper Elad 06
E_T = C*Sigma;                  % Required Average target Error in OMP
if Sigma > 5
    noIt = 10;
else
    noIt = 5;
end
MaxTPatches = 62001;

%% DCT Dictionary Creation
% D_DCT = Dict_DCT(n,K);
if Reduce_DC
    D_DCT = odctdict(n^2,K+1);
    D_DCT = D_DCT(:,2:end);
else
    D_DCT = odctdict(n^2,K);
end
%% Block Patches to Columns
fprintf('Learning Dictionary using %s\n',Algo);
if(prod([N1,N2]-n+1)> MaxTPatches)
    randPermutation =  randperm(prod([N1,N2]-n+1));
    selectedBlocks = randPermutation(1:MaxTPatches);

    Y = zeros(n^2,MaxTPatches);
    for i = 1:MaxTPatches
        [row,col] = ind2sub(size(Im_N)-n+1,selectedBlocks(i));
        currBlock = Im_N(row:row+n-1,col:col+n-1);
        Y(:,i) = currBlock(:);
    end
else
    Y = im2col(Im_N,[n,n],'sliding');       % Signal Y with patches as Columns
end

%% Reducing DC component
if (Reduce_DC)
    vecOfMeans = mean(Y);
    Y = Y-ones(size(Y,1),1)*vecOfMeans;
end

%% Data Whitening
% Y = Data_Whiten(Y,1);

%% Going into Dictionary Learning Algo for Training
D_ = normc(D_DCT);
for it = 1:noIt
    W = omp2(D_,Y,D_'*D_,(n*E_T));    %
    switch lower(Algo)
        case 'ksvd'
            [D_,W] = Optimize_K_SVD(Y,D_,W);
        case 's1'
            alpha = .37;
            [D_,W] = Optimize_S1(Y,D_,W,alpha);
        case 's1svd'
            alpha = 100;    gamma = 1;
            [D_,W] = Optimize_S1SVD(Y,D_,W,alpha,gamma);
        otherwise
            error('Invalid Learning Method Specified');
            break;
    end
    D_ = I_clearDictionary(D_,W,Y);
    disp(['Iteration # ',num2str(it),' With average number of Coefficients = ',num2str(nnz(W)/size(W,2))])
end

%% DEnoising with the Trained Dictionary
disp('Denoising with the Trained Dictionary')
Y = im2col(Im_N,[n,n],'sliding');
if (Reduce_DC)
    vecOfMeans = mean(Y);
    Y = Y-ones(size(Y,1),1)*vecOfMeans;
end
Coefs = omp2(D_,Y,D_'*D_,(n*E_T));
if (Reduce_DC)
    Y = D_*Coefs + ones(size(Y,1),1) * vecOfMeans;
else
    Y = D_*Coefs;
end

%% Generation and Averaging of The signal from Y (columns)
count = 1;
Weight= zeros(N1,N2);
IMout = zeros(N1,N2);
idx = 1:size(Y,2);
[rows,cols] = ind2sub(size(Im_N)-n+1,idx);
for i  = 1:length(cols)
    col = cols(i); row = rows(i);
    Y_ = reshape(Y(:,count),[n,n]);
    IMout(row:row+n-1,col:col+n-1) = IMout(row:row+n-1,col:col+n-1)+Y_;
    Weight(row:row+n-1,col:col+n-1) = Weight(row:row+n-1,col:col+n-1)+ones(n);
    count = count+1;
end;
Im_DN = (Im_N+0.034*Sigma*IMout)./(1+0.034*Sigma*Weight);

end

%% KSVD implementation for Elad 2006
function [D_,W] = Optimize_K_SVD(Y1,D_,W)
    R = Y1 - D_*W;
    for k=1:size(D_,2)
        I = find(W(k,:));
        Ri = R(:,I) + D_(:,k)*W(k,I);
        [U,S,V] = svds(Ri,1,'L');
        D_(:,k) = U;
        W(k,I) = S*V';
        R(:,I) = Ri - D_(:,k)*W(k,I);
    end
end

%% S1 implementation
function [D,W] = Optimize_S1(Y,D,W,alpha)
    Ek = Y - D * W;
    for k = 1:size(D,2)
        Eki = Ek + D(:,k)*W(k,:);
        for j = 1:2
            G = D(:,k)'*Eki;   g = std(G);  G = G./g; % std = 1
            %alpha2 = std(G)*alpha;
            W(k,:) = g.*sign(G).*max(0,abs(G)-alpha);
            D(:,k) = (Eki * W(k,:)')/norm(Eki * W(k,:)');
        end
%         nnz(W(k,:))
        Ek = Eki - D(:,k)*W(k,:);
    end
end

%% S1 WIth SVD Dictionary Update Stage
function [D,W] = Optimize_S1SVD(Y,D,W,alpha,gamma)
    Ek = Y - D * W;
    for k = 1:size(D,2)
        Eki = Ek + D(:,k)*W(k,:);
        % SVD
%         [D(:,k),s,v] = svds(Eki,1,'L');
%         W(k,:) = s*v';
        % Power Iteration
        for i = 1:5
            W(k,:) = D(:,k)'*Eki;
            D(:,k) = Eki*W(k,:)';    D(:,k) = D(:,k)/norm(D(:,k));
        end
        for j = 1:2
            G = D(:,k)'*Eki;  g=1; %g = std(G);  G = G./g;
            alpha2 = std(G)*alpha;
            W(k,:) = g.* sign(G).*max(0,(abs(G) - g.* alpha2./(abs(W(k,:)))));  %.^gamma
            D(:,k) = Eki*W(k,:)'/norm(Eki*W(k,:)');
        end
%             nnz(W(k,:))
        Ek = Eki - D(:,k)*W(k,:);
    end
end


% @Author: aaronmishkin
% @Date:   2018-06-07T19:03:42-07:00
% @Email:  amishkin@cs.ubc.ca
% @Last modified by:   aaronmishkin
% @Last modified time: 2018-07-26T13:09:11-07:00

function [y, X, y_te, X_te] = get_data_log_reg(name, seed)
%name: name of a dataset
%seed: seed used for creating a train/test set
%y (y_te): labels for a train (test) set
%X (X_te): features for a train (test) set
%Note: for classification problems, we use 0-1 encoding in labels.
switch name
case 'murphy_synth'
    setSeed(seed)
    N=30;
    D=2;
    mu1=[ones(N,1) 5*ones(N,1)];
    mu2=[-5*ones(N,1) 1*ones(N,1)];
    class1_std = 1;
    class2_std = 1.1;
    X = [class1_std*randn(N,2)+mu1;2*class2_std*randn(N,2)+mu2];
    y = [ones(N,1);zeros(N,1)];
    y = 2*y-1;
    % Set the test set to the be same as the training set.
    X_te = X; y_te = y;
case 'synth'
  setSeed(seed);
  N = 5000;
  D = 2;
  s2 = .01;
  X = randn(N,D);
  D = D + 1;
  w = [0.1; -1; +1];
  eta = [ones(N,1) X]*w + s2*randn(N,1);
  y = sign(eta);
  [X, y, X_te, y_te] = split_data(y, X, 0.5);

case {'a2a','a3a','a4a','a5a','a6a','a7a'}
    setSeed(seed);
  load(name);
  [N,D] = size(X);
  X = [ones(N,1) X];
  [N_te,D] = size(X_te);
  X_te = [ones(N_te,1) X_te];
  y = (y+1)/2;
  y_te = (y_te+1)/2;
  assert ( length(unique(abs(y))) == 2)
  assert ( length(unique(abs(2*y-1))) == 1)

case {'svmguide3'}
    setSeed(seed);
  load(name);
  [N,D] = size(X);
  X = [ones(N,1) X];
  [N_te,D] = size(X_te);
  X_te = [ones(N_te,1) X_te];
  y = (y+1)/2;
  y_te = (y_te+1)/2;
  assert ( length(unique(abs(y))) == 2)
  assert ( length(unique(abs(2*y-1))) == 1)

case 'svmguide1'
    setSeed(seed);
  load(name);
  [N,D] = size(X);
  X = [ones(N,1) X];
  [N_te,D] = size(X_te);
  X_te = [ones(N_te,1) X_te];
  assert ( length(unique(abs(y))) == 2)
  assert ( length(unique(abs(2*y-1))) == 1)

case 'a1a'
    setSeed(seed);
  load(name);
  [N,D] = size(X);
  X = [ones(N,1) X zeros(N,4)];
  [N_te,D] = size(X_te);
  X_te = [ones(N_te,1) X_te];
  y = (y+1)/2;
  y_te = (y_te+1)/2;
  assert ( length(unique(abs(y))) == 2)
  assert ( length(unique(abs(2*y-1))) == 1)

case {'colon-cancer'}
  load(name);
  [N,D] = size(X);
  X = [ones(N,1) X];
  y = (y+1)/2;
  setSeed(seed);
  [X, y, X_te, y_te] = split_data(y, X, 0.5);

case {'duke'}
  load(name);
  [N,D] = size(X);
  X = [ones(N,1) full(X)];
  y = (y+1)/2;
  setSeed(seed);
  [X, y, X_te, y_te] = split_data(y, X, 0.5);

case {'leukemia'}
    setSeed(seed);
  load(name);
  [N,D] = size(X);
  X = [ones(N,1) X];
  [N_te,D] = size(X_te);
  X_te = [ones(N_te,1) X_te];
  y = (y+1)/2;
  y_te = (y_te+1)/2;
  assert ( length(unique(abs(y))) == 2)
  assert ( length(unique(abs(2*y-1))) == 1)

case {'gisette_scale'}
  load(name);
  X = [X; X_te];
  y = [y; y_te];
  [N,D] = size(X);
  X = [ones(N,1) full(X)];
  y = (y+1)/2;
  unique(y)
  setSeed(seed);
  [X, y, X_te, y_te] = split_data(y, X, 0.5);

case {'covtype_binary_scale'}
  load(name);
  [N,D] = size(X);
  X = [ones(N,1) full(X)];
  y = y-1;
  setSeed(seed);
  [X, y, X_te, y_te] = split_data(y, X, 0.5);
  assert ( length(unique(abs(y))) == 2)
  assert ( length(unique(abs(2*y-1))) == 1)

case {'SUSY'}
  load('SUSY.amat','-mat');
  [N,D] = size(X);
  X = [ones(N,1) full(X)];
  setSeed(seed);
  [X, y, X_te, y_te] = split_data(y, X, 0.5);
  assert ( length(unique(abs(y))) == 2)
  assert ( length(unique(abs(2*y-1))) == 1)

case {'australian_scale', 'diabetes_scale'}
  load(name);
  [N,D] = size(X);
  X = [ones(N,1) full(X)];
  y = (y+1)/2;
  setSeed(seed);
  [X, y, X_te, y_te] = split_data(y, X, 0.5);
  assert ( length(unique(abs(y))) == 2)
  assert ( length(unique(abs(2*y-1))) == 1)

case {'breast_cancer_scale'}
    setSeed(seed);
  load(name);
  [N,D] = size(X);
  X = [ones(N,1) full(X)];
  y = (y-2)/2;
  [X, y, X_te, y_te] = split_data(y, X, 0.5);
  assert ( length(unique(abs(y))) == 2)
  assert ( length(unique(abs(2*y-1))) == 1)

case 'usps_3vs5'
  setSeed(seed);
  load('usps_resampled');
  y = ([train_labels test_labels] + 1)/2; % 1540 obs
  X = ([train_patterns test_patterns]);
  y = sum(bsxfun(@times, y, [0:9]'));
  idx = find(or((y==3), (y==5)));
  y = y(idx);
  y = (y==5);
  X = X(:,idx);
  X = X'; % 1540x256
  [N,D] = size(X);
  y = y(:); % in 0/1 encoding
  X = [ones(N,1) full(X)];
  [X, y, X_te, y_te] = split_data(y, X, 0.5);
%
%   y = 2*y - 1;
%   y_te = 2*y_te - 1;
%

otherwise
  error('no such name');
end
end

function [XTr, yTr, XTe, yTe] = split_data(y, X, prop)

  N = size(y,1);
	idx = randperm(N);
  Ntr = floor(prop * N);
	idxTr = idx(1:Ntr);
	idxTe = idx(Ntr+1:end);
  XTr = X(idxTr,:);
  yTr = y(idxTr);
  XTe = X(idxTe,:);
  yTe = y(idxTe);
end


function [ zsl_accuracy, Y_hit5 ] = zsl_el( S_est, S_te_gt, param)
% ZSL_EL calculates zero-shot classification accuracy
%
% INPUT:
%    S_est: estimated semantic labels
%    S_te_gt: ground truth semantic labels
%    param: other parameters
%
% Output:
%    zsl_accuracy: zero-shot classification accuracy (per-sample)

dist     =  1 - (pdist2(S_est, NormalizeFea(S_te_gt')', 'cosine'));
Y_hit5   = zeros(size(dist,1),param.HITK);
for i    = 1:size(dist,1)
    [~, I] = sort(dist(i,:),'descend');
    Y_hit5(i,:) = param.testclasses_id(I(1:param.HITK));
end

n = 0;
for i  = 1:size(dist,1)
    if ismember(param.test_labels(i),Y_hit5(i,:))
        n = n + 1;
    end
end
zsl_accuracy = n/size(dist,1);

end


domain = [-1,1,-1,1,-1,1];
bc = [1,1,1,1,1,1]; % Dirichelet BC

load('BoxTorusMesh.mat')
% epsm = 8.85*10^(-5)*5;
% epsp = 8.85*10^(-5);
% sigm = 100;
% sigp = 1;
% mum = (4*pi);
% mup = (4*pi)*2;
% x0=0; y0=0; z0=-0.3; r1=0.2; r2=pi/5; omega = 20; stre = 1;
% pde = TorusTime1(mum,mup,sigm,sigp,epsm,epsp,omega,stre,x0,y0,z0,r1,r2);
% TEND = 0.1;
epsm = 8.854*10^(-2);
epsp = 8.854*10^(-3);
sigm = 10;%100;
sigp = 1;%1;
mum = 4*pi*5;
mup = 4*pi;
x0=0; y0=0; z0=-0.3; r1=0.2; r2=pi/5; omega = 1; a = omega*sqrt(epsp*mup); b= 120; intPt = -1;
pde = TorusTimeInitial3(mum,mup,sigm,sigp,epsm,epsp,omega,x0,y0,z0,r1,r2,a,b,intPt);
TEND = 0.8;
InitCond = 1;
BDCond = 1;

nx = 64;  h=(domain(2) - domain(1))/nx;
ny = nx;
nz = nx;

Ntime = ceil(TEND/sqrt(8.854*10^(-3)*4*pi*2))*5*nx;%ceil(sqrt(nx));
deltaT = TEND/Ntime;
mesh = enrichMesh3D(mesh,0);
fem = genNedFEM3D(mesh,bc);
bm = epsm/deltaT^2 + sigm/(2*deltaT);
bp = epsp/deltaT^2 + sigp/(2*deltaT);
am = mum^(-1); ap = mup^(-1);

S = globMatrixNedFit3D(am,ap,1,mesh,fem,fem);
Me = globMatrixNedFit3D(epsm,epsp,0,mesh,fem,fem);
Ms = globMatrixNedFit3D(sigm,sigp,0,mesh,fem,fem);
Atotal = Me/deltaT^2 + Ms/(2*deltaT) + S/2;

Ndof = size(mesh.e,1);
bdidx = zeros(Ndof,1);
isBdEdge = true(Ndof,1);
isBdEdge(fem.mapper) = false;
bdidx(isBdEdge) = 1;
Tbd = spdiags(bdidx,0,Ndof,Ndof);
T = spdiags(1-bdidx,0,Ndof,Ndof);
A = T*Atotal*T + Tbd;

if InitCond == 0
    uppre = zeros(size(mesh.e,1),1);
    upre = zeros(size(mesh.e,1),1);
elseif InitCond ~=0
    tu1 = sum(feval(pde.E1,fem.gex,fem.gey,fem.gez,0).*fem.gew,2);
    tu2 = sum(feval(pde.E2,fem.gex,fem.gey,fem.gez,0).*fem.gew,2);
    tu3 = sum(feval(pde.E3,fem.gex,fem.gey,fem.gez,0).*fem.gew,2);
    tgt = mesh.p(mesh.e(:,2),:) - mesh.p(mesh.e(:,1),:);
    tgt = tgt./sum(tgt.^2,2).^(1/2);
    uppre = tu1.*tgt(:,1) + tu2.*tgt(:,2) + tu3.*tgt(:,3);
    tu1 = sum(feval(pde.Et1,fem.gex,fem.gey,fem.gez,0).*fem.gew,2);
    tu2 = sum(feval(pde.Et2,fem.gex,fem.gey,fem.gez,0).*fem.gew,2);
    tu3 = sum(feval(pde.Et3,fem.gex,fem.gey,fem.gez,0).*fem.gew,2);
    upretmp = tu1.*tgt(:,1) + tu2.*tgt(:,2) + tu3.*tgt(:,3);
    upre = uppre + deltaT*upretmp;
end

UHfit =zeros(size(mesh.e,1),ceil(Ntime/10)+2);
NumStep = 0;
CurrentT = 0;
SaveCount = 0;

tID1 = (mesh.tLoc == 1);
tID2 = (mesh.tLoc == 2);
e1tmp = unique(reshape(mesh.t_e(tID1,:),[],1));
e2tmp = unique(reshape(mesh.t_e(tID2,:),[],1));
eInt = intersect(e1tmp,e2tmp);
eid1 = setdiff(e1tmp,eInt);
eid2 = setdiff(e2tmp,eInt);
alpha = am*ones(size(mesh.e,1),1);
alpha(eid2) = ap;
alpha(eInt) = (am+ap)/2;
beta = bm*ones(size(mesh.e,1),1);
beta(eid2) = bp;
beta(eInt) = (bm+bp)/2;
NEdof = size(A,1);

while CurrentT < TEND

    NumStep = NumStep + 1;
    CurrentT = CurrentT + deltaT;
    fm1Current = @(x,y,z) pde.fm1(x,y,z,CurrentT);
    fm2Current = @(x,y,z) pde.fm2(x,y,z,CurrentT);
    fm3Current = @(x,y,z) pde.fm3(x,y,z,CurrentT);
    fp1Current = @(x,y,z) pde.fp1(x,y,z,CurrentT);
    fp2Current = @(x,y,z) pde.fp2(x,y,z,CurrentT);
    fp3Current = @(x,y,z) pde.fp3(x,y,z,CurrentT);
    rhsF1 = globNedFitRHS3D(fm1Current,fp1Current, mesh, fem, 0, 1);
    rhsF2 = globNedFitRHS3D(fm2Current,fp2Current, mesh, fem, 0, 2);
    rhsF3 = globNedFitRHS3D(fm3Current,fp3Current, mesh, fem, 0, 3);
    rhsFCurrent = rhsF1 + rhsF2 + rhsF3;

    if BDCond == 0
        tuCurrent = zeros(size(mesh.e,1),1);
    elseif BDCond ~= 0
        exactu1Current = @(x,y,z) pde.E1(x,y,z,CurrentT);
        exactu2Current = @(x,y,z) pde.E2(x,y,z,CurrentT);
        exactu3Current = @(x,y,z) pde.E3(x,y,z,CurrentT);
        tu1 = sum(feval(exactu1Current,fem.gex,fem.gey,fem.gez).*fem.gew,2);
        tu2 = sum(feval(exactu2Current,fem.gex,fem.gey,fem.gez).*fem.gew,2);
        tu3 = sum(feval(exactu3Current,fem.gex,fem.gey,fem.gez).*fem.gew,2);
        tgt = mesh.p(mesh.e(:,2),:) - mesh.p(mesh.e(:,1),:);
        tgt = tgt./sum(tgt.^2,2).^(1/2);
        tuCurrent = tu1.*tgt(:,1) + tu2.*tgt(:,2) + tu3.*tgt(:,3);
    end

    ub = tuCurrent;
    ub(fem.mapper) = 0;
    rhsB = Atotal*ub;
    JCurrent = rhsFCurrent - rhsB;
    %JCurrent(isBdEdge) = tuCurrent(isBdEdge);

    fcurrent = Me*(2*upre - uppre)/deltaT^2 + Ms*uppre/(2*deltaT) - S*uppre/2 + JCurrent;
    fcurrent(isBdEdge) = tuCurrent(isBdEdge);

    option.outsolver = 'cg';
    option.alpha = alpha;
    option.beta = beta;
    option.solver = 'amg';
    edge = mesh.e;
    option.smoother = 'BD';
    option.blklevel = 0;
    option.blkId = NEdof;
    [x,info] = amgMaxwellinterface2(A,fcurrent,mesh.p,edge,option);
    uppre = upre;
    upre = x;

    if mod(NumStep,40) == 0
        UHfit(:,SaveCount+1) = uppre;
        UHfit(:,SaveCount+2) = upre;
        SaveCount = SaveCount+2;
        save('UHfit1','UHfit','-v7.3');
        disp('save data')
    end

end

save('UHfit1','UHfit');


function out = IN_AutoMutualInfo(y,timeDelay,estMethod,extraParam)
% IN_AutoMutualInfo     Time-series automutual information
%
%---INPUTS:
%
% y: input time series (column vector)
%
% timeDelay: time lag for automutual information calculation
%
% estMethod: the estimation method used to compute the mutual information:
%           (*) 'gaussian'
%           (*) 'kernel'
%           (*) 'kraskov1'
%           (*) 'kraskov2'
%
% cf. Kraskov, A., Stoegbauer, H., Grassberger, P., Estimating mutual
% information: http://dx.doi.org/10.1103/PhysRevE.69.066138

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
% Check inputs:
% ------------------------------------------------------------------------------
if nargin < 2 || isempty(timeDelay)
    timeDelay = 1;
end
if ischar(timeDelay) && ismember(timeDelay,{'ac','tau'})
    timeDelay = CO_FirstCrossing(y,'ac',0,'discrete');
end

if nargin < 3 || isempty(estMethod)
    estMethod = 'kernel';
end

if nargin < 4
    extraParam = [];
end

N = length(y);
doPlot = false; % plot outputs to screen
minSamples = 5; % minimum 5 samples to compute a mutual information (could make higher?)

% Ensure y is a column vector
if size(y,2) > size(y,1)
    warning('Please input a column vector for y')
    y = y';
end

% ------------------------------------------------------------------------------
% Loop over time delays if a vector
numTimeDelays = length(timeDelay);
amis = nan(numTimeDelays,1);

if numTimeDelays > 1
    timeDelay = sort(timeDelay);
end

% Initialize miCalc object (needs to be reinitialized within the loop for kraskov):
if ~strcmp(estMethod,'gaussian')
    miCalc = IN_Initialize_MI(estMethod,extraParam,false); % NO ADDED NOISE!
end

for k = 1:numTimeDelays

    % Check enough samples to compute an automutual information
    if timeDelay(k) > N - minSamples
        % Time series is too short -- keep the remaining values as NaNs
        break
    end

    % Form the time-delay vectors y1 and y2
    y1 = y(1:end-timeDelay(k));
    y2 = y(1+timeDelay(k):end);

    if strcmp(estMethod,'gaussian')
        r = corr(y1,y2,'type','Pearson');
        amis(k) = -0.5*log(1 - r^2);
    else
        % Reinitialize for Kraskov:
        miCalc.initialise(1,1);

        % Set observations to time-delayed versions of the time series:
        miCalc.setObservations(y1,y2);

        % Compute:
        amis(k) = miCalc.computeAverageLocalOfObservations();
    end

    % Plot:
    if doPlot
        plot(y1,y2,'.k')
        title(sprintf('ami = %.3f',amis(k)))
        pause(0.1)
    end
end

if any(isnan(amis))
    warning(['Time series (N=%u) is too short for automutual information calculations',...
                ' up to lags of %u'],N,max(timeDelay))
end

if doPlot
    plot(amis,'-k')
end

%-------------------------------------------------------------------------------
% Outputs:
%-------------------------------------------------------------------------------
if numTimeDelays == 1
    out = amis; % a scalar
else
    % A structure
    for k = 1:numTimeDelays
        out.(sprintf('ami%u',timeDelay(k))) = amis(k);
    end
end

end


  function  Quaternion = madgwickAHRS( Gyroscope, Accelerometer, Magnetometer, quaternion_l,Beta,SamplePeriod)
            q = quaternion_l; % short name local variable for readability
            % Normalise accelerometer measurement
            if(norm(Accelerometer) == 0), return; end	% handle NaN
            Accelerometer = Accelerometer / norm(Accelerometer);	% normalise magnitude

            % Normalise magnetometer measurement
            if(norm(Magnetometer) == 0), return; end	% handle NaN
            Magnetometer = Magnetometer / norm(Magnetometer);	% normalise magnitude

            % Reference direction of Earth's magnetic feild
            h = quaternProd(q, quaternProd([0 Magnetometer], quaternConj(q)));
            b = [0 norm([h(2) h(3)]) 0 h(4)];

            % Gradient decent algorithm corrective step
            F = [-2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
                -2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
                -2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)
                ((2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3))) - Magnetometer(1))
                ((2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4))) - Magnetometer(2))
                ((2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2)) - Magnetometer(3))];
            J = [2*q(3),                 	-2*q(4),                    2*q(1),                         -2*q(2)
                -2*q(2),                 	-2*q(1),                    	-2*q(4),                         -2*q(3)
                0,                         4*q(2),                    4*q(3),                         0
               -2*b(4)*q(3),               2*b(4)*q(4),               -4*b(2)*q(3)-2*b(4)*q(1),       -4*b(2)*q(4)+2*b(4)*q(2)
                -2*b(2)*q(4)+2*b(4)*q(2),	2*b(2)*q(3)+2*b(4)*q(1),	2*b(2)*q(2)+2*b(4)*q(4),       -2*b(2)*q(1)+2*b(4)*q(3)
                2*b(2)*q(3),                2*b(2)*q(4)-4*b(4)*q(2),	2*b(2)*q(1)-4*b(4)*q(3),        2*b(2)*q(2)];
            step = (J'*F);
            step = step / norm(step);	% normalise step magnitude

            % Compute rate of change of quaternion
            qDot = 0.5 * quaternProd(q, [0 Gyroscope]) - Beta * step';

            % Integrate to yield quaternion
            q = q + qDot * SamplePeriod;
            Quaternion = q / norm(q); % normalise quaternion
        end


function H = hypot(f, g, pref)
%HYPOT   Robust computation of the square root of the sum of squares.
%   H = HYPOT(F, G) returns SQRT(ABS(F).^2 + ABS(G).^2) for two CHEBFUN objects
%   F and G (or a CHEBFUN and a double) carefully computed to avoid underflow
%   and overflow.
%
% Example:
%       f = chebfun(@(x) 3*[1e300*x 1e-300*x]);
%       g = chebfun(@(x) 4*[1e300*x 1e-300*x]);
%       % h1 = sqrt(f.^2 + g.^2) % This will fail because of overflow
%       h2 = hypot(f, g)
%
% See also ABS, NORM, SQRT.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Grab some preferences:
if ( nargin < 3 )
    pref = chebfunpref();
end

% Insert breaks at the roots:
if ( isa(f, 'chebfun') )
    f = addBreaksAtRoots(f, pref);
end
if ( isa(g, 'chebfun') )
    g = addBreaksAtRoots(g, pref);
end

% Call compose:
H = compose(f, @hypot, g, pref);

end


% RANGE = showSpyr (PYR, INDICES, RANGE, GAP, LEVEL_SCALE_FACTOR)
%
% Display a steerable pyramid, specified by PYR and INDICES
% (see buildSpyr), in the current figure.  The highpass band is not shown.
%
% RANGE is a 2-vector specifying the values that map to black and
% white, respectively.  These values are scaled by
% LEVEL_SCALE_FACTOR^(lev-1) for bands at each level.  Passing a value
% of 'auto1' sets RANGE to the min and max values of MATRIX.  'auto2'
% sets RANGE to 3 standard deviations below and above 0.0.  In both of
% these cases, the lowpass band is independently scaled.  A value of
% 'indep1' sets the range of each subband independently, as in a call
% to showIm(subband,'auto1').  Similarly, 'indep2' causes each subband
% to be scaled independently as if by showIm(subband,'indep2').
% The default value for RANGE is 'auto2'.
%
% GAP (optional, default=1) specifies the gap in pixels to leave
% between subbands.
%
% LEVEL_SCALE_FACTOR indicates the relative scaling between pyramid
% levels.  This should be set to the sum of the kernel taps of the
% lowpass filter used to construct the pyramid (default is 2, which is
% correct for L2-normalized filters.

% Eero Simoncelli, 2/97.

function [range] = showSpyr(pyr, pind, range, gap, scale);

nbands = spyrNumBands(pind);

%------------------------------------------------------------
%% OPTIONAL ARGS:

if (exist('range') ~= 1)
  range = 'auto2';
end

if (exist('gap') ~= 1)
  gap = 1;
end

if (exist('scale') ~= 1)
  scale = 2;
end

%------------------------------------------------------------

ht = spyrHt(pind);
nind = size(pind,1);

%% Auto range calculations:
if strcmp(range,'auto1')
  range = ones(nind,1);
  band = spyrHigh(pyr,pind);
  [mn,mx] = range2(band);
  for lnum = 1:ht
    for bnum = 1:nbands
      band = spyrBand(pyr,pind,lnum,bnum)/(scale^(lnum-1));
      range((lnum-1)*nbands+bnum+1) = scale^(lnum-1);
      [bmn,bmx] = range2(band);
      mn = min(mn, bmn);
      mx = max(mx, bmx);
    end
  end
  range = range * [mn mx]; 		% outer product
  band = pyrLow(pyr,pind);
  [mn,mx] = range2(band);
  range(nind,:) = [mn, mx];

elseif strcmp(range,'indep1')
  range = zeros(nind,2);
  for bnum = 1:nind
    band = pyrBand(pyr,pind,bnum);
    [mn,mx] = range2(band);
    range(bnum,:) =  [mn mx];
  end

elseif strcmp(range,'auto2')
  range = ones(nind,1);
  band = spyrHigh(pyr,pind);
  sqsum = sum(sum(band.^2));  numpixels = prod(size(band));
  for lnum = 1:ht
    for bnum = 1:nbands
      band = spyrBand(pyr,pind,lnum,bnum)/(scale^(lnum-1));
      sqsum = sqsum + sum(sum(band.^2));
      numpixels = numpixels + prod(size(band));
      range((lnum-1)*nbands+bnum+1) = scale^(lnum-1);
    end
  end
  stdev = sqrt(sqsum/(numpixels-1));
  range = range * [ -3*stdev 3*stdev ]; % outer product
  band = pyrLow(pyr,pind);
  av = mean2(band);   stdev = sqrt(var2(band));
  range(nind,:) = [av-2*stdev,av+2*stdev];

elseif strcmp(range,'indep2')
  range = zeros(nind,2);
  for bnum = 1:(nind-1)
    band = pyrBand(pyr,pind,bnum);
    stdev = sqrt(var2(band));
    range(bnum,:) =  [ -3*stdev 3*stdev ];
  end
  band = pyrLow(pyr,pind);
  av = mean2(band);   stdev = sqrt(var2(band));
  range(nind,:) = [av-2*stdev,av+2*stdev];

elseif isstr(range)
  error(sprintf('Bad RANGE argument: %s',range))

elseif ((size(range,1) == 1) & (size(range,2) == 2))
  scales = scale.^[0:(ht-1)];
  scales = ones(nbands,1) * scales;   %outer product
  scales = [1; scales(:); scale^ht];  %tack on highpass and lowpass
  range = scales * range;		% outer product
  band = pyrLow(pyr,pind);
  range(nind,:) = range(nind,:) + mean2(band) - mean(range(nind,:));

end

% CLEAR FIGURE:
clf;

colormap(gray);
cmap = get(gcf,'Colormap');
nshades = size(cmap,1);

%  Find background color index:
clr = get(gcf,'Color');
bg = 1;
dist = norm(cmap(bg,:)-clr);
for n = 1:nshades
  ndist = norm(cmap(n,:)-clr);
  if (ndist < dist)
    dist = ndist;
    bg = n;
  end
end

%% Compute positions of subbands:
llpos = ones(nind,2);

if (nbands == 2)
  ncols = 1;  nrows = 2;
else
  ncols = ceil((nbands+1)/2);   nrows = ceil(nbands/2);
end
relpos = [ (1-nrows):0, zeros(1,(ncols-1)); ...
           zeros(1,nrows), -1:-1:(1-ncols) ]';
if (nbands > 1)
  mvpos = [-1 -1];
else
  mvpos = [0 -1];
end
basepos = [0 0];

for lnum = 1:ht
  ind1 = (lnum-1)*nbands + 2;
  sz = pind(ind1,:)+gap;
  basepos = basepos + mvpos .* sz;
  if (nbands < 5)			% to align edges...
    sz = sz + gap*(ht-lnum+1);
  end
  llpos(ind1:ind1+nbands-1,:) = relpos * diag(sz) + ones(nbands,1)*basepos;
end

% lowpass band
sz = pind(nind-1,:)+gap;
basepos = basepos + mvpos .* sz;
llpos(nind,:) = basepos;

%% Make position list positive, and allocate appropriate image:
llpos = llpos - ones(nind,1)*min(llpos) + 1;
llpos(1,:) = [1 1];
urpos = llpos + pind - 1;
d_im = bg + zeros(max(urpos));

%% Paste bands into image, (im-r1)*(nshades-1)/(r2-r1) + 1.5
for bnum=2:nind
  mult = (nshades-1) / (range(bnum,2)-range(bnum,1));
  d_im(llpos(bnum,1):urpos(bnum,1), llpos(bnum,2):urpos(bnum,2)) = ...
      mult*pyrBand(pyr,pind,bnum) + (1.5-mult*range(bnum,1));
end

hh = image(d_im);
axis('off');
pixelAxes(size(d_im),'full');
set(hh,'UserData',range);


function gf=gradloglikGaPExp(x, varargin)
% f=loglikGaP(x, varargin)
% complete log likellihood of the GaP model
% Vxt = varargin{1};      %data
% sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
% alpha = varargin{3}; %parameters of the Gamma prior on the blinking
% beta = varargin{4}; %parameters of the Gamma prior on the blinking
% peval = varargin{5}; %parameters
% x(1:end-2*peval.ncomp) is Hkt

Vxt = varargin{1};      %data
sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
alpha = varargin{3}; %parameters of the Gamma prior on the blinking
beta = varargin{4}; %parameters of the Gamma prior on the blinking
peval = varargin{5}; %parameters

[Hkt_r, cx, cy, Wxk] = loglikGaPreadparam(x,varargin);

Hkt=exp(Hkt_r); %nonnegativity constrains
[Wxkbg,Hktbg]=addbg(Wxk, Hkt, peval.bg);

P=Wxkbg*Hktbg; %current approximation

%linear grasdient shifted by cx
xxvc = lineargrad([peval.nx, peval.ny, peval.ncomp], cx, 'xx');
yyvc = lineargrad([peval.nx, peval.ny, peval.ncomp], cy, 'yy');

% dW/dcx:
Wxtcx=1/sigpsf^2*xxvc.*Wxk;
% dW/dcy:
Wxtcy=1/sigpsf^2*yyvc.*Wxk;

% d(log(L))/dHkt:
% gfHkt=(alpha-1)*1./Hkt - 1/beta + Wxk'*(Vxt./P)-1;
gfHkt= Hkt.*((Wxk'*(Vxt./P)-1)-(alpha-1)./Hkt - ones(size(Hkt))*1/beta); %without background (->not Wxkgb) and d(log(L)/dcx)
% d(log(L))/dcx:
gfcx=diag(Wxtcx'*(Vxt./P-ones(peval.nx*peval.ny, peval.nt))*Hkt');
% d(log(L))/dcy:
gfcy=diag(Wxtcy'*(Vxt./P-ones(peval.nx*peval.ny, peval.nt))*Hkt');

gf = [reshape(gfHkt,1,peval.nt*peval.ncomp), gfcx', gfcy'];
% gf = [reshape(gfHkt,1,peval.nt*peval.ncomp)];
% gf = [gfcx', gfcy'];
gf=-gf; %conjugate gradient is minimizing!
end

function xxvc = lineargrad(sizevec, cx, dir)
switch dir
    case 'xx'
        xxp=double(xx(sizevec, 'corner')); %linear function - pixels
    case 'yy'
        xxp=double(yy(sizevec, 'corner')); %linear function - pixels
    otherwise
        error('Wrong dir')
end
xxv=reshape(xxp,sizevec(1)*sizevec(2),sizevec(3)); %linear function - vector
xxvc=xxv-repmat(cx,sizevec(1)*sizevec(2),1);
end


classdef FDA3 < PROBLEM
% <multi> <real> <large/none> <dynamic>
% Benchmark dynamic MOP proposed by Farina, Deb, and Amato
% taut --- 10 --- Number of generations for static optimization
% nt   --- 10 --- Number of distinct steps

%------------------------------- Reference --------------------------------
% M. Farina, K. Deb, and P. Amato, Dynamic multiobjective optimization
% problems: Test cases, approximations, and applications, IEEE Transactions
% on Evolutionary Computation, 2004, 8(5): 425-442.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        taut;       % Number of generations for static optimization
        nt;         % Number of distinct steps
        Optimums;   % Point sets on all Pareto fronts
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.taut,obj.nt] = obj.ParameterSet(10,10);
            obj.M = 2;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = [0,-ones(1,obj.D-1)];
            obj.upper    = [1, ones(1,obj.D-1)];
            obj.encoding = ones(1,obj.D);
        end
        %% Evaluate solutions
        function Population = Evaluation(obj,varargin)
            PopDec     = obj.CalDec(varargin{1});
            PopObj     = obj.CalObj(PopDec);
            PopCon     = obj.CalCon(PopDec);
            % Attach the current number of function evaluations to solutions
            Population = SOLUTION(PopDec,PopObj,PopCon,zeros(size(PopDec,1),1)+obj.FE);
            obj.FE     = obj.FE + length(Population);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            t = floor(obj.FE/obj.N/obj.taut)/obj.nt;
            PopObj(:,1) = PopDec(:,1).^(10.^(2*sin(0.5*pi*t)));
            G = abs(sin(0.5*pi*t));
            g = 1 + G + sum((PopDec(:,2:end)-G).^2,2);
            h = 1 - (PopObj(:,1)./g).^0.5;
            PopObj(:,2) = g.*h;
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            % Generate point sets on all Pareto fronts
            t = floor(0:obj.maxFE/obj.N/obj.taut)/obj.nt;
            G = abs(sin(0.5.*pi.*t));
            G = unique(round(G*1e6)/1e6);
            x = linspace(0,1,N)';
            obj.Optimums = {};
            for i = 1 : length(G)
                g = 1 + G(i);
                obj.Optimums(i,:) = {G(i),[x,g*(1-sqrt(x/g))]};
            end
            % Combine all point sets
            R = cat(1,obj.Optimums{:,2});
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            t      = floor(Population.adds/obj.N/obj.taut)/obj.nt;
            G      = abs(sin(0.5.*pi.*t));
            G      = round(G*1e6)/1e6;
            change = [0;find(G(1:end-1)~=G(2:end));length(G)];
            Scores = zeros(1,length(change)-1);
            allG   = cell2mat(obj.Optimums(:,1));
            for i = 1 : length(change)-1
                subPop    = Population(change(i)+1:change(i+1));
                Scores(i) = feval(metName,subPop,obj.Optimums{find(G(change(i)+1)==allG,1),2});
            end
            score = mean(Scores);
        end
        %% Display a population in the objective space
        function DrawObj(obj,Population)
            t      = floor(Population.adds/obj.N/obj.taut)/obj.nt;
            G      = abs(sin(0.5.*pi.*t));
            G      = round(G*1e6)/1e6;
            change = [0;find(G(1:end-1)~=G(2:end));length(G)];
            allG   = cell2mat(obj.Optimums(:,1));
            tempStream = RandStream('mlfg6331_64','Seed',2);
            for i = 1 : length(change)-1
                color = rand(tempStream,1,3);
                Draw(Population(change(i)+1:change(i+1)).objs,'o','MarkerSize',5,'Marker','o','Markerfacecolor',sqrt(color),'Markeredgecolor',color,{'\it f\rm_1','\it f\rm_2',[]});
                Draw(obj.Optimums{find(G(change(i)+1)==allG,1),2},'-','LineWidth',1,'Color',color);
            end
        end
    end
end


function b = chpsl ( ap, n, ipvt, b )

%*****************************************************************************80
%
%% CHPSL solves a complex hermitian system factored by CHPFA.
%
%  Discussion:
%
%    A division by zero may occur if CHPCO set RCOND to 0.0
%    or CHPFA set INFO nonzero.
%
%    To compute
%
%      inverse ( A ) * C
%
%    where C is a matrix with P columns
%
%      call chpfa(ap,n,ipvt,info)
%
%      if ( info == 0 )
%        do j = 1, p
%          call chpsl(ap,n,ipvt,c(1,j))
%        end do
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex AP(N*(N+1)/2), the output from CHPFA.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer IPVT(N), the pivot vector from CHPFA.
%
%    Input, complex B(N), the right hand side.
%
%    Output, complex B(N), the solution.
%

%
%  Loop backward applying the transformations and inverse ( D ) to B.
%
  k = n;
  ik = ( n * ( n - 1 ) ) / 2;

  while ( 0 < k )

    kk = ik + k;
%
%  1 x 1 pivot block.
%
    if ( 0 <= ipvt(k) )

      if ( k ~= 1 )

        kp = ipvt(k);

        if ( kp ~= k )
          t     = b(k);
          b(k)  = b(kp);
          b(kp) = t;
        end

        b(1:k-1) = b(1:k-1) + b(k) * ap(ik+1:ik+k-1);

      end
%
%  Apply D inverse.
%
      b(k) = b(k) / ap(kk);
      k = k - 1;
      ik = ik - k;

    else
%
%  2 x 2 pivot block.
%
      ikm1 = ik - ( k - 1 );

      if ( k ~= 2 )

        kp = abs ( ipvt(k) );

        if ( kp ~= k - 1 )
          t      = b(k-1);
          b(k-1) = b(kp);
          b(kp)  = t;
        end

        b(1:k-2) = b(1:k-2) + b(k)   * ap(ik+1:ik+k-2);
        b(1:k-2) = b(1:k-2) + b(k-1) * ap(ikm1+1:ikm1+k-2);

      end
%
%  Apply D inverse.
%
      km1k = ik + k - 1;
      kk = ik + k;
      ak = ap(kk) / conj ( ap(km1k) );
      km1km1 = ikm1 + k - 1;
      akm1 = ap(km1km1) / ap(km1k);
      bk = b(k) / conj ( ap(km1k) );
      bkm1 = b(k-1) / ap(km1k);
      denom = ak * akm1 - 1.0;
      b(k) = ( akm1 * bk - bkm1 ) / denom;
      b(k-1) = ( ak * bkm1 - bk ) / denom;
      k = k - 2;
      ik = ik - ( k + 1 ) - k;

    end

  end
%
%  Loop forward applying the transformations.
%
  k = 1;
  ik = 0;

  while ( k <= n )
%
%  1 x 1 pivot block.
%
    if ( 0 <= ipvt(k) )

      if ( k ~= 1 )

        b(k) = b(k) + conj ( ap(ik+1:ik+k-1) ) * transpose ( b(1:k-1) );
        kp = ipvt(k);

        if ( kp ~= k )
          t     = b(k);
          b(k)  = b(kp);
          b(kp) = t;
        end

      end

      ik = ik + k;
      k = k + 1;
%
%  2 x 2 pivot block.
%
    else

      if ( k ~= 1 )

        b(k) = b(k) + conj ( ap(ik+1:ik+k-1) ) * transpose ( b(1:k-1) );
        ikp1 = ik + k;
        b(k+1) = b(k+1) ...
          + conj ( ap(ikp1+1:ikp1+k-1) ) * transpose ( b(1:k-1) );
        kp = abs ( ipvt(k) );

        if ( kp ~= k )
          t     = b(k);
          b(k)  = b(kp);
          b(kp) = t;
        end

      end

      ik = ik + k + k + 1;
      k = k + 2;

    end

  end

  return
end


function [P,model] = chrrParseModel(model)
% Parse a COBRA model into the right format for the CHRR sampler
%
% USAGE:
%
%      [P,model] = chrrParseModel(model);
%
% We are trying to sample uniformly at random from the points v that satisfy:
%
% .. math::
%                     Sv = b\\
%             ~~ l_b \leq v \leq u_b
%
% INPUTS:
%    model:    COBRA model structure with fields:
%
%               * .S - The `m x n` stoichiometric matrix
%               * .lb - `n x 1` lower bounds on fluxes
%               * .ub - `n x 1` upper bounds on fluxes
%
% OPTIONAL INPUTS:
%               * .C - 'k x n' matrix of additional inequality constraints
%               * .d - 'k x 1' rhs of the above constraints
%               * .dsense - 'k x 1' the sense of the above constraints ('L' or 'G')
%
% OUTPUTS:
%    P:        A structure with fields:
%
%               * .A_eq - Equality constraint matrix (`model.S`)
%               * .b_eq - Right hand side of equality constraints (`model.b`)
%               * .A - Inequality constraint matrix (`[I_n 0; 0 -I_n]`)
%               * .b - Right hand side of inequality constraints (`[lb; -ub]`)
%
% .. Authors:
%       - Ben Cousins and Hulda Haraldsdóttir, 10/2016
%       - Ben Cousins, 12/2017, Moved objective function handling to preprocess function
%       - Ben Cousins, 05/2019, Added support for C,d inequalities.

dim = length(model.lb);

P.A = [eye(dim); -eye(dim)];
P.b = [model.ub; -model.lb];

if isfield(model,'C') && isfield(model,'d')
   for i=1:size(model.C,1)
      if model.dsense(i)=='G'
          % convert constraint to <=
          model.C(i,:) = model.C(i,:)*-1;
          model.d(i) = model.d(i)*-1;
      elseif model.dsense{i}=='E'
          error('Equality constraints not supported in C,d fields.');
      end
   end
   P.A = [P.A; model.C];
   P.b = [P.b; model.d];
end

P.A_eq = model.S;
P.b_eq = model.b;

end


function [f,J] = spm_fx_lfp(x,u,P,M)
% state equations for a neural mass model of erps
% FORMAT [f,J] = spm_fx_lfp(x,u,P,M)
% x      - state vector
%   x(:,1)  - voltage (spiny stellate cells)
%   x(:,2)  - voltage (pyramidal cells)         +ve
%   x(:,3)  - voltage (pyramidal cells)         -ve
%   x(:,4)  - current (spiny stellate cells)    +ve
%   x(:,5)  - current (pyramidal cells)         +ve
%   x(:,6)  - current (pyramidal cells)         -ve
%   x(:,7)  - voltage (inhibitory interneurons) +ve
%   x(:,8)  - current (inhibitory interneurons) +ve
%   x(:,9)  - voltage (pyramidal cells)
%   x(:,10) - voltage (inhibitory interneurons) -ve
%   x(:,11) - current (inhibitory interneurons) -ve
%   x(:,12) - voltage (inhibitory interneurons)
%
%   x(:,13) - slow potassium conductance
%
% f    = dx(t)/dt  = f(x(t))
% J    = df/dx
%
% Fixed parameter scaling [Defaults]
%
%  E = [32 16 4];             % extrinsic rates (forward, backward, lateral)
%  G = [1 1 1/2 1/2 1/8]*128; % intrinsic rates (g1, g2, g3, g4, g5)
%  D = [2 16];                % propagation delays (intrinsic, extrinsic)
%  H = [4 32];                % receptor densities (excitatory, inhibitory)
%  T = [4 16];                % synaptic constants (excitatory, inhibitory)
%  R = [2 1];                 % parameters of static nonlinearity
%
%__________________________________________________________________________
%
% This is a simplified version of spm_fx_erp
%
% David O, Friston KJ (2003) A neural mass model for MEG/EEG: coupling and
% neuronal dynamics. NeuroImage 20: 1743-1755
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fx_lfp.m 5369 2013-03-28 20:09:27Z karl $

% check if intrinsic connections are free parameters
%--------------------------------------------------------------------------
try, P.H; catch, P.H = 0; end

% get dimensions and configure state variables
%--------------------------------------------------------------------------
x    = spm_unvec(x,M.x);       % neuronal states
n    = size(x,1);              % number of sources
s    = size(x,2);              % number of states

% [default] fixed parameters
%--------------------------------------------------------------------------
E    = [32 16 4];              % extrinsic rates (forward, backward, lateral)
G    = [1 1 1/2 1/2 1/32]*128; % intrinsic rates (g1, g2 g3, g4)
D    = [2 4];                  % propagation delays (intrinsic, extrinsic)
H    = [8 32];                 % receptor densities (excitatory, inhibitory)
T    = [4 16];                 % synaptic constants (excitatory, inhibitory)
R    = [1 2];                  % parameters of static nonlinearity

% [specified] fixed parameters
%--------------------------------------------------------------------------
if isfield(M,'pF')
    try, E  = M.pF.E; end
    try, G  = M.pF.H; end
    try, D  = M.pF.D; end
    try, H  = M.pF.G; end
    try, T  = M.pF.T; end
    try, R  = M.pF.R; end
end

% exponential transform to ensure positivity constraints
%--------------------------------------------------------------------------
A{1} = exp(P.A{1})*E(1);
A{2} = exp(P.A{2})*E(2);
A{3} = exp(P.A{3})*E(3);
C    = exp(P.C);
G    = exp(P.H)*diag(G);

% intrinsic connectivity and parameters
%--------------------------------------------------------------------------
Te   = T(1)/1000*exp(P.T(:,1));      % excitatory time constants
Ti   = T(2)/1000*exp(P.T(:,2));      % inhibitory time constants
Tk   = 512/1000;                     % slow potassium
He   = H(1)*exp(P.G);                % excitatory receptor density
Hi   = H(2);                         % inhibitory receptor density

% pre-synaptic inputs: s(V) with threshold adaptation
%--------------------------------------------------------------------------
R      = R.*exp(P.R);
x      = x';
X      = x;
X(1,:) = X(1,:) - X(13,:);
S      = 1./(1 + exp(-R(1)*(X - R(2)))) - 1./(1 + exp(R(1)*R(2)));
dSdx   = R(1)*exp(-R(1)*(max(X,-128) - R(2)))./(1 + exp(-R(1)*(X - R(2)))).^2;

% input
%==========================================================================
if isfield(M,'u')

    % endogenous input
    %----------------------------------------------------------------------
    U = u(:)*32;

else
    % exogenous input
    %----------------------------------------------------------------------
    U = C*u(:);
end

% State: f(x) and Jacobian dfdx
%==========================================================================

% NB: activity-dependent reduction in inhibitory effective time-constant
%--------------------------------------------------------------------------
Ti    = 4/1000 + Ti;

% intrinsic coupling
%--------------------------------------------------------------------------
for i = 1:n

    % synaptic dynamics - dfdx
    %----------------------------------------------------------------------
    ke    = -2/Te(i);
    ki    = -2/Ti(i);
    Ke    = -1/(Te(i)^2);
    Ki    = -1/(Ti(i)^2);
    dfdx  = [0   0   0   1   0   0   0   0   0   0   0   0   0
             0   0   0   0   1   0   0   0   0   0   0   0   0
             0   0   0   0   0   1   0   0   0   0   0   0   0
             Ke  0   0   ke  0   0   0   0   0   0   0   0   0
             0   Ke  0   0   ke  0   0   0   0   0   0   0   0
             0   0   Ki  0   0   ki  0   0   0   0   0   0   0
             0   0   0   0   0   0   0   1   0   0   0   0   0
             0   0   0   0   0   0  Ke   ke  0   0   0   0   0
             0   0   0   0   1   -1  0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   1   0   0
             0   0   0   0   0   0   0   0   0  Ki   ki  0   0
             0   0   0   0   0   0   0   1   0   0  -1   0   0
             0   0   0   0   0   0   0   0   0   0   0   0  -1/Tk];

    % intrinsic afferents - dfdS
    %----------------------------------------------------------------------
    Ke    = He(i)/Te(i);
    Ki    = Hi/Ti(i);

    j1    = Ke*G(i,1);
    j2    = Ke*G(i,2);
    j3    = Ke*G(i,3);
    j4    = Ki*G(i,4);
    j5    = Ki*G(i,5);
    dfdS  = [0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   j1  0   0   0   0
             j2  0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   j4  0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   j3  0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0  j5   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
            4/Tk 0   0   0   0   0   0   0   0   0   0   0   0];

    % motion and Jacobian
    %----------------------------------------------------------------------
    dsdx       = diag(dSdx(:,i));
    dsdx(1,13) = -dsdx(1,1);
    dfdu       = sparse(4,1,Ke,s,1);

    F{i}       = dfdx*x(:,i) + dfdS*S(:,i) + dfdu*U(i);
    J{i,i}     = dfdx + dfdS*dsdx;

    % extrinsic afferents
    %----------------------------------------------------------------------
    for j = 1:n, if i ~= j

    k1    = Ke*(A{1}(i,j) + A{3}(i,j));
    k2    = Ke*(A{2}(i,j) + A{3}(i,j));
    k3    = Ki*(A{2}(i,j) + A{3}(i,j));
    dfdS  = [0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   k1  0   0   0   0
             0   0   0   0   0   0   0   0   k2  0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   k3  0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0];

    % motion and Jacobian
    %----------------------------------------------------------------------
    F{i}   = F{i} + dfdS*S(:,j);
    J{i,j} = dfdS*diag(dSdx(:,j));

    end, end
end

% construct motion and Jacobian
%--------------------------------------------------------------------------
for i = 1:n
    k      = (1:n:s*n) + (i - 1);
    f(k,1) = F{i};
    for j  = 1:n
        l         = [1:n:s*n] + (j - 1);
        dfdx(k,l) = J{i,j};
    end
end

% extrinsic and intrinsic delays
%--------------------------------------------------------------------------
De = D(2).*exp(P.D)/1000;
Di = D(1).*exp(P.I)/1000;
De = (eye(n,n) - 1).*De;
Di = (eye(s,s) - 1)*Di;
De = kron(ones(s,s),De);
Di = kron(Di,eye(n,n));

D  = Di + De;

% Implement: dx(t)/dt = f(x(t + d)) = inv(1 - D.*dfdx)*f(x(t))
%--------------------------------------------------------------------------
D  = spm_inv(speye(n*s,n*s) - D.*dfdx);
f  = D*f;
J  = D*dfdx;

return

% Equations of motion
%==========================================================================

% Supragranular layer (inhibitory interneurons): depolarizing current
%--------------------------------------------------------------------------
f(:,7)  = x(:,8);
f(:,8)  = (He.*((A{2} + A{3})*S(:,9) + G(:,3).*S(:,9)) ...
           - 2*x(:,8) - x(:,7)./Te)./Te;

% Supragranular layer (inhibitory interneurons): hyperpolarizing current
%--------------------------------------------------------------------------
f(:,10) = x(:,11);
f(:,11) = (Hi*G(:,5).*S(:,12) ...
           - 2*x(:,11) - x(:,10)./Ti)./Ti;

% Granular layer (spiny stellate cells): depolarizing current
%--------------------------------------------------------------------------
f(:,1)  = x(:,4);
f(:,4)  = (He.*((A{1} + A{3})*S(:,9) + G(:,1).*S(:,9) + U) ...
           - 2*x(:,4) - x(:,1)./Te)./Te;

% Infra-granular layer (pyramidal cells): depolarizing current
%--------------------------------------------------------------------------
f(:,2)  = x(:,5);
f(:,5)  = (He.*((A{2} + A{3})*S(:,9) + G(:,2).*S(:,1)) ...
           - 2*x(:,5) - x(:,2)./Te)./Te;

% Infra-granular layer (pyramidal cells): hyperpolarizing current
%--------------------------------------------------------------------------
f(:,3)  = x(:,6);
f(:,6)  = (Hi*G(:,4).*S(:,12) ...
           - 2*x(:,6) - x(:,3)./Ti)./Ti;

% Surpa and Infra-granular layer (pyramidal cells): Voltage
%--------------------------------------------------------------------------
f(:,9)  = x(:,5) - x(:,6);
f(:,12) = x(:,8) - x(:,11);

% Granular layer (spiny stellate cells): hyperpolarizing current
%--------------------------------------------------------------------------
f(:,13) = (4*S(:,1) - x(:,13))./Tk;

% Jacobian for delays (evaluate numerically for simplicity)
%==========================================================================
dfdx = spm_diff('spm_fx_lfp',x,u,P,1,1);


function F  = obj_actuator_freq_resp_param(x,amplitude_40, frequency_40, amplitude_100, frequency_100)
% Computes objective function for determination of basic parameters of
% the proportional valve actuator that match the required frequency
% response. The frequency response of a nonlinear system is obtained by
% using frestimate.  The required and actual frequency responses are
% compared.
% Copyright 2010 MathWorks, Inc.

% Manufacturer's characterstic
%RefFR_100_Frq =     [7 10 20 30 40 43 50 54];
%RefFR_100_Phs = -1*[25 35 59 75 87 90 96 100];
%RefFR_40_Frq =     [7 10 20 30 40 50 57 70];
%RefFR_40_Phs = -1*[21 30 50 63 75 85 90 100];

model = 'actuator_freq_resp';
load_system(model);

assignin('base','act_gain', x(1));
assignin('base','time_const', x(2));
assignin('base','act_saturation', x(3));

% Computing phase angle at current values of variable parameters

% Determine inputs and outputs
ios = getlinio(model);

% Generate input at test frequencies
in100 = frest.Sinestream(...
   'Frequency',frequency_100, ...
   'Amplitude', amplitude_100*ones(size(frequency_100)), ...
   'SimulationOrder', 'Sequential', ...
   'FreqUnits', 'Hz');

in40 = frest.Sinestream(...
   'Frequency',frequency_40, ...
   'Amplitude', amplitude_40*ones(size(frequency_40)), ...
   'SimulationOrder', 'Sequential', ...
   'FreqUnits', 'Hz');

% Test model, determine phase
sys100 = frestimate(model,ios,in100);
R100 = sys100.ResponseData(:);
Lag = angle(R100)/pi*180;
phase_100_20 = Lag(1); phase_100_43 = Lag(2);

% Test model, determine phase
sys40 = frestimate(model,ios,in40);
R40 = sys40.ResponseData(:);
Lag = angle(R40)/pi*180;
phase_40_20 = Lag(1); phase_40_57 = Lag(2);

% Calculate difference from manufacturer's characteristic
F = (phase_100_20 + 59)^2 + (phase_100_43 + 90)^2 + ...
    (phase_40_20 + 50)^2 + (phase_40_57 + 90)^2;

end


function [D,D1ols,q,C]=irfiv_ols_for_bootstrap_GK(txt,names,IVrotate,betadraw,n,Xdraw,Ydraw,k,p,enddate,startdate,cut1,cut2,cut3,cut4)
%% Copyright Ben Schumann
% function [D, gamma]=bear.irfiv_ols(names, betahat,sigmahat, n,X,Y,k,p,enddate,startdate)
% instrumental variable identification in an OLS setting
% inputs:  - matrix 'betahat': vec(OLS estimates of the reduced form)
%          - matrix 'sigmahat': vec(OLS estimates of sigma)
%          - matrix 'X': Independend Variable
%          - matrix 'Y': Dependend Variable
%          - matrix 'IV': Rotated IV from wild bootstrap
%          - integer 'IRFperiods': number of periods for IRFs
%          - integer 'n': number of endogenous variables in the VAR model (defined p 7 of technical guide)
%          - integer 'm': number of exogenous variables in the VAR model (defined p 7 of technical guide)
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
%          - integer 'T': number of observations
%          - string  'stardate': VAR startdate
%          - string  'enddate': VAR enddate
% outputs: - matrix 'D': record of the accepted draws for the structural matrix D
%          - matrix 'gamma': record of the draws for the structural disturbances variance-covariance matrix gamma
IV = IVrotate;
%% Preparation for first stage regression
%get reduced form residuals
beta = betadraw;
B    = reshape(beta,k,n);
EPS  = Ydraw-Xdraw*B;

[EPSIV,IVcut] = bear.cut_EPS_IV_GK_new(txt, names, EPS, IV, cut1, cut2, cut3, cut4, startdate, enddate, p);

%% Imposing the covariance restrictions
%E_1 = EPSIV'*IVcut/length(IVcut);
%E11 = E_1(1,:);
%E21 = E_1(2:end,:);
%Mu = E21*E11^(-1); %relative impulse vector
%% normalize to a one standard deviation shock
sigmahatIV=(1/(length(EPSIV)-k))*(EPSIV'*EPSIV);

%get the gamma vector
%partition the reduced form VCV
%Sigma11 = sigmahatIV(1,1);
%Sigma12 = sigmahatIV(1,2:end);
%Sigma21 = sigmahatIV(2:end,1);
%Sigma22 = sigmahatIV(2:end,2:end);

%Gamma = Sigma22 + Mu*Sigma11*Mu' - Sigma21*Mu' - Mu*Sigma21'; %%%%%Gamma ouput is not used after this
%get b12 as in Michelle Piffers notes
%b12b12t = (Sigma21-Mu*Sigma11)'*Gamma^(-1)*(Sigma21-Mu*Sigma11);
%b11b11t = Sigma11 - b12b12t;
% b11 = chol(bear.nspd(b11b11t)); %%this is the scaling vector

%% first stage regression (this results in the same vector as Mu)

%step 2: Regress the first reduced form shock on the instrument
Shock = EPSIV(:,1);
[nobs , ~] = size(IVcut);
XX = [ones(nobs,1) IVcut];
[~, nvar] = size(XX);
%get OLS estimate
XpXi = (XX'*XX)\eye(nvar);
betaIV=XpXi*(XX'*Shock);
%get predicted value
IVpred = XX*betaIV;

%% second stage regression
%step 3: Regress the other reduced form shocks on the predicted value
ImpactIRFIV = zeros(n,1);
ImpactIRFIV(1,1) = 1;

for hh=2:n
Shock = EPSIV(:,hh);
[nobs,~]= size(IVpred);
IVpredtemp = [ones(nobs,1) IVpred];
[~,nvar] = size(IVpredtemp);
IVpIVi = (IVpredtemp'*IVpredtemp)\eye(nvar);
betaIV2=IVpIVi*(IVpredtemp'*Shock);
ImpactIRFIV(hh,1) = betaIV2(2,1); %should be equal to Mu from 2:end
end

% step 5: Create the structural matrix and only fill the first column as
% this is the only one identified
D=zeros(n,n);
% Step 6: Replace the first Column in the Cholesky Decomposition by
%the structural impact matrix computed above
%%another way to retrieve b11 (the scalar that scales the IRF to be a 1sdt Shock) is simply
C=chol(bear.nspd(sigmahatIV),'lower');
b=ImpactIRFIV;
%%Recover the vector q that maps the first column of C into b such that Cq=b;
q = C\b;
%%b11 is the euclidian length of q
b11q = 1/norm(q);

%
D(1:end,1) = ImpactIRFIV*b11q;
D1ols = ImpactIRFIV*b11q; % for the IRFt6 TakeOLS option


function [ f,qualMeasOut] = PCSD(proj,geo,angles,maxiter,varargin)
%PCSD solves the reconstruction problem using projection-controlled steepest descent method
%
%   PCSD(PROJ,GEO,ALPHA,NITER) solves the reconstruction problem using
%   the projection data PROJ taken over ALPHA angles, corresponding to the
%   geometry described in GEO, using NITER iterations.
%
%   PCSD(PROJ,GEO,ALPHA,NITER,OPT,VAL,...) uses options and values for solving. The
%   possible options in OPT are:
%
%
%   'lambda':      Sets the value of the hyperparameter for the SART iterations.
%                  Default is 1
%
%   'lambdared':   Reduction of lambda.Every iteration
%                  lambda=lambdared*lambda. Default is 0.99
%
%       'init':    Describes diferent initialization techniques.
%                   •  'none'     : Initializes the image to zeros (default)
%                   •  'FDK'      : intializes image to FDK reconstrucition
%
%   'TViter':      Defines the amount of TV iterations performed per SART
%                  iteration. Default is 20
%
%   'maxL2err'     Maximum L2 error to accept an image as valid. This
%                  parameter is crucial for the algorithm, determines at
%                  what point an image should not be updated further.
%                  Default is 20% of the FDK L2 norm.
%   'Verbose'      1 or 0. Default is 1. Gives information about the
%                  progress of the algorithm.
% 'redundancy_weighting': true or false. Default is true. Applies data
%                         redundancy weighting to projections in the update step
%                         (relevant for offset detector geometry)
%  'groundTruth'  an image as grounf truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri and Manasavee Lohvithee
%--------------------------------------------------------------------------

%% parse inputs
[beta,beta_red,f,ng,verbose,epsilon,QualMeasOpts,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,varargin);

measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    res_prev=gt;
    clear gt
end
if nargout<2 && measurequality
    warning("Image metrics requested but none catched as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);

% does detector rotation exists?
if ~isfield(geo,'rotDetector')
    geo.rotDetector=[0;0;0];
end

%% Create weigthing matrices for the SART step
% the reason we do this, instead of calling the SART fucntion is not to
% recompute the weigths every AwASD-POCS iteration, thus effectively doubling
% the computational time
% Projection weigth, W

geoaux=geo;
geoaux.sVoxel([1 2])=geo.sVoxel([1 2])*1.1; % a Bit bigger, to avoid numerical division by zero (small number)
geoaux.sVoxel(3)=max(geo.sDetector(2),geo.sVoxel(3)); % make sure lines are not cropped. One is for when image is bigger than detector and viceversa
geoaux.nVoxel=[2,2,2]'; % accurate enough?
geoaux.dVoxel=geoaux.sVoxel./geoaux.nVoxel;
W=Ax(ones(geoaux.nVoxel','single'),geoaux,angles,'Siddon','gpuids',gpuids);
W(W<min(geo.dVoxel)/4)=Inf;
W=1./W;

% Compute V
V=computeV(geo,angles,num2cell(angles),num2cell(1:length(angles)),'gpuids',gpuids);

if redundancy_weights
    % Data redundancy weighting, W_r implemented using Wang weighting
    % reference: https://iopscience.iop.org/article/10.1088/1361-6560/ac16bc

    num_frames = size(proj,3);
    W_r = redundancy_weighting(geo);
    W_r = repmat(W_r,[1,1,num_frames]);
    % disp('Size of redundancy weighting matrix');
    % disp(size(W_r));
    W = W.*W_r; % include redundancy weighting in W
end

%Initialize image.
%f=zeros(geo.nVoxel','single');

iter=0;
offOrigin=geo.offOrigin;
offDetector=geo.offDetector;
rotDetector=geo.rotDetector;
stop_criteria=0;
DSD=geo.DSD;
DSO=geo.DSO;
%%
while ~stop_criteria %POCS
    % If quality is going to be measured, then we need to save previous image
    if measurequality && ~strcmp(QualMeasOpts,'error_norm')
        res_prev = f; % only store if necesary
    end
    if (iter==0 && verbose==1);tic;end
    iter=iter+1;

    %Estimation error in the projection domain
    est_proj=Ax(f,geo,angles,'interpolated','gpuids',gpuids);
    delta_p=im3Dnorm(est_proj-proj,'L2');

    %Enforcing ART along all projections if squared delta_p > epsilon
    if (delta_p^2)>epsilon
        for jj=1:size(angles,2)
            if size(offOrigin,2)==size(angles,2)
                geo.offOrigin=offOrigin(:,jj);
            end
            if size(offDetector,2)==size(angles,2)
                geo.offDetector=offDetector(:,jj);
            end
            if size(rotDetector,2)==size(angles,2)
                geo.rotDetector=rotDetector(:,jj);
            end
            if size(DSD,2)==size(angles,2)
                geo.DSD=DSD(jj);
            end
            if size(DSO,2)==size(angles,2)
                geo.DSO=DSO(jj);
            end
            f=f+beta* bsxfun(@times,1./V(:,:,jj),Atb(W(:,:,jj).*(proj(:,:,jj)-Ax(f,geo,angles(:,jj),'gpuids',gpuids)),geo,angles(:,jj),'gpuids',gpuids));

        end
    end

    %Non-negativity projection on all pixels
    if nonneg
        f=max(f,0);
    end

    geo.offDetector=offDetector;
    geo.offOrigin=offOrigin;
    geo.DSD=DSD;
    geo.DSO=DSO;
    geo.rotDetector=rotDetector;
    if measurequality
        qualMeasOut(:,iter)=Measure_Quality(res_prev,f,QualMeasOpts);
    end

    % Compute L2 error of actual image. Ax-b
    dd=im3Dnorm(Ax(f,geo,angles,'gpuids',gpuids)-proj,'L2');
    % Compute change in the image after last SART iteration
    dp_vec=(f-f0);

    if iter==1
        step=1;
    else
        step=delta_p/delta_p_first;
    end
    f0=f;
    %  TV MINIMIZATION
    % =========================================================================
    %  Call GPU to minimize TV
    f=minimizeTV(f0,step,ng,'gpuids',gpuids);    %   This is the MATLAB CODE, the functions are sill in the library, but CUDA is used nowadays
    %                                             for ii=1:ng
    %                                                 %delta=-0.00038 for thorax phantom
    %                                                 df=weighted_gradientTVnorm(f,delta);
    %                                                 df=df./im3Dnorm(df,'L2');
    %                                                 f=f-(step.*df);
    %                                             end

    % Compute change by TV min
    dg_vec=(f-f0);

    if iter==1
        delta_p_first=im3Dnorm((Ax(f0,geo,angles,'interpolated','gpuids',gpuids))-proj,'L2');
    end

    % Reduce SART step
    beta=beta*beta_red;

    % Check convergence criteria
    % ==========================================================================

    %Define c_alpha as in equation 21 in the journal
    c=dot(dg_vec(:),dp_vec(:))/(norm(dg_vec(:),2)*norm(dp_vec(:),2));
    %This c is examined to see if it is close to -1.0

    if (c<-0.99 && dd<=epsilon) || beta<0.005|| iter>maxiter
        if verbose
            disp(['Stopping criteria met']);
            disp(['   c    = ' num2str(c)]);
            disp(['   beta = ' num2str(beta)]);
            disp(['   iter = ' num2str(iter)]);
        end
        stop_criteria=true;
    end

    if (iter==1 && verbose==1)
        expected_time=toc*maxiter;
        disp('PCSD');
        disp(['Expected duration  :    ',secs2hms(expected_time)]);
        disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
        disp('');
    end

end
end

function [beta,beta_red,f0,ng,verbose,epsilon,QualMeasOpts,nonneg,gpuids,redundancy_weights]=parse_inputs(proj,geo,angles,argin)
opts=     {'lambda','lambda_red','init','tviter','verbose','maxl2err','qualmeas','nonneg','gpuids','redundancy_weighting'};
defaults=ones(length(opts),1);
% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('CBCT:PCSD:InvalidInput','Invalid number of inputs')
end

% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('CBCT:PCSD:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option isnot default, then extract value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('CBCT:PCSD:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end
    % parse inputs
    switch opt
        % Verbose
        %  =========================================================================
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
        % Lambda
        %  =========================================================================
        case 'lambda'
            if default
                beta=1;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:PCSD:InvalidInput','Invalid lambda')
                end
                beta=val;
            end
        % Lambda reduction
        %  =========================================================================
        case 'lambda_red'
            if default
                beta_red=0.99;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:PCSD:InvalidInput','Invalid lambda')
                end
                beta_red=val;
            end
        % Initial image
        %  =========================================================================
        case 'init'
            if default || strcmp(val,'none')
                f0=zeros(geo.nVoxel','single');

            else
                if strcmp(val,'FDK')
                    f0=FDK(proj, geo, angles);
                else
                    error('TIGRE:PCSD:InvalidInput','Invalid init')
                end
            end
        % Number of iterations of TV
        %  =========================================================================
        case 'tviter'
            if default
                ng=20;
            else
                ng=val;
            end
        %  Maximum L2 error to have a "good image"
        %  =========================================================================
        case 'maxl2err'
            if default
                epsilon=im3Dnorm(FDK(proj,geo,angles))*0.2; %heuristic
            else
                epsilon=val;
            end
        %Image Quality Measure
        %  =========================================================================
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('TIGRE:PCSD:InvalidInput','Invalid quality measurement parameters');
                end
            end
        %  Non negative
        %  =========================================================================
        case 'nonneg'
            if default
                nonneg=true;
            else
                nonneg=val;
            end
        %  GPU Ids
        %  =========================================================================
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'redundancy_weighting'
            if default
                redundancy_weights = true;
            else
                redundancy_weights = val;
            end
        otherwise
            error('TIGRE:PCSD:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option in PCSD()']);

    end
end

end


function [cst,cstJac] = autoGen_cst_footVel(q1p,q2p,q4p,q5p,q1m,q2m,q4m,q5m,dq1p,dq2p,dq4p,dq5p,dq1m,dq2m,dq4m,dq5m,l1,l2,l4,l5)
%AUTOGEN_CST_FOOTVEL
%    [CST,CSTJAC] = AUTOGEN_CST_FOOTVEL(Q1P,Q2P,Q4P,Q5P,Q1M,Q2M,Q4M,Q5M,DQ1P,DQ2P,DQ4P,DQ5P,DQ1M,DQ2M,DQ4M,DQ5M,L1,L2,L4,L5)

%    This function was generated by the Symbolic Math Toolbox version 6.2.
%    22-Oct-2015 19:14:34

t2 = sin(q1p);
t3 = sin(q2p);
t4 = sin(q4p);
t5 = sin(q5p);
t6 = sin(q1m);
t7 = sin(q2m);
t8 = sin(q4m);
t9 = sin(q5m);
cst = [dq1p.*l1.*t2+dq2p.*l2.*t3-dq4p.*l4.*t4-dq5p.*l5.*t5;-dq1m.*l1.*t6-dq2m.*l2.*t7+dq4m.*l4.*t8+dq5m.*l5.*t9];
if nargout > 1
    cstJac = reshape([0.0,0.0,dq1p.*l1.*cos(q1p),0.0,dq2p.*l2.*cos(q2p),0.0,0.0,0.0,-dq4p.*l4.*cos(q4p),0.0,-dq5p.*l5.*cos(q5p),0.0,l1.*t2,0.0,l2.*t3,0.0,0.0,0.0,-l4.*t4,0.0,-l5.*t5,0.0,0.0,0.0,0.0,-dq1m.*l1.*cos(q1m),0.0,-dq2m.*l2.*cos(q2m),0.0,0.0,0.0,dq4m.*l4.*cos(q4m),0.0,dq5m.*l5.*cos(q5m),0.0,-l1.*t6,0.0,-l2.*t7,0.0,0.0,0.0,l4.*t8,0.0,l5.*t9],[2, 22]);
end


function sol = gsp_regression_tik(G ,M, y , tau, param )
%GSP_REGRESSION_TIK Regression using graph and Tikhonov
%   Usage: sol = gsp_regression_tik(G ,M, y , tau );
%          sol = gsp_regression_tik(G ,M, y , tau, param );
%
%   Input parameters:
%       G   : Graph
%       M   : Mask (to determine with label is known)
%       y   : label (total size of the problem)
%       tau : regularization parameter (weight for tv)
%       param : optional structure of parameters
%
%   Output parameters:
%       sol : Solution of the problem
%
%   This function solve the following problem
%
%   .. argmin_x  || M x - y ||_2^2 + tau || nabla_G x ||_2^2
%
%   If tau is set to zero, then the following problem is solved
%
%   ..  argmin_x   || nabla_G x ||_2^2    s. t.  M x - y = 0
%
%   For the las problem, this function can compute an exact solution if
%   *param.exact* is activated. It will be efficient if the number of
%   unlabelled points is low.
%
%   Additional parameters
%   ---------------------
%
%   * *param.verbose* : Verbosity of the iterative algorithm
%   * *param.maxit* : maximum number of iteration for PCG
%
%   This function uses the UNLocBoX.
%
%   See also: gsp_classification_tik gsp_regression_tv
%

% Author: Nathanael Perraudin
% Date  : 24 July 2015
% Testing: test_graph_ml

%% Optional parameters

if nargin<5
    param = struct;
end

if nargin<4
    tau = 0;
end

if ~isfield(param,'verbose'), param.verbose = 1; end
if ~isfield(param,'tol'), param.tol = 1e-6; end
if ~isfield(param,'maxit'), param.maxit = 200; end
% if ~isfield(param,'direct'), param.direct = (tau==0); end
% if ~isfield(param,'exact'), param.exact = (numel(M)-nnz(M))<1000; end

if tau==0
    if param.verbose
        fprintf('Using direct solution \n')
    end
    if (numel(M) == size(M,1)) || (numel(M) == size(M,2))
        indl = find(M);
        indu = find(1-M);
    else
        error('I cannot handle this case yet');
    end
    Luu = (G.L(indu,indu));
    Wul = - G.L(indu,indl);
    tmp = (Wul * y(indl,:));
%     if ~param.exact
%         if ~isfield(param,'order'), param.order = 30; end
%         Gtemp.L = Luu;
%         Gtemp.N = size(Luu,1);
% %        Gtemp = gsp_estimate_lmax(Gtemp);
% %         cheb_coeffs = gsp_cheby_coeff(Gtemp, @(x) pinv_n(x,1e-8),...
% %         param.order, param.order +1);
% %         solt = gsp_cheby_op(Gtemp, cheb_coeffs, tmp);
%         paramt.method = 'lanczos';
%         paramt.order = param.order;
%         solt = gsp_filter_analysis(Gtemp, @(x) pinv_n(x,1e-8),tmp,paramt);
%
%     else
%        solt = pinv(full(Luu)) * tmp;
         solt = Luu \ tmp;
%     end
    sol = y;
    sol(indu,:) = solt;
    return
else

    %% prepare the graph
    if ~isfield(G,'lmax')
        G = gsp_estimate_lmax(G);
    end

    %% set the
    % setting the function f2 (see unlocbox for help)

    Mop =@(x) bsxfun(@times,M,x);

%     fg.grad = @(x) 2*Mop(Mop(x)-y);
%     fg.eval = @(x) norm(Mop(x)-y)^2;
%     fg.beta = 2;
% %     paramtik.verbose = param.verbose -1;
% %     ftik.prox = @(x,T) gsp_prox_tik(x,tau * T,G,paramtik);
%     ftik.eval = @(x) tau* sum(gsp_norm_tik(G,x));
%     ftik.grad = @(x) 2 * G.L * x;
%     ftik.beta = 2 * G.lmax;

%
%
% else
% %     param_b2.verbose = param.verbose -1;
% %     param_b2.y = y;
% %     param_b2.A = @(x) M.*x;
% %     param_b2.At = @(x) M.*x;
% %     param_b2.tight = param.tight;
% %     param_b2.epsilon = 0;
% %     fproj.prox = @(x,T) proj_b2(x,T,param_b2);
% %     fproj.eval = @(x) eps;
%
%     fproj.prox = @(x,T) x - Mop(x) + Mop(y);
%     fproj.eval = @(x) eps;
%     ftik.eval = @(x) sum(gsp_norm_tik(G,x));
%     Ltmp = G.L + G.L';
%     ftik.grad = @(x) Ltmp*x;
%     ftik.beta = 2*G.lmax;
% end

% %% solve the problem
%
% % setting different parameter for the simulation
% paramsolver = param;
%
% if tau > 0
%     sol = forward_backward(y,ftik,fg,paramsolver);
% else
%     sol = forward_backward(y,fproj,ftik,paramsolver);
% end

% sol = sol(logical(1-M));
%
% sol = reshape(sol,[],size(M,2));

    A = @(z)  vec ( Mop(reshape(z,[],size(y,2))) ...
        +  tau * G.L * reshape(z,[],size(y,2)));
    b =  vec(Mop(y));
    sol = pcg(A,b,param.tol,param.maxit,[],[],b);
    sol = reshape(sol,[],size(y,2));
end

end

% function r =  pinv_n(x,t)
%
% r = double(abs(x)>t) .* 1./x;
%
% end

%   * *param.direct* : Direct computation of the exact solution (only for
%     $\tau = 0$). (Default tau==0)
%   * *param.exact* : Exact computation of the exact solution (only for
%     $\tau = 0$ and param.direct = 0). (Default: (numel(M)-nnz(M))<1000 )


%% FUNCTION Least_TGL
% L21 Joint Feature Learning with Least Squares Loss.
%
%% OBJECTIVE
% argmin_W { sum_i^t (0.5 * norm (Y{i} - X{i}' * W(:, i))^2)
%            + opts.rho_L2 * \|W\|_2^2 + rho1 * \|W\|_{2,1} }
%
%% INPUT
% X: {n * d} * t - input matrix
% Y: {n * 1} * t - output matrix
% rho1: L2,1-norm group Lasso parameter.
% optional:
%   opts.rho_L2: L2-norm parameter (default = 0).
%
%% OUTPUT
% W: model: d * t
% funcVal: function value vector.
%
%% LICENSE
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] Evgeniou, A. and Pontil, M. Multi-task feature learning, NIPS 2007.
%   [2] Liu, J. and Ye, J. Efficient L1/Lq Norm Regularization, Technical
%       Report, 2010.
%
%% RELATED FUNCTIONS
%  Least_L21, init_opts

%% Code starts here
function [W, funcVal] = Least_L21(X, Y, rho1, opts)

if nargin <3
    error('\n Inputs: X, Y, rho1, should be specified!\n');
end
X = multi_transpose(X);

if nargin <4
    opts = [];
end

% initialize options.
opts=init_opts(opts);

if isfield(opts, 'rho_L2')
    rho_L2 = opts.rho_L2;
else
    rho_L2 = 0;
end

task_num  = length (X);
dimension = size(X{1}, 1);
funcVal = [];

% initialize a starting point
if isfield(opts,'W0')
    W0=opts.W0;
    if (nnz(size(W0)-[dimension, task_num]))
        error('\n Check the input .W0');
    end
elseif opts.init==2
    W0 = zeros(dimension, task_num);
elseif opts.init == 0
    XY = cell(task_num, 1);
    W0_prep = [];
    for t_idx = 1: task_num
        XY{t_idx} = X{t_idx}*Y{t_idx};
        W0_prep = cat(2, W0_prep, XY{t_idx});
    end
    W0 = W0_prep;
end

bFlag=0; % this flag tests whether the gradient step only changes a little

Wz= W0;
Wz_old = W0;

t = 1;
t_old = 0;

iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ws = (1 + alpha) * Wz - alpha * Wz_old;

    % compute function value and gradients of the search point
    gWs  = gradVal_eval(Ws);
    Fs   = funVal_eval (Ws);

    while true
        Wzp = FGLasso_projection(Ws - gWs/gamma, rho1 / gamma);
        Fzp = funVal_eval  (Wzp);

        delta_Wzp = Wzp - Ws;
        r_sum = norm(delta_Wzp, 'fro')^2;
        %         Fzp_gamma = Fs + trace(delta_Wzp' * gWs)...
        %             + gamma/2 * norm(delta_Wzp, 'fro')^2;
        Fzp_gamma = Fs + sum(sum(delta_Wzp.* gWs))...
            + gamma/2 * norm(delta_Wzp, 'fro')^2;

        if (r_sum <=1e-20)
            bFlag=1; % this shows that, the gradient step makes little improvement
            break;
        end

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Wz_old = Wz;
    Wz = Wzp;

    funcVal = cat(1, funcVal, Fzp + nonsmooth_eval(Wz, rho1));

    if (bFlag)
        % fprintf('\n The program terminates as the gradient step changes the solution very small.');
        break;
    end

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

W = Wzp;

% private functions

    function [X] = FGLasso_projection (D, lambda )
        % l2.1 norm projection.
        X = repmat(max(0, 1 - lambda./sqrt(sum(D.^2,2))),1,size(D,2)).*D;
    end

% smooth part gradient.
    function [grad_W] = gradVal_eval(W)
        if opts.pFlag
            grad_W = zeros(zeros(W));
            parfor i = 1:task_num
                grad_W (i, :) = X{i}*(X{i}' * W(:,i)-Y{i});
            end
        else
            grad_W = [];
            for i = 1:task_num
                grad_W = cat(2, grad_W, X{i}*(X{i}' * W(:,i)-Y{i}) );
            end
        end
        grad_W = grad_W+ rho_L2 * 2 * W;
    end

% smooth part function value.
    function [funcVal] = funVal_eval (W)
        funcVal = 0;
        if opts.pFlag
            parfor i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        else
            for i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        end
        funcVal = funcVal + rho_L2 * norm(W,'fro')^2;
    end

    function [non_smooth_value] = nonsmooth_eval(W, rho_1)
        non_smooth_value = 0;
        if opts.pFlag
            parfor i = 1 : size(W, 1)
                w = W(i, :);
                non_smooth_value = non_smooth_value ...
                    + rho_1 * norm(w, 2);
            end
        else
            for i = 1 : size(W, 1)
                w = W(i, :);
                non_smooth_value = non_smooth_value ...
                    + rho_1 * norm(w, 2);
            end
        end
    end
end


function [node,elem] = interfaceUniformrefine(node,elem,interfaceEdge,phi)

%% Construct data structure
totalEdge = uint32(sort([elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])],2));
[edge, ~, j] = unique(totalEdge,'rows');
N = size(node,1); NT = size(elem,1); NE = size(edge,1);
elem2edge = uint32(reshape(j,NT,3));

isInterfaceNode = false(N,1);
isInterfaceNode(interfaceEdge(:)) = true;

%% Add new nodes: middle points of all edges
tmpNode = (node(edge(:,1),:)+node(edge(:,2),:))/2;
isNewInterfaceNode = isInterfaceNode(edge(:,1)) & isInterfaceNode(edge(:,2));
a = node(edge(isNewInterfaceNode,1),:) - tmpNode(isNewInterfaceNode,:);
b = node(edge(isNewInterfaceNode,2),:) - tmpNode(isNewInterfaceNode,:);
a = tmpNode(isNewInterfaceNode,:) + [-a(:,2), a(:,1)];
b = tmpNode(isNewInterfaceNode,:) + [-b(:,2), b(:,1)];
tmpNode(isNewInterfaceNode,:) = findintersectbisect(phi,a,b);
node(N+1:N+NE,:) = tmpNode;

edge2newNode = uint32((N+1:N+NE)');

%% Refine each triangle into four triangles as follows
%     3
%    / \
%   5 - 4
%  / \ / \
% 1 - 6 - 2
t = 1:NT;
p(t,1:3) = elem(t,1:3);
p(t,4:6) = edge2newNode(elem2edge(t,1:3));
elem(t,:) = [p(t,1), p(t,6), p(t,5)];
elem(NT+1:2*NT,:) = [p(t,6), p(t,2), p(t,4)];
elem(2*NT+1:3*NT,:) = [p(t,5), p(t,4), p(t,3)];
elem(3*NT+1:4*NT,:) = [p(t,4), p(t,5), p(t,6)];


function [L1,L1_l,L1_x1] = reanchorPlucker(L0,x1)

% REANCHORPLUCKER  Plucker to anchored Plucker line conversion
%   REANCHORPLUCKER(L,X) reanchors the anchored Plucker line L to the point X.
%
%   [L1,L1_l,L1_x] = REANCHORPLUCKER(L,X) returns the Jacobians.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

x0 = L0(1:3);
n0 = L0(4:6);
v0 = L0(7:9);

L1 = [x1; n0 + cross((x0-x1),v0); v0];

if nargout > 1

    Z33 = zeros(3);
    I33 = eye(3);

    L1_l = [...
        Z33      Z33    Z33
        -hat(v0) I33 hat(x0-x1)
        Z33      Z33    I33];

    L1_x1 = [...
        I33
        hat(v0)
        Z33];

end

return

%%
syms t1 t2 t3 x1 x2 x3 a b c d n1 n2 n3 v1 v2 v3 y1 y2 y3 real

L0 = [x1;x2;x3;n1;n2;n3;v1;v2;v3];
y = [y1;y2;y3];

[L1,L1_l,L1_y] = reanchorPlucker(L0,y);

simplify(L1_l - jacobian(L1,L0))
simplify(L1_y - jacobian(L1,y))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE