function [s, err_mse, iter_time]=greed_omp_pinv(x,A,m,varargin)
% greed_omp_pinv: Orthogonal Matching Pursuit algorithm based on matlab
% pinv solution. Use not recommended, provided for reference only!
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Usage
% [s, err_mse, iter_time]=greed_omp_pinv(x,P,m,'option_name','option_value')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input
%   Mandatory:
%               x   Observation vector to be decomposed
%               P   Either:
%                       1) An nxm matrix (n must be dimension of x)
%                       2) A function handle (type "help function_format"
%                          for more information)
%                          Also requires specification of P_trans option.
%                       3) An object handle (type "help object_format" for
%                          more information)
%               m   length of s
%
%   Possible additional options:
%   (specify as many as you want using 'option_name','option_value' pairs)
%   See below for explanation of options:
%__________________________________________________________________________
%   option_name    |     available option_values                | default
%--------------------------------------------------------------------------
%   stopCrit       | M, corr, mse, mse_change                   | M
%   stopTol        | number (see below)                         | n/4
%   P_trans        | function_handle (see below)                |
%   maxIter        | positive integer (see below)               | n
%   verbose        | true, false                                | false
%   start_val      | vector of length m                         | zeros
%
%   Available stopping criteria :
%               M           -   Extracts exactly M = stopTol elements.
%               corr        -   Stops when maximum correlation between
%                               residual and atoms is below stopTol value.
%               mse         -   Stops when mean squared error of residual
%                               is below stopTol value.
%               mse_change  -   Stops when the change in the mean squared
%                               error falls below stopTol value.
%
%   stopTol: Value for stopping criterion.
%
%   P_trans: If P is a function handle, then P_trans has to be specified and
%            must be a function handle.
%
%   maxIter: Maximum of allowed iterations.
%
%   verbose: Logical value to allow algorithm progress to be displayed.
%
%   start_val: Allows algorithms to start from partial solution.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Outputs
%    s              Solution vector
%    err_mse        Vector containing mse of approximation error for each
%                   iteration
%    iter_time      Vector containing times for each iteration
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Description
%   greed_omp_pinv performs a greedy signal decomposition.
%   In each iteration a new element is selected depending on the inner
%   product between the current residual and columns in P.
%   The non-zero elements of s are approximated by orthogonally projecting
%   x onto the selected elements in each iteration.
%   This implementation uses matlab's pinv command to calculate the required
%   projection. This is very slow and only works for small matrices. It is
%   therefore only provided for reference.
%
% See Also
%   greed_qr, greed_omp_chol, greed_omp_cg, greed_omp_cgp,
%   greed_omp_linsolve, greed_gp, greed_nomp
%
% Copyright (c) 2007 Thomas Blumensath
%
% The University of Edinburgh
% Email: thomas.blumensath@ed.ac.uk
% Comments and bug reports welcome
%
% This file is part of sparsity Version 0.1
% Created: April 2007
%
% Part of this toolbox was developed with the support of EPSRC Grant
% D000246/1
%
% Please read COPYRIGHT.m for terms and conditions.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                    Default values and initialisation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[n1 n2]=size(x);
if n2 == 1
    n=n1;
elseif n1 == 1
    x=x';
    n=n2;
else
   display('x must be a vector.');
   return
end

sigsize     = x'*x/n;
initial_given=0;
err_mse     = [];
iter_time   = [];
STOPCRIT    = 'M';
STOPTOL     = ceil(n/4);
MAXITER     = n;
verbose     = false;
s_initial   = zeros(m,1);
vectnfact   = ones(m,1);

if verbose
   display('Initialising...')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                           Output variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

switch nargout
    case 3
        comp_err=true;
        comp_time=true;
    case 2
        comp_err=true;
        comp_time=false;
    case 1
        comp_err=false;
        comp_time=false;
    case 0
        error('Please assign output variable.')
    otherwise
        error('Too many output arguments specified')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       Look through options
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Put option into nice format
Options={};
OS=nargin-3;
c=1;
for i=1:OS
    if isa(varargin{i},'cell')
        CellSize=length(varargin{i});
        ThisCell=varargin{i};
        for j=1:CellSize
            Options{c}=ThisCell{j};
            c=c+1;
        end
    else
        Options{c}=varargin{i};
        c=c+1;
    end
end
OS=length(Options);
if rem(OS,2)
   error('Something is wrong with argument name and argument value pairs.')
end

for i=1:2:OS
   switch Options{i}
        case {'stopCrit'}
            if (strmatch(Options{i+1},{'M'; 'corr'; 'mse'; 'mse_change'},'exact'));
                STOPCRIT    = Options{i+1};
            else error('stopCrit must be char string [M, corr, mse, mse_change]. Exiting.'); end
        case {'stopTol'}
            if isa(Options{i+1},'numeric') ; STOPTOL     = Options{i+1};
            else error('stopTol must be number. Exiting.'); end
        case {'P_trans'}
            if isa(Options{i+1},'function_handle'); Pt = Options{i+1};
            else error('P_trans must be function _handle. Exiting.'); end
        case {'maxIter'}
            if isa(Options{i+1},'numeric'); MAXITER     = Options{i+1};
            else error('maxIter must be a number. Exiting.'); end
        case {'verbose'}
            if isa(Options{i+1},'logical'); verbose     = Options{i+1};
            else error('verbose must be a logical. Exiting.'); end
        case {'vecNormFac'}
            if isa(Options{i+1},'numeric')& length(Options{i+1}) == m , vectnfact = Options{i+1};
            else error('verbose must be a logical. Exiting.'); end
        case {'start_val'}
            if isa(Options{i+1},'numeric') & length(Options{i+1}) == m ;
                s_initial     = Options{i+1};
                initial_given=1;
            else error('start_val must be a vector of length m. Exiting.'); end
        otherwise
            error('Unrecognised option. Exiting.')
   end
end

if strcmp(STOPCRIT,'M')
    maxM=STOPTOL;
else
    maxM=MAXITER;
end

if nargout >=2
    err_mse = zeros(maxM,1);
end
if nargout ==3
    iter_time = zeros(maxM,1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        Make P and Pt functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if          isa(A,'float')      P =@(z) A*z;  Pt =@(z) A'*z;
elseif      isobject(A)         P =@(z) A*z;  Pt =@(z) A'*z;
elseif      isa(A,'function_handle')
    try
        if          isa(Pt,'function_handle'); P=A;
        else        error('If P is a function handle, Pt also needs to be a function handle. Exiting.'); end
    catch error('If P is a function handle, Pt needs to be specified. Exiting.'); end
else        error('P is of unsupported type. Use matrix, function_handle or object. Exiting.'); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        Do we start from zero or not?
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if initial_given ==1;
    IN          = find(s_initial);
    s=zeros(m,1);
    if isa(A,'function_handle') || isobject(A)
         Pmat=zeros(n,length(IN));
         for i=1:length(IN)
             mask=zeros(m,1);
             mask(IN(i))=1;
             Pmat(:,i)=P(mask);
         end
         s(IN)=pinv(Pmat)*x;
     else
         s(IN)=pinv(A(:,IN))*x;
     end
    Residual    = x-P(s);
    oldERR      = Residual'*Residual/n;

else
    IN          = [];
    Residual    = x;
    s           = s_initial;
    sigsize     = x'*x/n;
    oldERR      = sigsize;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                 Random Check to see if dictionary is normalised
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%         mask=zeros(m,1);
%         mask(ceil(rand*m))=1;
%         nP=norm(P(mask));
%         if abs(1-nP)>1e-3;
%             display('Dictionary appears not to have unit norm columns.')
%         end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        Main algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if verbose
   display('Main iterations...')
end
tic
t=0;
DR=Pt(Residual).*vectnfact;
done = 0;
iter=1;
while ~done
     DR(IN)=0;
     [v I]=max(abs(DR));
     IN=[IN I];
     if isa(A,'function_handle') || isobject(A)
         Pmat=zeros(n,length(IN));
         for i=1:length(IN)
             mask=zeros(m,1);
             mask(IN(i))=1;
             Pmat(:,i)=P(mask);
         end
         s(IN)=pinv(Pmat)*x;
     else
         s(IN)=pinv(A(:,IN))*x;
     end
         Residual=x-P(s);
         DR=Pt(Residual).*vectnfact;

    ERR=Residual'*Residual/n;
     if comp_err
         err_mse(iter)=ERR;
     end

     if comp_time
         iter_time(iter)=toc;
     end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        Are we done yet?
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

     if strcmp(STOPCRIT,'M')
         if iter >= STOPTOL
             done =1;
         elseif verbose && toc-t>10
            display(sprintf('Iteration %i. --- %i iterations to go',iter ,STOPTOL-iter))
            t=toc;
         end
    elseif strcmp(STOPCRIT,'mse')
         if comp_err
            if err_mse(iter)<STOPTOL;
                done = 1;
            elseif verbose && toc-t>10
                display(sprintf('Iteration %i. --- %i mse',iter ,err_mse(iter)))
                t=toc;
            end
         else
             if ERR<STOPTOL;
                done = 1;
             elseif verbose && toc-t>10
                display(sprintf('Iteration %i. --- %i mse',iter ,ERR))
                t=toc;
             end
         end
     elseif strcmp(STOPCRIT,'mse_change') && iter >=2
         if comp_err && iter >=2
              if ((err_mse(iter-1)-err_mse(iter))/sigsize <STOPTOL);
                done = 1;
             elseif verbose && toc-t>10
                display(sprintf('Iteration %i. --- %i mse change',iter ,(err_mse(iter-1)-err_mse(iter))/sigsize ))
                t=toc;
             end
         else
             if ((oldERR - ERR)/sigsize < STOPTOL);
                done = 1;
             elseif verbose && toc-t>10
                display(sprintf('Iteration %i. --- %i mse change',iter ,(oldERR - ERR)/sigsize))
                t=toc;
             end
         end
     elseif strcmp(STOPCRIT,'corr')
          if max(abs(DR)) < STOPTOL;
             done = 1;
          elseif verbose && toc-t>10
                display(sprintf('Iteration %i. --- %i corr',iter ,max(abs(DR))))
                t=toc;
          end
     end

    % Also stop if residual gets too small or maxIter reached
     if comp_err
         if err_mse(iter)<1e-16
             display('Stopping. Exact signal representation found!')
             done=1;
         end
     else

         if iter>1
             if ERR<1e-16
                 display('Stopping. Exact signal representation found!')
                 done=1;
             end
         end
     end

     if iter >= MAXITER
         display('Stopping. Maximum number of iterations reached!')
         done = 1;
     end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                    If not done, take another round
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

     if ~done
        iter=iter+1;
        oldERR=ERR;
     end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                  Only return as many elements as iterations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargout >=2
    err_mse = err_mse(1:iter);
end
if nargout ==3
    iter_time = iter_time(1:iter);
end

if verbose
   display('Done')
end

% Change history
%
% 8 of Februray: Algo does no longer stop if dictionary is not normaliesd.


function data=nDexample(cx,N,n,seedi)
rand('seed',seedi);
mu = rand(cx,n)*10;
x=[];
C=[];
for i=1:cx
    r=[];
    dum=rand(n,n)-0.5+diag(rand(n,1))+diag(ones(n,1))/3;
    R=dum*dum';
 %   R=eye(n)/3;
    for j=1:N
         r(j,:) = randn(1,size(R,1)) * R + mu(i,:);
    end
    C=[C;ones(N,1)*i];
    x=[x;r];
end
%plot(x(:,1),x(:,2),'.');
data=[x];


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;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Program to reconstruct Phantom-Head Model using Algebraic    %%%%%%%%%
%%%% Reconstruction Method.                                               %
%%%%     This code is implemented By :                                   %%                                                    %%
%%%%     AUTHOR: SAYEDALI A SHAIKH,                                     %%%
%%%%     M.Tech.(CSE)                                                  %%%%
%%%%     BVBCET,HUBLI-580031, E-mail Id:sayedalishaikh@gmail.com      %%%%%
%%%%     Date: 28/08/2009                                       %%%%%%%%%%%
% % %   Guided by: Mr Shrinivas D Desai                         %%%%%%%%%%%
%%%     Associate Prof, Dept of ISE, BVBCET Hubli - 580031      %%%%%%%%%%%
%%%%    sd_desai@bvb.edu    9845275066                          %%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Call Function Inputs To Enter Following Parameters
[size1,rotation,incr,s]=inputart();
%Orginal Image
I=phantom(size1);
[r1,c1]=size(I);
figure,imshow(I);
title('Original Phantom-Head Model Image ');

%Guessed Image
G=zeros(size(I));
[r2,c2]=size(G);
G2=zeros(size(G));

% Call Function To Pad The Original Image
[r3,c3,padIMG]=padO(I);
%figure,imshow(padIMG);
T1=padIMG;

% Call Function To Pad The Guessed Image
[r4,c4,padGIMG]=padG(G);
%figure,imshow(padGIMG);
T2=padGIMG;

% Calculate The Correction Factor
%Call Function To Choose The Denomenator Value w.r.t Angle and Increment

[z]=chooseart(rotation,incr);
z1=0;
z2=incr;
T3=T2;
THETA=0:incr:rotation;
s1=length(THETA);
for a=1:s1,
    org1=imrotate(T1,z1,'bilinear','crop');
% Call Function To Calculate The Row Sum and Column Sum for Original Image
    [rsumO,csumO]=calc_sum(org1,r3,z);
% Call Function To Calculate The Row Sum and Column Sum for Guessed Image
    [rsumG,csumG,padGIMG]=corr_factor(T2,r4,rsumO,csumO,r3,c3,s,z);
    %figure,imshow(padGIMG);
    G2=padGIMG;
    %figure,imshow(G2);
    T3=T3+G2;
    figure,imshow(T3);
    T3=imrotate(T3,z2,'bilinear','crop');
    z1=z1+incr;
end


% 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


function [ffun,flag] = limgrad(edge,elen,ffun,dfdx,imax)
%LIMGRAD impose "gradient-limits" on a function defined over
%an undirected graph.
%   [FNEW] = LIMGRAD(EDGE,ELEN,FFUN,DFDX,ITER) computes a
%   "gradient-limited" function FNEW on the undirected graph
%   {EDGE,ELEN}, where EDGE is an NE-by-2 array of edge ind-
%   ices, and ELEN is an NE-by-1 array of edge lengths.
%   Gradients are limited over the graph edges, such that
%
%       ABS(FNEW(N2)-FNEW(N1)) <= ELEN(II) * DFDX,
%
%   where N1=EDGE(II,1) and N2=EDGE(II,2) are the two nodes
%   in the II-TH edge. An iterative algorithm is used, swee-
%   ping over an "active-set" of graph edges until converge-
%   nce is achieved. A maximum of IMAX iterations are done.
%
%   [FNEW,FLAG] = LIMGRAD(...) also returns a boolean FLAG,
%   with FLAG=TRUE denoting convergence.
%
%   See also LIMHFN2

%   Darren Engwirda : 2017 --
%   Email           : engwirda@mit.edu
%   Last updated    : 18/04/2017

%---------------------------------------------- basic checks
    if ( ~isnumeric(edge) || ...
         ~isnumeric(elen) || ...
         ~isnumeric(ffun) || ...
         ~isnumeric(dfdx) || ...
         ~isnumeric(imax) )
        error('limgrad:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end
%---------------------------------------------- basic checks
    if (ndims(edge) ~= +2 || ...
        ndims(elen)  > +2 || ...
        ndims(ffun)  > +2 || ...
        numel(dfdx) ~= +1 || ...
        numel(imax) ~= +1 )
        error('limgrad:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(edge,2) < +2 || ...
        size(elen,2)~= +1 || ...
        size(ffun,2)~= +1 || ...
        size(edge,1)~= size(elen,1) )
        error('limgrad:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    nnod = size(ffun,1) ;

%---------------------------------------------- basic checks
    if (dfdx < +0. || imax < +0)
        error('limgrad:invalidInputArgument', ...
            'Invalid input parameter.');
    end
    if (min(min(edge(:,1:2))) < +1 || ...
            max(max(edge(:,1:2))) > nnod )
        error('limgrad:invalidInputArgument', ...
            'Invalid EDGE input array.') ;
    end

%-- IVEC(NPTR(II,1):NPTR(II,2)) are edges adj. to II-TH node
    nvec = [edge(:,1); edge(:,2)];
    ivec = [(1:size(edge,1))'; ...
            (1:size(edge,1))'] ;

   [nvec,pidx] = sort (nvec) ;
    ivec       = ivec (pidx) ;

    mark = false(nnod,1) ;
    mark(edge(:,1)) = true ;
    mark(edge(:,2)) = true ;

    idxx = find(diff(nvec) > +0) ;

    nptr = zeros(nnod,2) ;
    nptr(:,2) = -1 ;
    nptr(mark,1) = [+1; idxx+1];
    nptr(mark,2) = [idxx; nnod];

%----------------------------- ASET=ITER if node is "active"
    aset = zeros(size(ffun,1),1) ;

%----------------------------- exhaustive 'til all satisfied
    ftol = min(ffun) * sqrt(eps) ;

    for iter = +1 : imax

    %------------------------- find "active" nodes this pass
        aidx = find(aset == iter - 1) ;

        if (isempty(aidx)), break; end

    %------------------------- reorder => better convergence
       [~,idxx] = sort(ffun(aidx)) ;

        aidx = aidx(idxx);

    %------------------------- visit adj. edges and set DFDX
        for ipos = 1 : length(aidx)
            npos = aidx(ipos) ;
            for jpos = nptr(npos,1) ...
                     : nptr(npos,2)

                epos = ivec(jpos,1) ;

                nod1 = edge(epos,1) ;
                nod2 = edge(epos,2) ;

            %----------------- calc. limits about min.-value
                if (ffun(nod1) > ffun(nod2))

                fun1 = ffun(nod2) ...
                     + elen(epos) * dfdx ;

                if (ffun(nod1) > fun1+ftol)
                    ffun(nod1) = fun1;
                    aset(nod1) = iter;
                end

                else

                fun2 = ffun(nod1) ...
                     + elen(epos) * dfdx ;

                if (ffun(nod2) > fun2+ftol)
                    ffun(nod2) = fun2;
                    aset(nod2) = iter;
                end

                end

            end
        end

    end

    flag = (iter < imax) ;

end


function imResult = blendMode_LinearDodge(A, B, offsetW, offsetH)
%% Linear Dodge blending mode: simply sums the values in the two layers.
%   Blending with white gives white. Blending with black does not change
%   the image.
%
% Input:
%       A       -       Base Image
%       B       -       Top Image
%   offsetW     -   move picture B horizontally in respect to the top-left
%                   corner of picture A. Default value = 1.
%   offsetH     -   move picture B vertically in respect to the top-left
%                   corner of picture A. Default value = 1.
%
% Output:
%       imResult    -   Result of the blending, having the same size of the
%                       Base Image A.
%

%% Check Input
a = size(A);
b = size(B);
blendMode_checkInput(nargin, a, b, func2str(@blendMode_LinearDodge));

if nargin < 3
    offsetW = 1;
    offsetH = 1;
end

if nargin < 4
    offsetH = 1;
end

%% Implementation
imResult = A;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    [A, B] = blendMode_ResizeImages(A, B, a, b, offsetW, offsetH);
end

C = blendMode_Add(A, B, offsetW, offsetH);

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    imResult = blendMode_CreateResult(imResult, C, offsetW, offsetH);
else
    imResult = C;
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 C = DiagCorr(a,b)
% e.g. dim of A: nFrames*nCells, like in Matlab 'corr' function
An=bsxfun(@minus,a,mean(a,1));
Bn=bsxfun(@minus,b,mean(b,1));
An=bsxfun(@times,An,1./sqrt(sum(An.^2,1)));
Bn=bsxfun(@times,Bn,1./sqrt(sum(Bn.^2,1)));
C=sum(An.*Bn,1);
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


function dataout=modeWPE_NLMS_BSS(nummics, numrefs, datain)
%
% Perform wpe, nlms, and bss.
% nummics:              no. of mic channels
% numrefs:              no. of reference channels
% datain:               input data
% dataout:              output data
%

fs=16000;

%% perform wpe
addpath('wpe_v1.33');

mic=datain(:, 1:2);
ref=datain(:, 3);

cfgs='wpe_v1.33/settings/local.m';
early=wpe(mic, cfgs);

%% perform nlms
addpath('Speex-AEC-matlab-master');

%    Usage:
%
%       speex_mdf_out = speex_mdf(Fs, u, d, filter_length, frame_size, dbg_var_name);
%
%       Fs                  sample rate
%       u                   speaker signal, column vector in range [-1; 1]
%       d                   microphone signal, column vector in range [-1; 1]
%       filter_length       typically 250ms, i.e. 4096 @ 16k FS
%                           must be a power of 2
%       frame_size          typically 8ms, i.e. 128 @ 16k Fs
%                           must be a power of 2
%       dbg_var_name        internal state variable name to trace.
%                           Default: 'st.leak_estimate'.
%
%    Jonathan Rouach <jonr@waves.com>
%
filter_length=512;
frame_size=256;
nearend1=speex_mdf(fs, ref, early(:, 1), filter_length, frame_size);
nearend2=speex_mdf(fs, ref, early(:, 2), filter_length, frame_size);
nearend=[nearend1.e, nearend2.e];

%% perform stft
addpath('stft2');
% fft size
fftsize=512;
stftshift=fftsize/2;

M=nummics;
N=M;

Xtf=cell(M, 1);
for m=1:M
    Xtf{m}=stft(nearend(:, m), stftshift, fftsize, false);
end
[K, T]=size(Xtf{1});

Ytf=cell(M, 1);
for m=1:M
    Ytf{m}=zeros(K, T);
end

%% params
% forgetting factor for bss
BF_FORGET=0.999;
% the shape parameter of the source prior
GAMMA=0.2;
%
% used to keep stable
%
VAR_BIAS=0.01;
STABLE_EPS=1e-3;
BF_DIAGLOAD=1e-6;

%% space for bss
% the weighted correlation matrices
C1=cell(K, 1);
C2=cell(K, 1);
for k=1:K
    C1{k}=STABLE_EPS*eye(M, M);
    C2{k}=STABLE_EPS*eye(M, M);
end

% demixing matrices
Demix=cell(K, 1);
for k=1:K
    Demix{k}=eye(N, M);
end

%% perform bss
for tau=1:T
    %% perform bss
    Bssout=zeros(K, M);

    %
    % calculate nonlinearity
    %
    phi1=0;
    phi2=0;

    for k=1:K
        x=zeros(M, 1);
        for m=1:M
            x(m)=Xtf{m}(k, tau);
        end

        y=Demix{k}*x;
        % output data
        Bssout(k, :)=y.';

        phi1=phi1+abs(y(1))^2;
        phi2=phi2+abs(y(2))^2;
    end

    phi1=(1-BF_FORGET)*(phi1+VAR_BIAS)^((GAMMA-2)/2);
    phi2=(1-BF_FORGET)*(phi2+VAR_BIAS)^((GAMMA-2)/2);

    % update the demixing matrices
    for k=1:K
        %
        % accumulate the weighted correlation
        %
        x=zeros(M, 1);
        for m=1:M
            x(m)=Xtf{m}(k, tau);
        end

        C1{k}=BF_FORGET*C1{k}+phi1*(x*x');
        C2{k}=BF_FORGET*C2{k}+phi2*(x*x');

        %
        % solve gev problem
        %
        [Ev, Ed]=eig(C2{k}+BF_DIAGLOAD*eye(M, M), C1{k}+BF_DIAGLOAD*eye(M, M));
        if Ed(1, 1)>=Ed(2, 2)
            e1=Ev(:, 1);
            e2=Ev(:, 2);
        else
            e1=Ev(:, 2);
            e2=Ev(:, 1);
        end

        D=[e1'; e2'];

        %
        % solve the scaling ambiguity
        %
        A=inv(D);

        if abs(A(1, 1))>=abs(A(2, 1))
            a1=A(1, 1);
        else
            a1=A(2, 1);
        end

        if abs(A(2, 2))>=abs(A(1, 2))
            a2=A(2, 2);
        else
            a2=A(1, 2);
        end

        D=diag([a1; a2])*D;
        Demix{k}=D;
    end

    for m=1:M
        Ytf{m}(:, tau)=Bssout(:, m);
    end
end

%% perform istft and output signal
dataout=zeros(dataLength(T, stftshift, fftsize ), N);
for n=1:N
    dataout(:, n)=istft(Ytf{n}, stftshift, false);
end

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 that optimizes Pose given an initial solution of the rotation only
%
% IMPORTANT: THIS FUNCTION ONLY DEALS WITH THE ORTHOGRAPHIC CASE RIGHT NOW
%
% This function optimizes the pose estimation of one camera with respect to
% another given only the 2 corresponding views.  The data is supposed to be
% centered and only the rotation has to be estimated. I it is here
% performed using simple gradient descent.
%
% The following code explains how the code to compute the
% error/gradient/hessian was created using matlab symbolic toolbox.
%
% Vincent's Structure From Motion Toolbox      Version 3.0
% Copyright (C) 2008-2011 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

%%%%%  Infimum in TSFM   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
syms a b c cc d dd real

R = [ a^2+b^2-c^2-d^2 2*b*c-2*a*d 2*a*c+2*b*d; ...
 2*a*d+2*b*c a^2-b^2+c^2-d^2 2*c*d-2*a*b; ...
 2*b*d-2*a*c 2*a*b+2*c*d a^2-b^2-c^2+d^2 ]/(a^2+b^2+c^2+d^2);
R = R(1:2,:);
dRa = simple(diff(R,a));
dRb = simple(diff(R,b));
dRc = simple(diff(R,c));
dRd = simple(diff(R,d));
RtR = R'*R;
RtdRa = R'*simple(diff(R,a));
RtdRb = R'*simple(diff(R,b));
RtdRc = R'*simple(diff(R,c));
RtdRd = R'*simple(diff(R,d));

%%%%%  Rest of the code    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
l = [ R(:); RtR(:) ]
l = [ R(:); dRa(:); dRb(:); dRc(:); dRd(:); RtR(:); RtdRa(:); RtdRb(:); RtdRc(:); RtdRd(:) ];
l = simple( l );

maple restart;
com = 'res := [ ';
k = 1;
for j = 1 : 6
  com = [ com 'r[' num2str(j) ']=' char(l(k)) ', '];
  k = k + 1;
end
kk=1
for i = 1 : 4
  for j = 1 : 6
	com = [ com 'dR[' num2str(kk) ']=' char(l(k)) ', '];
	k = k + 1;
	kk = kk + 1;
  end
end
for j = 1 : 9
  com = [ com 'rtR[' num2str(j) ']=' char(l(k)) ', '];
  k = k + 1;
end
kk = 1;
for i = 1 : 4
  for j = 1 : 9
    com = [ com 'rtDR[' num2str(kk) ']=' char(l(k)) ', '];
	k = k + 1;
	kk = kk + 1;
	end
end
com = [ com(1:end-2) '];' ];

%%%%%  Below is the code to generate C-code    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Mupad try, but it sucks ...
%  reset(symengine);
%  out = evalin(symengine, [ 'opts := generate::optimize([R=expr(' char(Rp') ') ]);' ] );
%
%  out = evalin(symengine, [ 'opts := generate::optimize([R=expr(' char(Rp') '), dR=expr(' char(l(:,3:10)) '), ddR=expr(' char(l(:,11:end)) ')]);' ] );
%  evalin(symengine, [ ':=rhs(opts[-1]);' ] );
%
%
%
%  out = [ char(evalin(symengine, [ 'generate::C(opts[1..-2]);' ] )) ...
%    char( evalin(symengine, [ 'generate::C(rhs(opts[-1]))' ] )) ];
%  out = strrep( out, '"', '' );
%  out = strrep( out, [ var '[0][' ], [ var '[' ] );

%%% now, save your crazy array into the res variable
maple( com );

%  %%% cost of the assigments with no optimization
%  maple('codegen[cost](res)')
%  % maple( 'opt1 := [codegen[optimize](l)]' );
%  %%% cost of the assigment with the normal optimization
%  in = [ 'l:=' char( l ) ]; maple( in ); maple('codegen[cost](codegen[optimize](l))')

%%% code for better optimization and corresponding cost
maple( 'opt2 := [codegen[optimize](res,tryhard)]' );
maple('codegen[cost](opt2)')

%%% Convert to C code
out = maple( 'codegen[C](opt2)' );

%%% clean the C code and convert it to Matlab code (replace [] by ()
out = strrep( out, ';   ', ';\n' );
out = strrep( out, '~', '' );

tout = ''
existing = cell(1,0);
tmp = regexp( out,'(t\d*)', 'tokens' )
for i = 1 : length(tmp)
  ii = tmp(i);
  ii = ii{1}{1};
  doExist = false;
  for j = 1 : size(existing,2)
    if strcmp(existing{j},ii)
	  doExist = true;
	  break;
	end
  end
  if ~doExist
    existing{1,end+1} = ii;
    tout = [ tout, ', ', ii ];
  end
end
out = [ 'double ', tout(2:end), ';\n', out ];
fprintf(out)

while 1
  tok = regexp( out,'\[(\d*)\]\[(\d*)\]', 'tokens' );
  tokStart = regexp( out,'\[(\d*)\]\[(\d*)\]', 'start' );
  tokEnd = regexp( out,'\[(\d*)\]\[(\d*)\]', 'end' );

  if isempty(tok); break; end

  out = [ out( 1:tokStart(1)-1 ) '(' num2str(str2double(tok{1}{1})+1) ...
      ',' num2str(str2double(tok{1}{2})+1) ')' out( tokEnd(1)+1 : end ) ];
end

fprintf( out );


function lA = shortAxis(grains,varargin)
% short axis of a grain
%
% the long axis is the direction of the smallest
% <grain2d.principalComponents.html,principal component> of a grain
%
% Syntax
%   sA = grains.shortAxis
%
% Input
%  grains - @grain2d
%
% Output
%  sA - @vector3d direction of the shortest elongation
%
% See also
% grain2d/principalComponents

omega = principalComponents(grains);

lA = vector3d.byPolar(pi/2,omega+pi/2,'antipodal');


% Load original BFM
load('01_MorphableModel.mat');

% Load modified FW expression model
load('3DDFA_Release/Matlab/Model_Expression.mat');

% Load FW to BFM mapping
load('util/map_tddfa_to_basel.mat');
% Fix zero-based indexing
map_tddfa_to_basel=map_tddfa_to_basel+1;

model.shapePC = zeros(3*53490,10);
for i=1:5
model.shapePC(:,i) = shapePC(:,i).*shapeEV(i);
end

%% Extrapolate expressions to mouth interior via Laplace-Beltrami
vertices = double(reshape(shapeMU,3,53490)');
[L,~]=LaplaceBeltrami(vertices',tl');
% Selection matrix for BFM vertices to FW
S = sparse(1:length(map_tddfa_to_basel),double(map_tddfa_to_basel),ones(length(map_tddfa_to_basel),1),length(map_tddfa_to_basel),53490);
for i=1:5
    expression = ([L; S]\[L*vertices; S*vertices+reshape(w_exp(:,i),3,53215)'])-vertices;
    expression = expression';
    model.shapePC(:,i+5) = expression(:);
end
%% Load UV coordinates and resample model
load('util/BFM_UV.mat');
[ newmodel ] = resampleModel( shapeMU,model.shapePC,[],tl,UV,112 );
newmodel.shapeMU=newmodel.shapeMU./1000;
newmodel.shapePC=newmodel.shapePC./1000;
save model.mat -struct newmodel
%% Generate random mesh
clear FV
FV.faces = newmodel.faces;
alpha = randn(10,1);
FV.vertices = reshape(newmodel.shapeMU+newmodel.shapePC*alpha,3,112^2)';
figure; patch(FV, 'FaceColor', [1 1 1], 'EdgeColor', 'none', 'FaceLighting', 'phong'); axis equal; light; axis tight


  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


%% Calculate geodesic error curves
clear all; close all; clc

addpath(genpath('./'))
addpath(genpath('./../Tools/'))

mesh_0 = load('./faust_synthetic/shapes/tr_reg_080'); %Choose the indices of the test pair
mesh_1 = load('./faust_synthetic/shapes/tr_reg_087'); %Choose the indices of the test pair

X = load('./Results/test_faust_synthetic/080_087.mat'); %Choose the indices of the test pair
[~, matches] = max(squeeze(X.softCorr),[],1);

D_model = load('.\faust_synthetic\distance_matrix\tr_reg_087.mat'); %Choose the indices of the test pair
D_model = D_model.D;

gt_matches = 1:6890;
errs = calc_geo_err(matches, gt_matches, D_model);
curve = calc_err_curve(errs, 0:0.001:1.0)/100;
plot(0:0.001:1.0, curve); set(gca, 'xlim', [0 0.1]); set(gca, 'ylim', [0 1])

xlabel('Geodeisc error')
ylabel('Correspondence Accuracy %')


function [w]=ADMM_solve_w(params,use_sz,model_w,h_f)

    w = gpuArray(params.w_init*single(ones(use_sz)));
    q = w;
    m = w;

    mu    = 1;
    betha = 10;
    mumax = 10000;
    i = 1;
    params.admm_iterations=2;
    T = prod(use_sz);
    h=T*real(ifft2(h_f));
%     hw=bsxfun(@times,h,model_w);
    hw=h;
    Hh=sum(hw.^2,3);
%     Hh=sum(real(bsxfun(@times,h_f(:),conj(h_f(:)))));

    %   ADMM
    while (i <= params.admm_iterations)
        %   solve for w- please refer to the paper for more details
      %  w = (q-m)/(1+(params.admm_lambda1/mu)*Hh);
        w = bsxfun(@rdivide,(q-m),(1+(params.admm_lambda1/mu)*Hh));
        %   solve for q
        q=(params.admm_lambda2*model_w + mu*(w+m))/(params.admm_lambda2 + mu);

        %   update m
        m = m + (w - q);

        %   update mu- betha = 10.
        mu = min(betha * mu, mumax);
        i = i+1;

    end

end


function hmm = hmmrotatepca(hmm,Gamma,XXGXX)

for k = 1:length(hmm.state)

    % Orthogonalize W to the standard PCA subspace
    W = hmm.state(k).W.Mu_W;
    %[W,~] = svd(hmm.state(k).W.Mu_W,'econ');

%     % Enforce a sign convention on the coefficients:
%     % the largest element in each column will have a positive sign.
    [~,maxind] = max(abs(W), [], 1);
    [d1, d2] = size(W);
    colsign = sign(W(maxind + (0:d1:(d2-1)*d1)));
    W = bsxfun(@times, W, colsign);
    hmm.state(k).W.Mu_W = W;

    % recompute covariance of W
    v = hmm.Omega.Gam_rate / hmm.Omega.Gam_shape;
    SW = XXGXX{k} * W / sum(Gamma(:,k));
    M = W'*W+v*eye(size(W,2));
    iS_W = v*eye(size(W,2))+M\W'*SW; S_W = inv(iS_W);
    for n = 1:size(hmm.state(k).W.iS_W,1)
        hmm.state(k).W.iS_W(n,:,:) = iS_W;
        hmm.state(k).W.S_W(n,:,:) = S_W;
    end

end


%   TTeMPS Toolbox.
%   Michael Steinlechner, 2013-2016
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt

function [xR, residuum, gradnorm, cost, times] = RiemannPrecondSteep( L, F, X0, Lh, Ph, opts )
% L is the actual operator
%    needs a apply(L, X) interface but can be anything
% Lh is the operator that represents the (inexact) Euclidean Hessian
%    needs a apply(L, X) interface but can be anything
% Ph is the preconditioner for Lh; should be a TTeMPS_op_laplace operator
%
% When L is Laplace+perturbation, taking Lh and Ph both the Laplacian works
% well.

t_start = tic();
% set default opts
if ~exist( 'opts', 'var');       opts = struct();     end
if ~isfield( opts, 'maxiter');   opts.maxiter = 500;  end
if ~isfield( opts, 'tol');       opts.tol = 1e-16;     end
if ~isfield( opts, 'safe_norm');       opts.safe_norm = false;     end

d = X0.order;
n = X0.size;

[xL, xR, G] = gauge_matrices( X0 );

%xL = orthogonalize(X, d);
%xR = orthogonalize(X, 1);

cost = zeros(opts.maxiter, 1);
residuum = zeros(opts.maxiter, 1);
gradnorm = zeros(opts.maxiter, 1);
times = zeros(opts.maxiter, 1);
normRHS = norm(F);

for i = 1:opts.maxiter

    g = euclid_grad( L, xR, F );

    cost(i) = cost_function_res( xR, g );
    residuum(i) = norm(g, opts.safe_norm) / normRHS;
    times(i) = toc(t_start);

    % test for stopping criterion
    if abs(residuum(i)) < opts.tol
        sprintf( 'Current residual: %g', residuum(i))
        residuum = residuum(1:i);
        cost = cost(1:i);
        times = times(1:i);
        sprintf( 'RiemannLinsolve CONVERGED after %i iterations', i )
        break
    end

    grad = TTeMPS_tangent_orth( xL, xR, g );
    gradnorm(i) = norm( grad );

    sprintf('steepest descent step %i', i)
    %P_grad = solvePrecond( L, P, grad, xL, xR, opts );
    P_grad = solvePrecond_noSaddle( Lh, Ph, grad, xL, xR, opts, G );

    %check_precond_laplace(P, grad, P_grad)
    %eta = -P_grad;

    %line search
    alpha = linesearch_linearized( L, P_grad, g )
    %alpha = linesearch_linearized2( L, P_grad, grad )
    %alpha = -1;
    xR = tangentAdd(  P_grad, alpha, true );

    [xL, G] = left_orth_with_gauge( xR );
    %xL = orthogonalize( X, d );
    %xR = orthogonalize( X, 1 );
end

end

function res = cost_function( L, X, F )
res = 0.5*innerprod( X, apply(L, X) ) - innerprod( X, F );
end
function res = cost_function_res( X, res )
res = 0.5*innerprod( X, res );
end

function res = euclid_grad( L, X, F )
res = apply(L, X) - F;
end

function alpha = linesearch_linearized( L, xi, g )
eta = tangent_to_TTeMPS( xi );
alpha = -innerprod( eta, g );
alpha = alpha / innerprod( eta, apply(L, eta) );
end

function alpha = linesearch_linearized2( L, xi, grad )
alpha = -innerprod( xi, grad );
eta = tangent_to_TTeMPS( xi );
alpha = alpha / innerprod( eta, apply(L, eta) );
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


close all
clear all
path(path,'..\..\..\FUZZCLUST')
%the data
load motorcycle.txt
data.X = motorcycle(:,[1 2]);

[N,n]=size(data.X);

%data normalization
data = clust_normalize(data,'range');
plot(data.X(:,1),data.X(:,2),'.')
hold on
%parameters
param.c=4;
param.vis=1;
param.val=2;
%clustering
result=kmedoid(data,param);
%validation
result = validity(result,data,param);
result.validity


function coords = makeSmoothCoords(c)

coords = [];
ii = 1;coordsInd = 1;
buff = 10;
while ii<size(c,2)
    n = c(2,ii);
    if n>100
        x = c(1,ii+1:ii+n);
        y = c(2,ii+1:ii+n);
        x = [x(end-buff:end) x x(1:buff)]; % buffer makes ends meet
        y = [y(end-buff:end) y y(1:buff)];
        x = smooth(x,25,'loess');
        y = smooth(y,25,'loess');
        x = [x(buff+1:end-buff-1);x(buff+1)];
        y = [y(buff+1:end-buff-1);y(buff+1)];
        coords(coordsInd).x = x;
        coords(coordsInd).y = y;
        coordsInd = coordsInd+1;

    end
    ii = ii+n+1;
end


function infsup(c)
%INFSUP       Display of interval taylor in infsup notation
%
%   infsup(c)
%

% written  05/21/09     S.M. Rump
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/07/12     S.M. Rump  complete redesign
%

  INTLAB_TAYLOR_ORDER = getappdata(0,'INTLAB_TAYLOR_ORDER');

  loose = strcmp(get(0,'FormatSpacing'),'loose');

  name = inputname(1);
  if isempty(name)                    % happens for display(taylorinit(random))
    name = 'ans';
  end

  numvar = size(c.t,1)-1;
  if numvar~=INTLAB_TAYLOR_ORDER
    warning('**** number of dependent variables and partial derivatives do not coincide')
  end

  INTLAB_INTVAL_DISPLAY = getappdata(0,'INTLAB_INTVAL_DISPLAY');
  setappdata(0,'INTLAB_INTVAL_DISPLAY','DisplayInfsup');
  display(c,name)
  setappdata(0,'INTLAB_INTVAL_DISPLAY',INTLAB_INTVAL_DISPLAY);


% 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


% 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 y = sprice_a(a, b, r, n, f, k, t,cp)
% sabr prices using the risk neutral density psabr
% and integrating this density with respect to the payoff

nl = length(k);
y = ones(1,nl);
if (cp == 1)
    % call
    for j = 1:nl
    F = @(x) (x-k(j)) .* psabr(a, b, r, n, f, x, t);
    y(j) = quad(F,k(j),1);
    end
else
    % put
    for j = 1:nl
    F = @(x) (k(j)-x) .* psabr(a, b, r, n, f, x, t);
    y(j) = quad(F,0.0001,k(j));
    end
end

end


function test_symm_clustering()
%
% demonstration file for NMFLibrary.
%
% This file illustrates how to use this library.
% This demonstrates Symm-ANLS algorithm and Symm-Newton algorithm.
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai on Jun. 26, 2019

    clc;
    clear;
    close all;

    %% generate synthetic data of (mxn) matrix
    if 1
        input = importdata('../../data/ORL.mat');
        M = input.data;
        gnd = input.label;
    else
        input = importdata('../../data/COIL20.mat');
        M = (input.TrainSet.X)';
        gnd = (input.TrainSet.y)';
    end
    clear input;

    V = calcu_similarity_matrix(M);
    rank = length(unique(gnd));

    %W_init = 2 * full(sqrt(mean(mean(V)) / rank)) * rand(m, rank);
    %W_init = rand(m, rank);
    %options.x_init.W = W_init;
    %options.x_init.H = (options.x_init.W)';

    lambda = 0.66;

    %% initialize rank to be factorized
    options.verbose = 2;
    options.max_epoch = 100;
    options.calc_symmetry = true;
    options.calc_clustering_acc = true;
    options.clustering_gnd = gnd;
    options.clustering_classnum = rank;
    %options.clustering_eval_num = 10;
    %options.init_alg = 'symm_mean';

    %% perform factroization
    % Symm-ANLS
    options.alpha = lambda;
    [w_symm_anls, infos_symm_anls] = symm_anls(V, rank, options);
    % Symm-Newton
    [w_symm_newton, infos_symm_newton] = symm_newton(V, rank, options);
    % Symm-Hals
    options.lambda = lambda;
    [w_symm_halsacc, infos_symm_halsacc] = symm_halsacc(V, rank, options);

    %% plot
    display_graph('epoch','cost', {'Symm-ANLS', 'Symm-Newton', 'Symm-HALS'}, ...
        {w_symm_anls, w_symm_newton, w_symm_halsacc}, {infos_symm_anls, infos_symm_newton, infos_symm_halsacc});
    display_graph('time','cost', {'Symm-ANLS', 'Symm-Newton', 'Symm-HALS'}, ...
        {w_symm_anls, w_symm_newton, w_symm_halsacc}, {infos_symm_anls, infos_symm_newton, infos_symm_halsacc});

    %% symmetry
    display_graph('epoch','symmetry', {'Symm-ANLS', 'Symm-Newton', 'Symm-HALS'}, ...
        {w_symm_anls, w_symm_newton, w_symm_halsacc}, {infos_symm_anls, infos_symm_newton, infos_symm_halsacc});

    %% clustering
    display_graph('epoch','clustering_acc', {'Symm-ANLS', 'Symm-Newton', 'Symm-HALS'}, ...
        {w_symm_anls, w_symm_newton, w_symm_halsacc}, {infos_symm_anls, infos_symm_newton, infos_symm_halsacc});
    display_graph('epoch','clustering_nmi', {'Symm-ANLS', 'Symm-Newton', 'Symm-HALS'}, ...
        {w_symm_anls, w_symm_newton, w_symm_halsacc}, {infos_symm_anls, infos_symm_newton, infos_symm_halsacc});
    display_graph('epoch','clustering_purity', {'Symm-ANLS', 'Symm-Newton', 'Symm-HALS'}, ...
        {w_symm_anls, w_symm_newton, w_symm_halsacc}, {infos_symm_anls, infos_symm_newton, infos_symm_halsacc});

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


close all
clear all
clc

%Specify start and end points
start_point = [100;40]
end_point = [55;40]

%Specify external boundaries
external_boundaries = [0,0;60,0;60,45;45,45;45,59;75.5,40;106,59;106,45;91,45;91,0;151,0;151,105;50,105;0,60];

%Let the external boundaries form a closed polygon for graphical
%visualization purposes
external_boundaries_draw = external_boundaries;
external_boundaries_draw(size(external_boundaries,1)+1,:) = external_boundaries_draw(1,:);

hold on
axis equal

%Plot the boundaries, starting points and end points
plot(external_boundaries_draw(:,1), external_boundaries_draw(:,2), 'Color', 'black')
plot(start_point(1), start_point(2), 'X', 'Color', 'green')
plot(start_point(1), start_point(2), 'O', 'Color', 'green')
plot(end_point(1), end_point(2), 'X', 'Color', 'red')
plot(end_point(1), end_point(2), 'O', 'Color', 'red')

tic
waypoint_coordinates = pathfinder(start_point, end_point, external_boundaries)
toc

%Plot the chosen path
plot(waypoint_coordinates(:,1), waypoint_coordinates(:,2), 'Color', 'blue')


% Fig. 5.18   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%
clf
 np=1;
dp=[1 .2 6.6*6.6+.1*.1 0 0];
nc=[1 1];
dc=[1 12];
nol=conv(np,nc);
dol=conv(dp,dc);
rlocus(nol,dol)
title(' Fig. 5.18 Root locus for noncollocated system')
axis([-15, 5, -7.5, 7.5])
z=0:.1:.9;
 wn=2:2:14;
 sgrid(z, wn)
 hold off


function imgRec = BoschettiDec(name)
%
%
%       imgRec = BoschettiDec(name)
%
%       Input:
%           -name: the prefix of the compressed HDR images using Boschetti
%           et al. method.
%
%
%     Copyright (C) 2012  Francesco Banterle
%
%     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/>.
%

%Read metadata
info = imfinfo([name, '_bos_RGB.jp2']);
decoded = sscanf(cell2mat(info.Comments), '%g', 3);
nBit = decoded(1);
maxE = decoded(2);
minE = decoded(3);

maxVal = 2^nBit - 1;

%Reading and Decoding Eq
EqDec = double(imread([name, '_bos_E.jp2'])) / maxVal;
EDec = EqDec * (maxE - minE) + minE;
mult = 2.^EDec;

%Decoding RGB
RGBDec = double(imread([name, '_bos_RGB.jp2'])) / maxVal;

%Reconstruction
imgRec = zeros(size(RGBDec));
for i=1:size(imgRec, 3)
    imgRec(:,:,i) = (RGBDec(:,:,i)) .* mult;
end

end