function [IDX,sep] = otsu(I,n)

%OTSU Global image thresholding/segmentation using Otsu's method.
%   IDX = OTSU(I,N) segments the image I into N classes by means of Otsu's
%   N-thresholding method. OTSU returns an array IDX containing the cluster
%   indices (from 1 to N) of each point. Zero values are assigned to
%   non-finite (NaN or Inf) pixels.
%
%   IDX = OTSU(I) uses two classes (N=2, default value).
%
%   [IDX,sep] = OTSU(...) also returns the value (sep) of the separability
%   criterion within the range [0 1]. Zero is obtained only with data
%   having less than N values, whereas one (optimal value) is obtained only
%   with N-valued arrays.
%
%   Notes:
%   -----
%   It should be noticed that the thresholds generally become less credible
%   as the number of classes (N) to be separated increases (see Otsu's
%   paper for more details).
%
%   If I is an RGB image, a Karhunen-Loeve transform is first performed on
%   the three R,G,B channels. The segmentation is then carried out on the
%   image component that contains most of the energy.
%
%   Example:
%   -------
%   load clown
%   subplot(221)
%   X = ind2rgb(X,map);
%   imshow(X)
%   title('Original','FontWeight','bold')
%   for n = 2:4
%     IDX = otsu(X,n);
%     subplot(2,2,n)
%     imagesc(IDX), axis image off
%     title(['n = ' int2str(n)],'FontWeight','bold')
%   end
%   colormap(gray)
%
%   Reference:
%   ---------
%   Otsu N, A Threshold Selection Method from Gray-Level Histograms,
%   IEEE Trans. Syst. Man Cybern. 9:62-66;1979
%
%   See also GRAYTHRESH, IM2BW
%
%   -- Damien Garcia -- 2007/08, revised 2010/03
%   Visit my website for more details about OTSU

error(nargchk(1,2,nargin))

% Check if is the input is an RGB image
isRGB = isrgb(I);

assert(isRGB | ndims(I)==2,...
    'The input must be a 2-D array or an RGB image.')

%% Checking n (number of classes)
if nargin==1
    n = 2;
elseif n==1;
    IDX = NaN(size(I));
    sep = 0;
    return
elseif n~=abs(round(n)) || n==0
    error('MATLAB:otsu:WrongNValue',...
        'n must be a strictly positive integer!')
elseif n>255
    n = 255;
    warning('MATLAB:otsu:TooHighN',...
        'n is too high. n value has been changed to 255.')
end

I = single(I);

%% Perform a KLT if isRGB, and keep the component of highest energy
if isRGB
    sizI = size(I);
    I = reshape(I,[],3);
    [V,D] = eig(cov(I));
    [tmp,c] = max(diag(D));
    I = reshape(I*V(:,c),sizI(1:2)); % component with the highest energy
end

%% Convert to 256 levels
I = I-min(I(:));
I = round(I/max(I(:))*255);

%% Probability distribution
unI = sort(unique(I));
nbins = min(length(unI),256);
if nbins==n
    IDX = ones(size(I));
    for i = 1:n, IDX(I==unI(i)) = i; end
    sep = 1;
    return
elseif nbins<n
    IDX = NaN(size(I));
    sep = 0;
    return
elseif nbins<256
    [histo,pixval] = hist(I(:),unI);
else
    [histo,pixval] = hist(I(:),256);
end
P = histo/sum(histo);
clear unI

%% Zeroth- and first-order cumulative moments
w = cumsum(P);
mu = cumsum((1:nbins).*P);

%% Maximal sigmaB^2 and Segmented image
if n==2
    sigma2B =...
        (mu(end)*w(2:end-1)-mu(2:end-1)).^2./w(2:end-1)./(1-w(2:end-1));
    [maxsig,k] = max(sigma2B);

    % segmented image
    IDX = ones(size(I));
    IDX(I>pixval(k+1)) = 2;

    % separability criterion
    sep = maxsig/sum(((1:nbins)-mu(end)).^2.*P);

elseif n==3
    w0 = w;
    w2 = fliplr(cumsum(fliplr(P)));
    [w0,w2] = ndgrid(w0,w2);

    mu0 = mu./w;
    mu2 = fliplr(cumsum(fliplr((1:nbins).*P))./cumsum(fliplr(P)));
    [mu0,mu2] = ndgrid(mu0,mu2);

    w1 = 1-w0-w2;
    w1(w1<=0) = NaN;

    sigma2B =...
        w0.*(mu0-mu(end)).^2 + w2.*(mu2-mu(end)).^2 +...
        (w0.*(mu0-mu(end)) + w2.*(mu2-mu(end))).^2./w1;
    sigma2B(isnan(sigma2B)) = 0; % zeroing if k1 >= k2

    [maxsig,k] = max(sigma2B(:));
    [k1,k2] = ind2sub([nbins nbins],k);

    % segmented image
    IDX = ones(size(I))*3;
    IDX(I<=pixval(k1)) = 1;
    IDX(I>pixval(k1) & I<=pixval(k2)) = 2;

    % separability criterion
    sep = maxsig/sum(((1:nbins)-mu(end)).^2.*P);

else
    k0 = linspace(0,1,n+1); k0 = k0(2:n);
    [k,y] = fminsearch(@sig_func,k0,optimset('TolX',1));
    k = round(k*(nbins-1)+1);

    % segmented image
    IDX = ones(size(I))*n;
    IDX(I<=pixval(k(1))) = 1;
    for i = 1:n-2
        IDX(I>pixval(k(i)) & I<=pixval(k(i+1))) = i+1;
    end

    % separability criterion
    sep = 1-y;

end

IDX(~isfinite(I)) = 0;

%% Function to be minimized if n>=4
    function y = sig_func(k)

        muT = sum((1:nbins).*P);
        sigma2T = sum(((1:nbins)-muT).^2.*P);

        k = round(k*(nbins-1)+1);
        k = sort(k);
        if any(k<1 | k>nbins), y = 1; return, end

        k = [0 k nbins];
        sigma2B = 0;
        for j = 1:n
            wj = sum(P(k(j)+1:k(j+1)));
            if wj==0, y = 1; return, end
            muj = sum((k(j)+1:k(j+1)).*P(k(j)+1:k(j+1)))/wj;
            sigma2B = sigma2B + wj*(muj-muT)^2;
        end
        y = 1-sigma2B/sigma2T; % within the range [0 1]

    end

end

function isRGB = isrgb(A)
% --- Do we have an RGB image?
% RGB images can be only uint8, uint16, single, or double
isRGB = ndims(A)==3 && (isfloat(A) || isa(A,'uint8') || isa(A,'uint16'));
% ---- Adapted from the obsolete function ISRGB ----
if isRGB && isfloat(A)
    % At first, just test a small chunk to get a possible quick negative
    mm = size(A,1);
    nn = size(A,2);
    chunk = A(1:min(mm,10),1:min(nn,10),:);
    isRGB = (min(chunk(:))>=0 && max(chunk(:))<=1);
    % If the chunk is an RGB image, test the whole image
    if isRGB, isRGB = (min(A(:))>=0 && max(A(:))<=1); end
end
end


function [aDeriv,aJacob,aHess,papt]=aCVRuv(x)
%ACVRUV The drift function for a continuous-time motion model where the
%       the target state is given in monostatic range and u-v direction
%       cosine coordinates and the motion is constant-velocity in 3D
%       Cartesian coordinates.
%
%INPUTS: x The 6XN state vector of N targets in the order of
%          [r;u;;v;rDot;uDot;vDot], where r is monostatic range and u and v
%          are direction cosines. The target is assumed to always be in
%          front of the radar and not pass behind the radar.
%
%OUTPUTS: aDeriv The 6XN set of time-derivatives of the N state vectors
%                under the Cartesian linear motion model in r-u-v
%                coordinates.
%         aJacob The 6X6XN set of Jacobians of aDeriv, which can be useful
%                in extended Kalman filters. This is the derivative of each
%                component of aDeriv (selected by row) with respect to the
%                elements of the state [r,u,v,rDot,uDot,vDot] selected by
%                column.
%
%A full derivation of the dynamic model is provided in [1]. Summarizing it
%here, let rVec be a position vector. We shall use the orthonormal basis
%vectors:
%uVec1=[u;v;sqrtw]
%uVec2=[sqrt(w/(1-v2));0;-u/sqrt(1-v2)]
%uVec3=[-u*v/sqrt(1-v2);sqrt(1-v2);-v*sqrt(w/(1-v2))];
%Note that uVec2 is a normalized version of the derivative of uVec1 with
%respect to u and uVec3 is cross(u1,u2). The time-derivatives of the basis
%vectors can themselves be expressed as basis vectors:
%uVec1Dot=c1*u2+c2*u3
%uVec2Dot=c3*u1+c4*c3
%uVec3Dot=c5*u1+c6*u2
%The coefficients c1 through c6 are omitted for brevity but can be found
%by taking the total derivative of each basis vector with respect to time
%and then taking the dot products of the result with the orthonormal basis
%vectors.
%The vector rVec is written
%rVec=r*u1
%The velocity vector is the derivative of rVec. Using the chain rule and
%substituting in the derivative parameters for the basis vectors,
%rVecDot=rDot*uVec1+r*c1*uVec2+r*c2*uVec3
%The decond derivative is more complicated, but simplifies to
%rVecDDot=a1*uVec1+a2*uVec2+a3*uVec3
%where two D's indicates two time-derivatives.
%a1 is a radial acceleration and a3 and a3 are two other Cartesian
%components of acceleration. The expressions for them are
% a1=rDDot+r*(c1*c3+c2*c5);
% a2=2*rDot*c1+r*(c1Dot+c2*c6);
% a3=2*rDot*c2+r*(c2Dot+c1*c4);
%The expression for the drift function comes from setting a1=a2=a3=0 and
%solving for the second derivative terms. The second derivatives in u and v
%arise through the derivatives of c1 and c2.
%
%EXAMPLE:
%Here, we verify that integrating forward with this model is equivalent to
%linear motion in Cartesian coordinates.
% xInitCart=[400;920;1000;-100;20;64];
% T=50;%The prediction time.
% F=FPolyKal(T,6,1);
% xInitRuv=stateCart2Ruv(xInitCart);
% xEndCart=F*xInitCart;
% RelTol=1e-10;
% AbsTol=1e-13;
% xStepsRuv=RKAdaptiveOverRange(xInitRuv,[0;T],@(x,t)aCVRuv(x),0.01,0,[],[],RelTol,AbsTol);
% xEndRuvRK=xStepsRuv(:,end);
% xEndRuvExact=stateCart2Ruv(xEndCart);
% max(abs(xEndRuvRK-xEndRuvExact)./xEndRuvExact)
%One will observe that the error is less than 1e-11, which is a good
%agreement.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic Linear Dynamic Models in Local Coordinates,"
%    Naval Research Laboratory, Washington, D.C., Tech. Rep.
%    NRL/MR/5344--19-9882, 24 Aug. 2019.
%
%August 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

r=x(1,:);
u=x(2,:);
v=x(3,:);
rDot=x(4,:);
uDot=x(5,:);
vDot=x(6,:);

u2=u.^2;
v2=v.^2;
r2=r.^2;

w2=max(0,1-u2-v2);
w=sqrt(w2);

diffV2=1-v2;
diffV=sqrt(diffV2);

diffU2=1-u2;
diffU=sqrt(diffU2);

uDot2=uDot.*uDot;
vDot2=vDot.*vDot;
w3=w2.*w;

rDDot=-(r*(-uDot2*diffV2-2*u*uDot*v*vDot-diffU2*vDot2))/w2;
uDDot=-((2*rDot*uDot)/r)-u*(vDot2*diffU2+uDot2*diffV2+2*u*v*uDot*vDot)/w2;
vDDot=-((2*rDot*vDot)/r)-v*(uDot2*diffV2+vDot2*diffU2+2*u*v*uDot*vDot)/w2;

aDeriv=[rDot;uDot;vDot;rDDot;uDDot;vDDot];

if(nargout>1)
    N=size(x,2);
    if(N>1)
        error('Derivatives are only available for numPoints=1.')
    end

    w4=w2^2;

    ddrDDdr=-(-uDot2*diffV2-2*u*uDot*v*vDot-diffU2*vDot2)/w2;
    ddrDDdu=(2*r*(u*uDot+v*vDot)*(uDot-uDot*v2+u*v*vDot))/w4;
    ddrDDdv=-((2*r*(-u*uDot*v-diffU2*vDot)*(u*uDot+v*vDot))/w4);

    ddrDDduDot=((2*r*(uDot-uDot*v2+u*v*vDot))/w2);
    ddrDDdvDot=-(2*r*(-u*uDot*v-diffU2*vDot))/w2;

    dduDDdr=(2*rDot*uDot)/r2;
    dduDDdu=(-uDot2*(1+u2-v2)*diffV2-4*u*uDot*v*diffV2*vDot+(-1+v2-u2*(-2+u2+3*v2))*vDot2)/w4;
    dduDDdv=(2*u*(-u*uDot*v-diffU2*vDot)*(u*uDot+v*vDot))/w4;
    dduDDdrDot=-((2*uDot)/r);
    dduDDduDot=-((2*rDot)/r)-(2*u*(uDot-uDot*v2+u*v*vDot))/w2;
    dduDDdvDot=-(2*u*(u*uDot*v+vDot-u2*vDot))/w2;

    ddvDDdr=(2*rDot*vDot)/r2;
    ddvDDdu=-((2*v*(u*uDot+v*vDot)*(uDot-uDot*v2+u*v*vDot))/w4);
    ddvDDdv=(uDot2*(u2*(1-3*v2)-diffV2^2)-4*u*diffU2*uDot*v*vDot+diffU2*(-1+u2-v2)*vDot2)/w4;
    ddvDDdrDot=-((2*vDot)/r);
    ddvDDduDot=-(2*v*(uDot-uDot*v2+u*v*vDot))/w2;
    ddvDDdvDot=-((2*rDot)/r)-(2*v*(u*uDot*v+vDot-u2*vDot))/w2;

    aJacob=[0,               0,              0,              1,              0,              0;
            0,               0,              0,              0,              1,              0;
            0,               0,              0,              0,              0,              1;
            ddrDDdr,         ddrDDdu,        ddrDDdv,        0,              ddrDDduDot,     ddrDDdvDot;
            dduDDdr,         dduDDdu,        dduDDdv,        dduDDdrDot,     dduDDduDot,     dduDDdvDot;
            ddvDDdr,         ddvDDdu,        ddvDDdv,        ddvDDdrDot,     ddvDDduDot,     ddvDDdvDot];

    if(nargout>2)
        aHess=zeros(6,6,6);

        u4=u2*u2;
        v4=v2*v2;
        w6=w3*w3;
        diffU4=diffU2*diffU2;
        diffV4=diffV2*diffV2;

        ddrDDdrdr=0;
        ddrDDdudr=(2*(u*uDot+v*vDot)*(uDot-uDot*v2+u*v*vDot))/w4;
        ddrDDdvdr=-((2*(-u*uDot*v-diffU2*vDot)*(u*uDot+v*vDot))/w4);
        ddrDDduDotdr=((2*(uDot-uDot*v2+u*v*vDot))/w2);
        ddrDDdvDotdr=-(-2*u*uDot*v-2*diffU2*vDot)/w2;

        ddrDDdrdu=ddrDDdudr;
        ddrDDdudu=-(2*r*(-uDot2*(1+3*u2-v2)*diffV2-2*u*uDot*v*(3+u2-3*v2)*vDot+v2*(-1-3*u2+v2)*vDot2))/w6;
        ddrDDdvdu=(2*r*(uDot*(1+u2-v2)+2*u*v*vDot)*(2*u*uDot*v+vDot-u2*vDot+v2*vDot))/w6;
        ddrDDduDotdu=(-4*r*u*uDot*(-diffV2)+2*r*v*(1+u2-v2)*vDot)/w4;
        ddrDDdvDotdu=(2*r*v*(uDot*(1+u2-v2)+2*u*v*vDot))/w4;

        ddrDDdrdv=ddrDDdvdr;
        ddrDDdudv=ddrDDdvdu;
        ddrDDdvdv=-(2*r*(u2*uDot2*(-1+u2-3*v2)+2*u*uDot*v*(-3+3*u2-v2)*vDot+diffU2*(-1+u2-3*v2)*vDot2))/w6;
        ddrDDduDotdv=(2*r*u*(2*u*uDot*v+vDot-u2*vDot+v2*vDot))/w4;
        ddrDDdvDotdv=(2*r*u*uDot*(1-u2+v2)+4*r*diffU2*v*vDot)/w4;

        ddrDDdrdRDot=0;
        ddrDDdudRDot=0;
        ddrDDdvdRDot=0;
        ddrDDduDotdRDot=0;
        ddrDDdvDotdRDot=0;

        ddrDDdrduDot=ddrDDduDotdr;
        ddrDDduduDot=ddrDDduDotdu;
        ddrDDdvduDot=ddrDDduDotdv;
        ddrDDduDotduDot=(2*r*diffV2)/w2;
        ddrDDdvDotduDot=(2*r*u*v)/w2;

        ddrDDdrdvDot=ddrDDdvDotdr;
        ddrDDdudvDot=ddrDDdvDotdu;
        ddrDDdvdvDot=ddrDDdvDotdv;
        ddrDDduDotdvDot=ddrDDdvDotduDot;
        ddrDDdvDotdvDot=(2*r*diffU2)/w2;

        %%%
        dduDDdrdr=-((4*rDot*uDot)/r^3);
        dduDDdudr=0;
        dduDDdvdr=0;
        dduDDdrDotdr=(2*uDot)/r2;
        dduDDduDotdr=(2*rDot)/r2;
        dduDDdvDotdr=0;

        dduDDdrdu=dduDDdudr;
        dduDDdudu=-(2*u*uDot2*(3+u2-3*v2)*diffV2-4*uDot*v*diffV2*(-1-3*u2+v2)*vDot+2*u*v2*(3+u2-3*v2)*vDot2)/w6;
        dduDDdvdu=-(2*u2*uDot2*v*(3+u2-3*v2)+4*u*uDot*(1-v4+u2*(-1+3*v2))*vDot+2*v*(1-u4+(-1+3*u2)*v2)*vDot2)/w6;
        dduDDdrDotdu=0;
        dduDDduDotdu=-(2*diffV2*(uDot*(1+u2-v2)+2*u*v*vDot))/w4;
        dduDDdvDotdu=(-4*u*uDot*v*diffV2-2*(diffU4+(-1+3*u2)*v2)*vDot)/w4;

        dduDDdrdv=dduDDdvdr;
        dduDDdudv=dduDDdvdu;
        dduDDdvdv=-(2*u*(u2*uDot2*(1-u2+3*v2)+2*u*uDot*v*(3-3*u2+v2)*vDot-diffU2*(-1+u2-3*v2)*vDot2))/w6;
        dduDDdrDotdv=0;
        dduDDduDotdv=(2*u2*(-2*u*uDot*v+u2*vDot-(1+v2)*vDot))/w4;
        dduDDdvDotdv=(2*u*(u*uDot*(-1+u2-v2)-2*diffU2*v*vDot))/w4;

        dduDDdrdRDot=dduDDdrDotdr;
        dduDDdudRDot=dduDDdrDotdu;
        dduDDdvdRDot=dduDDdrDotdv;
        dduDDdrDotdRDot=0;
        dduDDduDotdRDot=-(2/r);
        dduDDdvDotdRDot=0;

        dduDDdrduDot=dduDDduDotdr;
        dduDDduduDot=dduDDduDotdu;
        dduDDdvduDot=dduDDduDotdv;
        dduDDdrDotduDot=dduDDduDotdRDot;
        dduDDduDotduDot=-((2*u*diffV2)/w2);
        dduDDdvDotduDot=-(2*u2*v)/w2;

        dduDDdrdvDot=dduDDdvDotdr;
        dduDDdudvDot=dduDDdvDotdu;
        dduDDdvdvDot=dduDDdvDotdv;
        dduDDdrDotdvDot=dduDDdvDotdRDot;
        dduDDduDotdvDot=dduDDdvDotduDot;
        dduDDdvDotdvDot=-(2*u*diffU2)/w2;

        %%%
        ddvDDdrdr=-((4*rDot*vDot)/r^3);
        ddvDDdudr=0;
        ddvDDdvdr=0;
        ddvDDdrDotdr=(2*vDot)/r2;
        ddvDDduDotdr=0;
        ddvDDdvDotdr=(2*rDot)/r2;

        ddvDDdrdu=ddvDDdudr;
        ddvDDdudu=-(2*v*(uDot2*(-1+v)*(1+v)*(-1-3*u2+v2)+2*u*uDot*v*(3+u2-3*v2)*vDot+v2*(1+3*u2-v2)*vDot2))/w6;
        ddvDDdvdu=-(2*u*uDot2*(1-v4+u2*(-1+3*v2))+4*uDot*v*(1-u4+(-1+3*u2)*v2)*vDot+2*u*v2*(3-3*u2+v2)*vDot2)/w6;
        ddvDDdrDotdu=0;
        ddvDDduDotdu=(2*v*(2*u*uDot*(-diffV2)+v*(-1-u2+v2)*vDot))/w4;
        ddvDDdvDotdu=-((2*v2*(uDot*(1+u2-v2)+2*u*v*vDot))/w4);

        ddvDDdrdv=ddvDDdvdr;
        ddvDDdudv=ddvDDdvdu;
        ddvDDdvdv=-(2*u2*uDot2*v*(3-3*u2+v2)-4*u*diffU2*uDot*(-1+u2-3*v2)*vDot+2*diffU2*v*(3-3*u2+v2)*vDot2)/w6;
        ddvDDdrDotdv=0;
        ddvDDduDotdv=(-2*uDot*(diffV4+u2*(-1+3*v2))-4*u*diffU2*v*vDot)/w4;
        ddvDDdvDotdv=(2*diffU2*(-2*u*uDot*v+u2*vDot-(1+v2)*vDot))/w4;

        ddvDDdrdRDot=ddvDDdrDotdr;
        ddvDDdudRDot=ddvDDdrDotdu;
        ddvDDdvdRDot=ddvDDdrDotdv;
        ddvDDdrDotdRDot=0;
        ddvDDduDotdRDot=0;
        ddvDDdvDotdRDot=-(2/r);

        ddvDDdrduDot=ddvDDduDotdr;
        ddvDDduduDot=ddvDDduDotdu;
        ddvDDdvduDot=ddvDDduDotdv;
        ddvDDdrDotduDot=ddvDDduDotdRDot;
        ddvDDduDotduDot=-((2*v*diffV2)/w2);
        ddvDDdvDotduDot=-(2*u*v2)/w2;

        ddvDDdrdvDot=ddvDDdvDotdr;
        ddvDDdudvDot=ddvDDdvDotdu;
        ddvDDdvdvDot=ddvDDdvDotdv;
        ddvDDdrDotdvDot=ddvDDdvDotdRDot;
        ddvDDduDotdvDot=ddvDDdvDotduDot;
        ddvDDdvDotdvDot=((2*(-1+u^2)*v)/w2);

        aHess(:,:,1)=[0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrdr,       ddrDDdudr,      ddrDDdvdr,      0,              ddrDDduDotdr,   ddrDDdvDotdr;
                      dduDDdrdr,       dduDDdudr,      dduDDdvdr,      dduDDdrDotdr,   dduDDduDotdr,   dduDDdvDotdr;
                      ddvDDdrdr,       ddvDDdudr,      ddvDDdvdr,      ddvDDdrDotdr,   ddvDDduDotdr,   ddvDDdvDotdr];
        aHess(:,:,2)=[0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrdu,       ddrDDdudu,      ddrDDdvdu,      0,              ddrDDduDotdu,   ddrDDdvDotdu;
                      dduDDdrdu,       dduDDdudu,      dduDDdvdu,      dduDDdrDotdu,   dduDDduDotdu,   dduDDdvDotdu;
                      ddvDDdrdu,        ddvDDdudu,     ddvDDdvdu,      ddvDDdrDotdu,   ddvDDduDotdu,   ddvDDdvDotdu];
        aHess(:,:,3)=[0,               0,              0,              0               0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrdv,       ddrDDdudv,      ddrDDdvdv,      0,              ddrDDduDotdv,   ddrDDdvDotdv;
                      dduDDdrdv,       dduDDdudv,      dduDDdvdv,      dduDDdrDotdv,   dduDDduDotdv,   dduDDdvDotdv;
                      ddvDDdrdv,       ddvDDdudv,      ddvDDdvdv,      ddvDDdrDotdv,   ddvDDduDotdv,   ddvDDdvDotdv];
        aHess(:,:,4)=[0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrdRDot,    ddrDDdudRDot,   ddrDDdvdRDot,   0,              ddrDDduDotdRDot,ddrDDdvDotdRDot;
                      dduDDdrdRDot,    dduDDdudRDot,   dduDDdvdRDot,   dduDDdrDotdRDot,dduDDduDotdRDot,dduDDdvDotdRDot;
                      ddvDDdrdRDot,    ddvDDdudRDot,   ddvDDdvdRDot,   ddvDDdrDotdRDot,ddvDDduDotdRDot,ddvDDdvDotdRDot];
        aHess(:,:,5)=[0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrduDot,    ddrDDduduDot,   ddrDDdvduDot,   0,              ddrDDduDotduDot,ddrDDdvDotduDot;
                      dduDDdrduDot,    dduDDduduDot,   dduDDdvduDot,   dduDDdrDotduDot,dduDDduDotduDot,dduDDdvDotduDot;
                      ddvDDdrduDot,    ddvDDduduDot,   ddvDDdvduDot,   ddvDDdrDotduDot,ddvDDduDotduDot,ddvDDdvDotduDot];
        aHess(:,:,6)=[0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      0,               0,              0,              0,              0,              0;
                      ddrDDdrdvDot,    ddrDDdudvDot,   ddrDDdvdvDot,   0,              ddrDDduDotdvDot,ddrDDdvDotdvDot;
                      dduDDdrdvDot,    dduDDdudvDot,   dduDDdvdvDot,   dduDDdrDotdvDot,dduDDduDotdvDot,dduDDdvDotdvDot;
                      ddvDDdrdvDot,    ddvDDdudvDot,   ddvDDdvdvDot,   ddvDDdrDotdvDot,ddvDDduDotdvDot,ddvDDdvDotdvDot];

        if(nargout>3)
            papt=zeros(6,1);
        end
    end
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [PY_x coeff] = bss_tvproj(x,Y,tvshape,tvstep)

% compute the orthogonal projection of x onto the space of shifted windowed versions of the row(s) of Y
%
% Usage: [PY_x coeff] = bss_tvproj(x,Y,tvshape,tvstep)
%
% Input:
%   - x: row vector of length T corresponding to the signal to be projected,
%   - Y: vector or matrix of length T with n_rows rows which windowed rows span the projection space.
%   - tvshape : row vector of length V corresponding to the window applied
%   to Y to define the projection space
%   - tvstep  : number of samples between two adjacent windows
%
% Ouput:
%   - PY_x: row vector of length T containing the orthogonal projection of
%   x onto the range of the shifted windowed versions of the row(s) of Y.
%   - coeff : matrix with n_rows rows and n_frames columns containing the coefficients
%   of the projection
%
% Developers:  - Cedric Fevotte (fevotte@tsi.enst.fr) - Emmanuel Vincent
% (emmanuel.vincent@irisa.fr) - Remi Gribonval (remi.gribonval@irisa.fr)

% 1. Padd the signals with zeros on the left and the right
V        = size(tvshape,2); % the size of the window
NOVERLAP = V-tvstep;        % convert the hop size into a number of overlapping samples
NPadLeft = tvstep;
NPadRight= 2*V;
x = [zeros(size(x,1),NPadLeft) x zeros(size(x,1),NPadRight)];
Y = [zeros(size(Y,1),NPadLeft) Y zeros(size(Y,1),NPadRight)];

% 2. Decompose Y into frames using tvshape as a window

[Y_frames frames_index] = bss_make_frames(Y,tvshape,NOVERLAP);

% 3. Y_frames is a 3-D array of size n_frames x V x n_rows
% we reshape it to get a 2-D array of size (n_frames x n_rows) x V

[n_frames V1 n_rows]= size(Y_frames); % The frames are Y_frames(f,:,n)
Y_frames = permute(Y_frames,[2 1 3]);
% Y_frames is now V x n_frames x nrows, the frames are Y_frames(:,f,n)

Y_frames = reshape(Y_frames,V,n_frames*n_rows);
% Y_frames is now V x (n_frames x n_rows), the frames are Y_frames(:,k)
% with k=(n-1)*n_frames+f

Y_frames = permute(Y_frames,[2 1]);
% Y_frames is now (n_frames x n_rows) x V, the frames are Y_frames(k,:)
% with k=(n-1)*n_frames+f

% 4. Compute the inner products between x and the frames of the row(s) of Y

x_frames  = bss_make_frames(x,ones(1,V),NOVERLAP);
% x_frames is an array n_frames x V x 1 which frames are x_frames(n,:)

ip = zeros(n_frames*n_rows,1);
for n=1:n_rows % loop on rows of Y
    idxrange = ((n-1)*n_frames)+(1:n_frames); % the index of the frames in Y_frames corresponding to the selected row
    ip(idxrange) = sum(x_frames .* Y_frames(idxrange,:),2);
end

% 5. Compute the Gram matrix, which is square of size (n_frames x n_rows) x
% (n_frames x n_rows)
Gram = sparse(n_frames*n_rows,n_frames*n_rows);

for f=1:n_frames
    for f1 = 1:n_frames
        % locate the range of the intersection between frames
        first = max(frames_index(f),frames_index(f1));
        last  = min(frames_index(f),frames_index(f1))+V-1;
        % if the intersection is non empty, fill it
        if (first <= last)
            nrange  = (first:last)-frames_index(f)+1;
            nrange1 = (first:last)-frames_index(f1)+1;
            idxrange  = f  + n_frames*((1:n_rows)-1);
            idxrange1 = f1 + n_frames*((1:n_rows)-1);
            Gram(idxrange,idxrange1) = Y_frames(idxrange,nrange)*Y_frames(idxrange1,nrange1)';
        end
    end
end

% 6. Apply the inverse of the Gram matrix to ip to get coeff
%coeff = full(Gram)\ip;
coeff = pinv(full(Gram))*ip;
% 7.  Reconstruct using coefficients alpha
PY_x = zeros(size(x));

for f=1:n_frames
    idxrange = f + n_frames*((1:n_rows)-1);
    trange   = frames_index(f)+(0:(V-1));
    PY_x(trange) = PY_x(trange) + coeff(idxrange)'*Y_frames(idxrange,:);
end

% 8. Truncate the projection to the support of the signal
PY_x = PY_x(:,(NPadLeft+1):(end-NPadRight));

% 9. The coefficients are a n_frames*n_rows column vector with entries
% coeff(f+(n-1)*n_frames). We reshape it
coeff = reshape(coeff,n_frames,n_rows);
coeff = permute(coeff,[1 2]);


function degree = r8poly_degree ( na, a )

%*****************************************************************************80
%
%% R8POLY_DEGREE returns the degree of a polynomial.
%
%  Discussion:
%
%    The degree of a polynomial is the index of the highest power
%    of X with a nonzero coefficient.
%
%    The degree of a constant polynomial is 0.  The degree of the
%    zero polynomial is debatable, but this routine returns the
%    degree as 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NA, the dimension of A.
%
%    Input, real A(NA+1), the coefficients of the polynomials.
%
%    Output, integer DEGREE, the degree of A.
%
  degree = na;

  while ( 0 < degree )

    if ( a(degree+1) ~= 0.0 )
      return
    end

    degree = degree - 1;

  end

  return
end


function [H] = globalrescale(f);

% GLOBALRESCALE creates the homogenous spatial transformation matrix
% for a 7 parameter rigid-body transformation with global rescaling
%
% Use as
%   [H] = globalrescale(f)
%
% The transformation vector f should contain the
%   x-shift
%   y-shift
%   z-shift
% followed by the
%   pitch (rotation around x-axis)
%   roll  (rotation around y-axis)
%   yaw   (rotation around z-axis)
% followed by the
%   global rescaling factor

% Copyright (C) 2000-2005, Robert Oostenveld
%
% 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 2 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% $Log: globalrescale.m,v $
% Revision 1.1  2009/01/30 04:02:06  arno
% *** empty log message ***
%
% Revision 1.3  2005/08/15 08:15:32  roboos
% reimplemented the rotate function, which contained an error (the error is in the AIR technical reference)
% changed all functions to be dependent on the rotate, translate and scale function
% all functions now behave consistenly, which also means that they are not compleetly backward compatible w.r.t. the order of the rotations
%
% Revision 1.2  2004/05/19 09:57:07  roberto
% added GPL copyright statement, added CVS log item
%

% compute the homogenous transformation matrix for the translation
T = translate(f([1 2 3]));

% compute the homogenous transformation matrix for the rotation
R = rotate_new(f([4 5 6]));

% compute the homogenous transformation matrix for the global scaling
S = scale(f([7 7 7]));

% compute the homogenous transformation matrix for the combination
H = T*R*S;


function h = plot_gaussian_ellipsoid(m, C, sdwidth, npts, axh,alphaChannel,color)
% PLOT_GAUSSIAN_ELLIPSOIDS plots 2-d and 3-d Gaussian distributions
%
% H = PLOT_GAUSSIAN_ELLIPSOIDS(M, C) plots the distribution specified by
%  mean M and covariance C. The distribution is plotted as an ellipse (in
%  2-d) or an ellipsoid (in 3-d).  By default, the distributions are
%  plotted in the current axes. H is the graphics handle to the plotted
%  ellipse or ellipsoid.
%
% PLOT_GAUSSIAN_ELLIPSOIDS(M, C, SD) uses SD as the standard deviation
%  along the major and minor axes (larger SD => larger ellipse). By
%  default, SD = 1. Note:
%  * For 2-d distributions, SD=1.0 and SD=2.0 cover ~ 39% and 86%
%     of the total probability mass, respectively.
%  * For 3-d distributions, SD=1.0 and SD=2.0 cover ~ 19% and 73%
%     of the total probability mass, respectively.
%
% PLOT_GAUSSIAN_ELLIPSOIDS(M, C, SD, NPTS) plots the ellipse or
%  ellipsoid with a resolution of NPTS (ellipsoids are generated
%  on an NPTS x NPTS mesh; see SPHERE for more details). By
%  default, NPTS = 50 for ellipses, and 20 for ellipsoids.
%
% PLOT_GAUSSIAN_ELLIPSOIDS(M, C, SD, NPTS, AX) adds the plot to the
%  axes specified by the axis handle AX.
%
% Examples:
% -------------------------------------------
%  % Plot three 2-d Gaussians
%  figure;
%  h1 = plot_gaussian_ellipsoid([1 1], [1 0.5; 0.5 1]);
%  h2 = plot_gaussian_ellipsoid([2 1.5], [1 -0.7; -0.7 1]);
%  h3 = plot_gaussian_ellipsoid([0 0], [1 0; 0 1]);
%  set(h2,'color','r');
%  set(h3,'color','g');
%
%  % "Contour map" of a 2-d Gaussian
%  figure;
%  for sd = [0.3:0.4:4],
%    h = plot_gaussian_ellipsoid([0 0], [1 0.8; 0.8 1], sd);
%  end
%
%  % Plot three 3-d Gaussians
%  figure;
%  h1 = plot_gaussian_ellipsoid([1 1  0], [1 0.5 0.2; 0.5 1 0.4; 0.2 0.4 1]);
%  h2 = plot_gaussian_ellipsoid([1.5 1 .5], [1 -0.7 0.6; -0.7 1 0; 0.6 0 1]);
%  h3 = plot_gaussian_ellipsoid([1 2 2], [0.5 0 0; 0 0.5 0; 0 0 0.5]);
%  set(h2,'facealpha',0.6);
%  view(129,36); set(gca,'proj','perspective'); grid on;
%  grid on; axis equal; axis tight;
% -------------------------------------------
%
%  Gautam Vallabha, Sep-23-2007, Gautam.Vallabha@mathworks.com

%  Revision 1.0, Sep-23-2007
%    - File created
%  Revision 1.1, 26-Sep-2007
%    - NARGOUT==0 check added.
%    - Help added on NPTS for ellipsoids

if ~exist('sdwidth', 'var'), sdwidth = 1; end
if ~exist('npts', 'var'), npts = []; end
if ~exist('axh', 'var'), axh = gca; end
if ~exist('alphaChannel','var'),alphaChannel=1;end
if ~exist('color','var'),color=[0 0 1];end

if numel(m) ~= length(m),
    error('M must be a vector');
end
if ~( all(numel(m) == size(C)) )
    error('Dimensionality of M and C must match');
end
if ~(isscalar(axh) && ishandle(axh) && strcmp(get(axh,'type'), 'axes'))
    error('Invalid axes handle');
end

set(axh, 'nextplot', 'add');

switch numel(m)
%   case 2, if alphaChannel,  h=show2d_noalpha(m(:),C,sdwidth,npts,axh,color); else h=show2d(m(:),C,sdwidth,npts,axh,alphaChannel,color); end
    case 2, h=show2d(m(:),C,sdwidth,npts,axh,alphaChannel,color);
    case 3, h=show3d(m(:),C,sdwidth,npts,axh);
   otherwise
      error('Unsupported dimensionality');
end

if nargout==0,
    clear h;
end

%-----------------------------
function h = show2d(means, C, sdwidth, npts, axh,alphaChannel,color)
if isempty(npts), npts=50; end
% plot the gaussian fits
tt=linspace(0,2*pi,npts)';
x = cos(tt); y=sin(tt);
ap = [x(:) y(:)]';
[v,d]=eig(C);
d = sdwidth * sqrt(d); % convert variance to sdwidth*sd
bp = (v*d*ap) + repmat(means, 1, size(ap,2));
%if alphaChannel == 1
%h = plot(bp(1,:), bp(2,:), '-', 'parent', axh);
%set(h,'color',color);
%else
h = patchline(bp(1,:),bp(2,:),'linewidth',2,'linestyle','-','edgealpha',alphaChannel,'edgecolor',color,'LineSmoothing','on');
%end

function h = show2d_noalpha(means, C, sdwidth, npts, axh,color)
if isempty(npts), npts=50; end
% plot the gaussian fits
tt=linspace(0,2*pi,npts)';
x = cos(tt); y=sin(tt);
ap = [x(:) y(:)]';
[v,d]=eig(C);
d = sdwidth * sqrt(d); % convert variance to sdwidth*sd
bp = (v*d*ap) + repmat(means, 1, size(ap,2));
h = plot(bp(1,:), bp(2,:), '-', 'parent', axh);
set(h,'color',color);

%-----------------------------
function h = show3d(means, C, sdwidth, npts, axh)
if isempty(npts), npts=20; end
[x,y,z] = sphere(npts);
ap = [x(:) y(:) z(:)]';
[v,d]=eig(C);
if any(d(:) < 0)
   fprintf('warning: negative eigenvalues\n');
   d = max(d,0);
end
d = sdwidth * sqrt(d); % convert variance to sdwidth*sd
bp = (v*d*ap) + repmat(means, 1, size(ap,2));
xp = reshape(bp(1,:), size(x));
yp = reshape(bp(2,:), size(y));
zp = reshape(bp(3,:), size(z));
h = surf(axh, xp,yp,zp);


%RPY2R Roll-pitch-yaw angles to rotation matrix
%
% R = RPY2R(ROLL, PITCH, YAW, OPTIONS) is an SO(3) orthonornal rotation
% matrix (3x3) equivalent to the specified roll, pitch, yaw angles angles.
% These correspond to rotations about the X, Y, Z axes respectively. If
% ROLL, PITCH, YAW are column vectors (Nx1) then they are assumed to
% represent a trajectory and R is a three-dimensional matrix (3x3xN), where
% the last index corresponds to rows of ROLL, PITCH, YAW.
%
% R = RPY2R(RPY, OPTIONS) as above but the roll, pitch, yaw angles angles
% angles are taken from consecutive columns of the passed matrix RPY =
% [ROLL, PITCH, YAW].  If RPY is a matrix (Nx3) then they are assumed to
% represent a trajectory and R is a three-dimensional matrix (3x3xN), where
% the last index corresponds to rows of RPY which are assumed to be [ROLL,
% PITCH, YAW].
%
% Options::
%  'deg'   Compute angles in degrees (radians default)
%  'zyx'   Return solution for sequential rotations about Z, Y, X axes (Paul book)
%
% Note::
% - In previous releases (<8) the angles corresponded to rotations about ZYX. Many
%   texts (Paul, Spong) use the rotation order ZYX. This old behaviour can be enabled
%   by passing the option 'zyx'
%
% See also TR2RPY, EUL2TR.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function R = rpy2r(roll, varargin)
    opt.zyx = false;
    opt.deg = false;
    [opt,args] = tb_optparse(opt, varargin);

    % unpack the arguments
    if numcols(roll) == 3
		pitch = roll(:,2);
		yaw = roll(:,3);
		roll = roll(:,1);
	elseif nargin >= 3
        pitch = args{1};
        yaw = args{2};
    else
        error('RTB:rpy2r:badarg', 'bad arguments')
    end

    % optionally convert from degrees
    if opt.deg
        d2r = pi/180.0;
        roll = roll * d2r;
        pitch = pitch * d2r;
        yaw = yaw * d2r;
    end

    if ~opt.zyx
        % XYZ order
        if numrows(roll) == 1
            R = rotx(roll) * roty(pitch) * rotz(yaw);
        else
            R = zeros(3,3,numrows(roll));
            for i=1:numrows(roll)
                R(:,:,i) = rotx(roll(i)) * roty(pitch(i)) * rotz(yaw(i));
            end
        end
    else
        % old ZYX order (as per Paul book)
        if numrows(roll) == 1
            R = rotz(roll) * roty(pitch) * rotx(yaw);
        else
            R = zeros(3,3,numrows(roll));
            for i=1:numrows(roll)
                R(:,:,i) = rotz(roll(i)) * roty(pitch(i)) * rotx(yaw(i));
            end
        end
    end


function x = jacobi2 ( n, a, b, x )

%*****************************************************************************80
%
%% JACOBI2 carries out one step of the Jacobi iteration.
%
%  Discussion:
%
%    MATLAB's vector notation is used to express the operation in one line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of equations.
%
%    Input, real A(N,N), the matrix.
%
%    Input, real B(N), the right hand side.
%
%    Input, real X(N), the estimated solution.
%
%    Output, real X_NEW(N), the improved estimate of the solution.
%
  x = ( b - a * x + ( diag ( a ) .* x ) ) ./ diag ( a );

  return
end


function G = gaborPyramid(s, theta, sz);
% Create a gabor wavelet pyramid.
%
% G = gaborPyramid(<s=0:2>, <theta=22.5:22.5:180>, <sz=[256 256]>);
%
% BACKGROUND: this function creates a wavelet pyramid analogous to that
% used by Kay et al (Nature 2000) to represent images for application to
% fMRI activity patterns. This pyramid contains a series of Gabor wavelets,
% each similar to the one described by Tai Sing Lee, "Image Representation
% Using 2D Gabor Wavelets".
% (IEEE Transactions 1996: online at www.cnbc.cmu.edu/~tai/papers/pami.pdf)
%
% "Gabor" wavelets (or "Weyl-Heisenberg groups" in physics) are a combined
% harmonic and Gaussian function which the physicist Dennis Gabor showed
% convey an optimal combination of spatial (or temporal) position localization
% and frequency-domain localization. These functions serve as models of
% receptive fields for early visual neurons, esp. simple cells. See the Lee
% paper for more background.
%
% "Pyramid" refers to the fact that, for different spatial frequencies,
% there will be a different number of channels, spanning the different
% positions that can be localized. This code follows the Kay et al
% convention of having spatial frequencies be powers of 2 in cycles per
% image: for 2 cycles per image, there will be 2^2=4 possible positions;
% for 4 cycles per image, there will be 4^2 possible positions.
%
%
% INPUTS:
% s: vector of spatial frequencies to use. s should be an integer, and specify
% the log base 2 of the spatial frequency, in units of cycles / image. That is,
% 2^s is the number of cycles in the image for a given channel.
% E.g.: s=0 indicates a single cycle per image; s=1 indicates two cycles
% per image. A DC offset is automatically added to the list of channels as well,
% which has undefined spatial frequency (2^s = 0).
%
% theta: vector of orientations to use, in degrees counterclockwise from
% the x0 axis. That is, 0=horizontal, 45 = increasing in x0/y0 direction, 90 =
% vertical, 135 = increasing in x0 while decreasing in y0 direction. Note
% that theta is cyclic with modulo 180 (theta=200 is the same as theta=20).
%
% sz: size [x y] of the resulting wavelets.
%
% OUTPUTS:
% Return results in a structure G, with the following fields:
%	odd: 3-D matrix of size [imgSize(1) imgSize(2) nChannels].
%		odd-symmetric (sine) component channels.
%		The # of channels depends on omega and theta (see above), and
%		represents the even channels only in the complex function.
%	even: same size as odd, containing even-symmetric (cosine) component
%	channels.
%	cyclesPerImage: vector of wavelengths corresponding to each channel.
%	orientation: vector of orientations corresponding to each channel.
%	x0: vector of x0 position centers for each channel
%	y0: vector of y0 position centers for each channel.
%
% REFERENCES: We implement a version of the wavelet decomposition used in
% Kay et al., Nature 2008. See also www.cnbc.cmu.edu/~tai/papers/pami.pdf.
%
% ras, 03/2008.
if notDefined('s'),			s = 0:2;					end
if notDefined('theta'),		theta = 22.5:22.5:180;		end
if notDefined('sz'),		sz = [128 128];				end

%%%%% params / setup
%% constants
kappa = 2.5; % pi;  % constrains the spat. freq. bandwith to be 1 octave

%% parse inputs
if length(sz)==1  % square image, only 1 size specified
	sz = [sz sz];
end

%% init empty struct
G.odd = [];
G.even = [];
G.layer = [];
G.cyclesPerImage = [];
G.orientation = [];
G.x0 = [];
G.y0 = [];

%%%%% (1) determine the number of channels
% first, convert the spatial frequency param s into units of cycles per
% image (cpi):
cpi = 2 .^ s;

% we add one channel for the DC component:
cpi = [0 cpi];

% for each # of cycles/image, we have a square grid of positions for that
% frequency (see Kay et al, Sup Fig 2).
tmp = sum( cpi .^ 2 ); % # of joint spatial-frequency/position combinations

% for each layer except the first one, we have one channel for each
% orientation. So, figure out the total # of channels:
nChannels = tmp * length(theta) + 1;
G.nChannels = nChannels;

%%%%% (2) determine parameters for each channel
% now, assign to each channel a unique combination of spatial frequency
% (cyclesPerImage), orientation, and position (x0 and y0). We'll call the
% set of channels corresponding to this spat. freq a "layer" in the pyramid:
for layer = 1:length(cpi)
	% the 0 cycles/image layer is a special case: only 1 DC channel
	if cpi(layer)==0
		G.layer = 1;
		G.cyclesPerImage = 0;
		G.orientation = 0; % could be NaN; this will help w/ plotting
		G.x0 = 0;
		G.y0 = 0;

	else
		% # positions along x/y axis for this layer
		nGrid = cpi(layer);

		% # channels in this layer
		n = length(theta) * nGrid ^ 2;

		% append cpi, orientation values
		G.layer = [G.layer repmat(layer, [1 n])];
		G.cyclesPerImage = [ G.cyclesPerImage, repmat(cpi(layer), [1 n]) ];
		G.orientation = [ G.orientation, repmat(theta, [1 nGrid^2]) ];

		% get a grid of (x0, y0) centers for each position for this layer
		% these positions are in pixels from the uppper left-hand corner of
		% the image:
		yRange = linspace(0, sz(1), nGrid+2);  yRange = yRange(2:end-1);
		xRange = linspace(0, sz(2), nGrid+2);  xRange = xRange(2:end-1);
		[x0 y0] = meshgrid( xRange, yRange );

		% we tile the position space separately for each orientation
		x0 = repmat(x0(:)', [length(theta) 1]);
		y0 = repmat(y0(:)', [length(theta) 1]);
		G.x0 = [G.x0, x0(:)'];
		G.y0 = [G.y0, y0(:)'];
	end
end

%%%%% (3) create the channels
% initialize the empty channels
G.odd	= zeros(sz(1), sz(2), nChannels);
G.even	= zeros(sz(1), sz(2), nChannels);

% main loop
for n = 1:nChannels
	% get a map of (x,y) pixel positions relative to the center of the
	% Gabor function for this channel (x0, y0) across the image
	[x y] = meshgrid( [1:sz(2)] - G.x0(n), [1:sz(1)] - G.y0(n) );

	% express the orientation in radians
	theta = G.orientation(n) * pi/180;

	% convert spatial frequency from units of cycles per image to units of
	% radians per unit distance: we take as the image size the larger of
	% the two edge lengths for the image:
	omega = 2*pi * G.cyclesPerImage(n)*2 / max(sz);

	% compute the wavelet
	phi = gaborWavelet(x, y, theta, omega, kappa);

	% grab the odd and even components
	G.even(:,:,n)	= real(phi);
	G.odd(:,:,n)	= imag(phi);
end

% to save memory, let's make everything single-precision
for f = fieldnames(G)'
	G.(f{1}) = single( G.(f{1}) );
end

return


%% DEMO 9:  Algorithms 04. Total Variation minimization algorithms
%
%
%  This demo presents the Total variation algorithms in TIGRE. Total
%  variation algorithms try to minimize the variation (gradient) of the
%  image, assuming its piecewise smooth, as most things in nature are (i.e.
%  human body).
%
% This set of algorithms is specially good performing when the noise is
% very big or the number of projections is small, however, they require more
% computational time and memory than the other algorithms to run.
%
%
% This Demo is not fully complete, as several algorithms are not shown
% here, yet are available in TIGRE, namely:
%
% - AwASD_POCS:  Edge preserving ASD_POCS (Adaptative weigthed).
% - OS_AwASD_POCS: OS-version of the previous algorithm
% - PCSD: A version of ASD_POCS that heuristically select some of the
%         parameters, particularly epsilon (maxL2norm)
% - AwPCSD: Edge preserving version of the previous algorithm
% - OS_AwPCSD: block-wise version of the previous
%
% You can use these algorithms in a very similar manner as below examples.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------
%% Initialize
clear;
close all;

%% Define Geometry
geo=defaultGeometry('nVoxel',[128;128;128]);

%% Load data and generate projections
% see previous demo for explanation
angles=linspace(0,2*pi-2*pi/30,30);
head=headPhantom(geo.nVoxel);
projections=Ax(head,geo,angles,'interpolated');
noise_projections=addCTnoise(projections);

%% Lets create a OS-SART test for comparison
[imgOSSART,errL2OSSART]=OS_SART(noise_projections,geo,angles,60);

%% Total Variation algorithms
%
%  ASD-POCS: Adaptative Steeppest Descent-Projection On Convex Subsets
% Often called POCS-TV
%==========================================================================
%==========================================================================
%  ASD-POCS minimizes At-B and the TV norm separately in each iteration,
%  i.e. reconstructs the image first and then reduces the TV norm, every
%  iteration. As the other algorithms the mandatory inputs are projections,
%  geometry, angles and maximum iterations.
%
% ASD-POCS has a veriety of optional arguments, and some of them are crucial
% to determine the behaviour of the algorithm. The advantage of ASD-POCS is
% the power to create good images from bad data, but it needs a lot of
% tunning.
%
%  Optional parameters that are very relevant:
% ----------------------------------------------
%    'maxL2err'    Maximum L2 error to accept an image as valid. This
%                  parameter is crucial for the algorithm, determines at
%                  what point an image should not be updated further.
%                  Default is 20% of the FDK L2 norm.
%
% its called epsilon in the paper
epsilon=im3Dnorm(Ax(FDK(noise_projections,geo,angles),geo,angles)-noise_projections,'L2')*0.15;
%   'alpha':       Defines the TV hyperparameter. default is 0.002.
%                  However the paper mentions 0.2 as good choice
alpha=0.002;

%   'TViter':      Defines the amount of TV iterations performed per SART
%                  iteration. Default is 20

ng=25;

% Other optional parameters
% ----------------------------------------------
%   'lambda':      Sets the value of the hyperparameter for the SART iterations.
%                  Default is 1
%
%   'lambdared':   Reduction of lambda Every iteration
%                  lambda=lambdared*lambda. Default is 0.99
%
lambda=1;
lambdared=0.9999;

%   'alpha_red':   Defines the reduction rate of the TV hyperparameter
alpha_red=0.95;

%   'Ratio':       The maximum allowed image/TV update ration. If the TV
%                  update changes the image more than this, the parameter
%                  will be reduced.default is 0.95
ratio=0.94;

%   'Verbose'      1 or 0. Default is 1. Gives information about the
%                  progress of the algorithm.

verb=true;

imgASDPOCS=ASD_POCS(noise_projections,geo,angles,50,...
                    'TViter',ng,'maxL2err',epsilon,'alpha',alpha,... % these are very important
                     'lambda',lambda,'lambda_red',lambdared,'Ratio',ratio,'Verbose',verb); % less important.

%  OS_ASD_POCS: Odered Subset-TV algorithm
%==========================================================================
%==========================================================================
%
% The logical next step to imporce ASD-POCS is substituting SART with a
% faster algorithm, such as OS-SART
%
% The parameters are the same as in ASD-POCS, but also have 'BlockSize' and
% @OrderStrategy', taken from OS-SART

imgOSASDPOCS=OS_ASD_POCS(noise_projections,geo,angles,50,...
                    'TViter',ng,'maxL2err',epsilon,'alpha',alpha,... % these are very important
                     'lambda',lambda,'lambda_red',lambdared,'Ratio',ratio,'Verbose',verb,...% less important.
                      'BlockSize',size(angles,2)/10,'OrderStrategy','angularDistance'); %OSC options

%  B-ASD-POCS-beta
%==========================================================================
%==========================================================================
% Is another version of ASD-POCS that has uses Bregman iteration, i.e.
% updates the data vector every some ASD-POCS iteration, bringing closer
% the data to the actual image. According to the original article, this
% accelerates convergence rates, giving a better image in the same amount
% of iteratiosn.
%
% It has all the inputs from ASD-POCS, and has 3 extra:
%   'beta'         hyperparameter controling the Bragman update. default=1
%
%   'beta_red'     reduction of the beta hyperparameter. default =0.75
%
%   'bregman_iter' amount of global bregman iterations. This will define
%                  how often the bregman iteration is executed. It has to
%                  be smaller than the number of iterations.

%Note that the number of iteration for TV has changed

imgBASDPOCSbeta=B_ASD_POCS_beta(noise_projections,geo,angles,50,...
                    'TViter',40,'maxL2err',epsilon,'alpha',alpha,... % these are very important
                     'lambda',lambda,'lambda_red',lambdared,'Ratio',ratio,'Verbose',verb,... % less important.
                      'beta',0.5,'beta_red',0.7,'bregman_iter',10); % bregman options

%   SART-TV
%==========================================================================
%==========================================================================
%
%   This implementation differs more from the others, as it minimizes the
%   ROF model, i.e. when minimizing the total variation of the image, it
%   also takes into account the information of the image. If only the total
%   variation minimization step was run in the rest of the algorithms, the
%   result would be a flat image (as that is the minimum total variation
%   image), altertatively, the ROF model enforces the image not to change too
%   much.
%
%   This algirths performs better with more projections, but noisy data, by
%   enforncing the TV very little
%
%   The optional parameters are for the total variatiot part of the image:
%
%
%
%   'TViter'       amoutn of iteration in theTV step. Default 50
%
%   'TVlambda'     hyperparameter in TV iteration. IT gives the ratio of
%                  importance of the image vs the minimum total variation.
%                  default is 15. Lower means more TV denoising.
%

imgSARTTV=SART_TV(noise_projections,geo,angles,50,'TViter',100,'TVlambda',50);

% IRN_TV_CGLS
%==========================================================================
%==========================================================================
% CGLS with TV regularization in an inner/outer iteration scheme
%
% 'lambda' hyperparameter in TV norm. It gives the ratio of
%          importance of the image vs the minimum total variation.
%          default is 15. Lower means less TV denoising.
%
% 'niter_outer' Number of outer iterations. Each outer iteration will
%               perform niter number of inner iterations, in the example
%               below, 20.Albeit this seems that it does many more
%               iterations than the other algorithms, this is an inherently
%               faster algorithm, both in convergence and time.

imgIRN_TV_CGLS=IRN_TV_CGLS(noise_projections,geo,angles,20,'lambda',5,'niter_outer',2);
% hybrid_fLSQR_TV
%==========================================================================
%==========================================================================
% flexyble hybrod LSQR. Has TV regularization with reorthogonalization.
%
% 'lambda' hyperparameter in TV norm. It gives the ratio of
%          importance of the image vs the minimum total variation.
%          default is 15. Lower means less TV denoising.
%
imgflsqr=hybrid_fLSQR_TV(projections,geo,angles,20,'lambda',5);

 %% Lets visualize the results
% Notice the smoother images due to TV regularization.
%
%     head              OS-SART           ASD-POCS
%
%     OS-ASD-POCS       B-ASD-POCS-beta   SART-TV
%
%     IRN-TV-CGLS        hybrid-fLSQR       heads

plotImg([imgIRN_TV_CGLS imgflsqr  head;
         imgOSASDPOCS imgBASDPOCSbeta imgSARTTV;
         head, imgOSSART, imgASDPOCS ] ,'Dim','Z','Step',2,'clims',[0 1])
 % error
plotImg(abs([head-imgIRN_TV_CGLS head-imgflsqr  head-head;
         head-imgOSASDPOCS head-imgBASDPOCSbeta head-imgSARTTV;
         head-head, head-imgOSSART, head-imgASDPOCS ]) ,'Dim','Z','Slice',64,'clims',[0 0.1])

%%
% Obligatory XKCD reference: https://xkcd.com/833/


% ATANH  Inverse hyperbolic tangent.
%    ATANH(X) is the inverse hyperbolic tangent of the elements of X.
%
%    See also TANH.
%
%    Reference page in Doc Center
%       doc atanh
%
%    Other functions named atanh
%
%       codistributed/atanh    gpuArray/atanh    sym/atanh    ts/atanh
%


function mse = MeanSquareError(x,y)
% This function computes the mean square error
% between x and y

mse = sum( sum( (y - x).^2 ) );
mse = mse / prod(size(x));


function [r,s] = xytors(x,y)

% function [r,s] = xytors(x, y)
% Purpose : From (x,y) in equilateral triangle to (r,s) coordinates in standard triangle

L1 = (sqrt(3.0)*y+1.0)/3.0;
L2 = (-3.0*x - sqrt(3.0)*y + 2.0)/6.0;
L3 = ( 3.0*x - sqrt(3.0)*y + 2.0)/6.0;

r = -L2 + L3 - L1; s = -L2 - L3 + L1;
return;


function varargout = angleSort3d(pts, varargin)
%ANGLESORT3D Sort 3D coplanar points according to their angles in plane
%
%   PTS2 = angleSort3d(PTS);
%   Considers all points are located on the same plane, and sort them
%   according to the angle on plane. PTS is a [Nx2] array. Note that the
%   result depend on plane orientation: points can be in reverse order
%   compared to expected. The reference plane is computed besed on the 3
%   first points.
%
%   PTS2 = angleSort3d(PTS, PTS0);
%   Computes angles between each point of PTS and PT0. By default, uses
%   centroid of points.
%
%   PTS2 = angleSort3d(PTS, PTS0, PTS1);
%   Specifies the point which will be used as a start.
%
%   [PTS2, I] = angleSort3d(...);
%   Also return in I the indices of PTS, such that PTS2 = PTS(I, :);
%
%   See also:
%   points3d, angles3d, angleSort
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2005-11-24
% Copyright 2005 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY :
%   04/01/2007: remove unused variables

% default values
pt0     = mean(pts, 1);
pt1     = pts(1,:);

if length(varargin)==1
    pt0 = varargin{1};
elseif length(varargin)==2
    pt0 = varargin{1};
    pt1 = varargin{2};
end

% create support plane
plane   = createPlane(pts(1:3, :));

% project points onto the plane
pts2d   = planePosition(pts, plane);
pt0     = planePosition(pt0, plane);
pt1     = planePosition(pt1, plane);

% compute origin angle
theta0  = atan2(pt1(2)-pt0(2), pt1(1)-pt0(1));
theta0  = mod(theta0 + 2*pi, 2*pi);

% translate to reference point
n       = size(pts, 1);
pts2d   = pts2d - repmat(pt0, [n 1]);

% compute angles
angle   = atan2(pts2d(:,2), pts2d(:,1));
angle   = mod(angle - theta0 + 4*pi, 2*pi);

% sort points according to angles
[angle, I] = sort(angle); %#ok<ASGLU>

% format output
if nargout<2
    varargout{1} = pts(I, :);
elseif nargout==2
    varargout{1} = pts(I, :);
    varargout{2} = I;
end


%SKEW Create skew-symmetric matrix
%
% S = SKEW(V) is a skew-symmetric matrix formed from V.
%
% If V (1x1) then S =
%
%           | 0  -v |
%           | v   0 |
%
% and if V (1x3) then S =
%
%           |  0  -vz   vy |
%           | vz    0  -vx |
%           |-vy   vx    0 |
%
%
% Notes::
% - This is the inverse of the function VEX().
% - These are the generator matrices for the Lie algebras so(2) and so(3).
%
% References::
% - Robotics, Vision & Control: Second Edition, Chap 2,
%   P. Corke, Springer 2016.
%
% See also SKEWA, VEX.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

function S = skew(v)
    if isvec(v,3)
        % SO(3) case
        S = [  0   -v(3)  v(2)
              v(3)  0    -v(1)
             -v(2) v(1)   0];
    elseif isvec(v,1)
        % SO(2) case
        S = [0 -v; v 0];
    else
        error('SMTB:skew:badarg', 'argument must be a 1- or 3-vector');
    end


function f = p37_f ( n, x )

%*****************************************************************************80
%
%% P37_F evaluates the objective function for problem 37.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Zbigniew Michalewicz,
%    Genetic Algorithms + Data Structures = Evolution Programs,
%    Third Edition,
%    Springer Verlag, 1996,
%    ISBN: 3-540-60676-9,
%    LC: QA76.618.M53.
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  arg = - ( x(1) - pi )^2 - ( x(2) - pi )^2;
  f = - cos ( x(1) ) * cos ( x(2) ) * exp ( arg );

  return
end


function pass = test_grad( )
% Test curl

tol = 1e2*chebfunpref().cheb2Prefs.chebfun2eps;

% Check gradient of an empty diskfun is an empty diskfunv.
u = diskfun;
f = grad(u);
pass(1) = isempty(f) & isa(f,'diskfunv');

% Check gradient of the zero function is zero
f = diskfun(@(x,y) 0*x);
pass(2) = norm(grad(f)) < tol;

%
% Check gradient of a non-zero function gives the correct result
%

% Example 1
u = grad(diskfun(@(x,y,z) x.^4.*(y-y.^2)) );
% Exact gradient
f = diskfun(@(x,y) 4*x.^3.*(y-y.^2));
g = diskfun(@(x,y) x.^4.*(1-2*y));
exact = diskfunv(f,g);
pass(3) = norm(u-exact) < tol;

% Example 2
u = grad(diskfun(@(x,y) cos(4*x).*sin(x.*y)));
% Exact gradient
f = diskfun(@(x,y) -4*sin(4*x).*sin(x.*y)+cos(4*x).*cos(x.*y).*y);
g = diskfun(@(x,y) cos(4*x).*cos(x.*y).*x);
exact = diskfunv(f,g);
pass(4) = norm(u-exact) < 2*tol;

% Example 3
u = grad(diskfun(@(x,y) exp(-3*(x.^2+(y+.2).^2)) ));
% Exact gradient
f = diskfun(@(x,y) -6.*x.*exp(-3*(x.^2+(y+.2).^2)));
g = diskfun(@(x,y) -6.*(y+.2).*exp(-3*(x.^2+(y+.2).^2)));
exact = diskfunv(f,g);
pass(5) = norm(u-exact) < 10*tol;

% Check that the grad and gradient give the same result.
gradientu = gradient(diskfun(@(x,y) exp(-3*(x.^2+(y+.2).^2)) ));
pass(6) = isequal(u,gradientu);

end


function [ sa, seed ] = wshrt ( d, n, np, seed )

%*****************************************************************************80
%
%% WSHRT returns a random Wishart variate.
%
%  Discussion:
%
%    This routine is a Wishart variate generator.
%
%    On output, SA is an upper-triangular matrix of size NP * NP,
%    written in linear form, column ordered, whose elements have a
%    Wishart(N, SIGMA) distribution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2014
%
%  Author:
%
%    Original FORTRAN77 version by William Smith, Ronald Hocking.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    William Smith, Ronald Hocking,
%    Algorithm AS 53, Wishart Variate Generator,
%    Applied Statistics,
%    Volume 21, Number 3, pages 341-345, 1972.
%
%  Parameters:
%
%    Input, real D(NP*(NP+1)/2), the upper triangular array that
%    represents the Cholesky factor of the correlation matrix SIGMA.
%    D is stored in column-major form.
%
%    Input, integer N, the number of degrees of freedom.
%    1 <= N <= NP.
%
%    Input, integer NP, the size of variables.
%
%    Input/output, integer SEED, a seed for the random number generator.
%
%    Output, real SA(NP*(NP+1)/2), a sample from the Wishart distribution.
%
  k = 1;
%
%  Load SB with independent normal (0, 1) variates.
%
  nnp = floor ( ( np * ( np + 1 ) ) / 2 );
  sb = zeros ( nnp );

  while ( k <= nnp )

    [ u1, u2, seed ] = rnorm ( seed );

    sb(k) = u1;
    k = k + 1;

    if ( k <= nnp )
      sb(k) = u2;
      k = k + 1;
    end

  end
%
%  Load diagonal elements with square root of chi-square variates.
%
  sa = zeros ( nnp );

  ns = 0;

  for i = 1 : np
%
%  The original text read "DF = N - I + 1".
%  This should read "DF = NP - I + 1".
%
    df = np - i + 1;
    ns = ns + i;
    u1 = 2.0 / ( 9.0 * df );
    u2 = 1.0 - u1;
    u1 = sqrt ( u1 );
%
%  Wilson-Hilferty formula for approximating chi-square variates:
%  The original code did not take the absolute value!
%
    sb(ns) = sqrt ( df * abs ( ( u2 + sb(ns) * u1 ) ^ 3 ) );

  end

  rn = n;
  nr = 1;

  for i = 1 : np
    nr = nr + i - 1;
    for j = i : np
      ip = nr;
      nq = floor ( ( j * ( j - 1 ) ) / 2 ) + i - 1;
      c = 0.0;
      for k = i : j
        ip = ip + k - 1;
        nq = nq + 1;
        c = c + sb(ip) * d(nq);
      end
      sa(ip) = c;
    end
  end

  for i = 1 : np
    ii = np - i + 1;
    nq = nnp - np;
    for j = 1 : i
      ip = floor ( ( ii * ( ii - 1 ) ) / 2 );
      c = 0.0;
      for k = i : np
        ip = ip + 1;
        nq = nq + 1;
        c = c + sa(ip) * sa(nq);
      end
      sa(nq) = c / rn;
      nq = nq - 2 * np + i + j - 1;
    end
  end

  return
end


%%DEMOGRAVCODENOCOMPILE This file demonstrates the functions for spherical
%                       harmonic synthesis of gravitational and terrain
%                       data. Unlike the function DemoGravCode, this
%                       function does not make any plots, but does run fast
%                       even when the mex files have not ben compiled. The
%                       gravitational code library (that this function is
%                       part of) must have been added to Matlab's search
%                       path for this function to work.
%
%Rather than using advanced spherical harmonic models at high orders, this
%function loads spherical harmonic coefficients only up to degree and order
%50 for a simple ellipsoidal Earth model. The gravitational potential and
%acceleration due to gravity from the ellipsoidal model as generated by the
%coefficients are shown to match (within reasonable precision bounds) the
%results obtained from closed-form functions.
%
%When run, the error in the potential should be 5.9474e-14% and the error
%in the magnitude of the gravitational acceleration should be 3.3790e-12%.
%
%February 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

M=50;%The maximum degree and order of the model.
%Generate coefficients for an ellipsoidal Earth model.
[C,S,a,c]=ellipsGravCoeffs(M);

lambda=-20*pi/180;%Longitude
theta=80*pi/180;%Ellipsoidal latitude
%The zero altitude places the point on the reference ellipsoid.
pointEllips=[theta;lambda;0];
pointSpher=ellips2Sphere(pointEllips);%Convert to spherical coordinates.

%Compute the potential and acceleration due to gravity.
[V,gradV]=spherHarmonicEval(C,S,pointSpher,a,c);

%Add in the centripetal effects of the Earth's rotation. The formulae for
%the extra terms in the potential and acceleration due to gravity can be
%found in Chapters 2.1 and 2.7 of
%B. Hofmann-Wellenhof and H. Moritz, Physical Geodesy, 2nd ed.
%SpringerWienNewYork, 2006.
CartPoint=ellips2Cart(pointEllips);%Convert to Cartesian coordinates.
omega=Constants.WGS84EarthRotationRate;
U=V+0.5*omega^2*(CartPoint(1)^2+CartPoint(2)^2);%The gravity potential
%Acceleration due to gravity with the Earth's rotation.
g=gradV+omega^2*[CartPoint(1);CartPoint(2);0];

%Get the exact values of U and g from explicit formulas for the ellipsoidal
%Earth approximation.
[U0,g0]=ellipsParam2Grav(CartPoint);

disp('The geodetic latitude and longitude of the point on the surface of')
disp('the WGS-84 reference ellipsoid where the values are computed are (in degrees)')
latitude=theta*(180/pi)
longitude=lambda*(180/pi)

disp('The gravity potential from the spherical harmonic solution is (in m^2/s^2)')
U

disp('The gravitational acceleration vector (with centripetal acceleration)')
disp('from the spherical harmonic solution is (in m/s^2)')
g

disp('The percent error in the potential from the spherical harmonic')
disp('approximation and from the explicit solution is')
percentErrorPotential=100*(U-U0)/U0

disp('The percent error in the gravitational acceleration magnitude from')
disp('the spherical harmonic approximation and from the explicit solution is')
percentErrorGravAccel=100*(norm(g)-norm(g0))/norm(g0)

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
% ##2
%==============================================================================
%
% Tutorial for FAIR: regulariztion
%
% given a certain force field f on a 2D domain, this tutorial
% computes and visualizes the curvature regularized displacement u, such that
%
%  B'*B u = f
%
% where B is the discrete curvature operator
% see also getCurvatureMatrix
%==============================================================================

clear, close all, help(mfilename);

                    % 2D example
omega  = [0,1,0,1]; % physical domain
m      = [16,12];   % number of discretization points
n      = prod(m);   % number of cells, and staggered dimensions
hd     = prod((omega(2:2:end)-omega(1:2:end))./m);
alpha  = 10;

% generate cell centered and nodal grids
% note: computation is cell centered, others used for visualization
xc = getCellCenteredGrid(omega,m);
xn = getNodalGrid(omega,m);

% build elasticity operator on a staggered grid and visualize
B = getCurvatureMatrix(omega,m);
FAIRfigure(1); clf; subplot(2,2,1); spy(B); title('B curcature on cell-centered grid')

% create a force field o cell centered grid and visualize
fc  = [0*xc(1:n);-10*sin(pi*xc(1:n))];

% compute the displacement from (B'*B)*us = fs
% note B is derivative operator and has null space (constants)

% remove constant parts: E'*(f-E*w) = 0
E = [ones(n,1)*[1,0];ones(n,1)*[0,1]];
fc = fc - E*((E'*E)\(E'*fc));

% solve for displacements
warning off % matrix is singular, but MATLAB doesn't care
uc = hd*alpha*(B'*B)\fc;
warning on

% visualization on cell centered and nodal grids
un = grid2grid(uc,m,'centered','nodal');

% shortcuts for visualization,
vecNorm   = @(v) sqrt(sum(reshape(v,[],2).^2,2));
viewField = @(v) viewImage2Dsc(vecNorm(v),omega,m);
PG        = @(x) plotGrid(x,omega,m);

% visualize force field
FAIRfigure(1); subplot(2,2,3);
viewField(fc); hold on; colormap(gray); colorbar; PG(xn);
qh = quiver(xc(1:n),xc(n+1:end),fc(1:n),fc(n+1:end),1);
set(qh,'color','r','linewidth',1)
title('a force field')

% visualize displacement field
FAIRfigure(1); subplot(2,2,4);
viewField(uc); hold on; colormap(gray); colorbar; PG(xn);
qh = quiver(xc(1:n),xc(n+1:end),uc(1:n),uc(n+1:end),1);
set(qh,'color','g','linewidth',1)
title('the displacement field')

% visuaize the displaced grid
FAIRfigure(1); subplot(2,2,2);
xh = PG(xn); hold on; axis equal image; yh = PG(xn+un);
set(xh,'linewidth',2); set(xh,'linewidth',2,'color','g');
title('initial and displaced grids')

%==============================================================================


function [trans,rot,scale,skew] = affineDecompose(A)
%
% [trans,rot,scale,skew] = affineDecompose(A)
%
% Decomposes an affine (orthogonal linear) transformation matrix into its
% four componets: translations, rotations scales and skews.
%
% (See affineBuild for more info.)
%
% HISTORY:
%   2004.03.12 RFD (bob@white.stanford.edu) shamelessly copied the core
%   algorithm from spm99's spm_imatrix.m.
%   2005.07.08 ras (sayres at stanford edu) imported into mrVista 2.0

% Translations and zooms
%-----------------------------------------------------------------------
R         = A(1:3,1:3);
C         = chol(R'*R);
P         = [A(1:3,4)' 0 0 0  diag(C)'  0 0 0];
if det(R)<0, P(7)=-P(7);end % Fix for -ve determinants

% Shears
%-----------------------------------------------------------------------
C         = diag(diag(C))\C;
P(10:12)  = C([4 7 8]);
R0        = affineBuild([0 0 0], [0 0 0], P(7:9), P(10:12));
R0        = R0(1:3,1:3);
R1        = R/R0;

% This just leaves rotations in matrix R1
%-----------------------------------------------------------------------
%[          c5*c6,           c5*s6, s5]
%[-s4*s5*c6-c4*s6, -s4*s5*s6+c4*c6, s4*c5]
%[-c4*s5*c6+s4*s6, -c4*s5*s6-s4*c6, c4*c5]

P(5) = asin(rang(R1(1,3)));
if (abs(P(5))-pi/2).^2 < 1e-9,
	P(4) = 0;
	P(6) = atan2(-rang(R1(2,1)), rang(-R1(3,1)/R1(1,3)));
else,
	c    = cos(P(5));
	P(4) = atan2(rang(R1(2,3)/c), rang(R1(3,3)/c));
	P(6) = atan2(rang(R1(1,2)/c), rang(R1(1,1)/c));
end;
trans = P(1:3);
rot = P(4:6);
scale = P(7:9);
skew = P(10:12);
return;

% There may be slight rounding errors making b>1 or b<-1.
function a = rang(b)
a = min(max(b, -1), 1);
return;


function y = logSt(X, mu, sigma, v)
% Compute log pdf of a Student's t distribution.
% Input:
%   X: d x n data matrix
%   mu: mean
%   sigma: variance
%   v: degree of freedom
% Output:
%   y: probability density in logrithm scale y=log p(x)
% Written by mo Chen (sth4nth@gmail.com).
[d,k] = size(mu);

if size(sigma,1)==d && size(sigma,2)==d && k==1
    [R,p]= chol(sigma);
    if p ~= 0
        error('ERROR: sigma is not SPD.');
    end
    X = bsxfun(@minus,X,mu);
    Q = R'\X;
    q = dot(Q,Q,1);  % quadratic term (M distance)
    o = -log(1+q/v)*((v+d)/2);
    c = gammaln((v+d)/2)-gammaln(v/2)-(d*log(v*pi)+2*sum(log(diag(R))))/2;
    y = c+o;
elseif size(sigma,1)==d && size(sigma,2)==k
    lambda = 1./sigma;
    ml = mu.*lambda;
    q = bsxfun(@plus,X'.^2*lambda-2*X'*ml,dot(mu,ml,1)); % M distance
    o = bsxfun(@times,log(1+bsxfun(@times,q,1./v)),-(v+d)/2);
    c = gammaln((v+d)/2)-gammaln(v/2)-(d*log(pi*v)+sum(log(sigma),1))/2;
    y = bsxfun(@plus,o,c);
elseif size(sigma,1)==1 && size(sigma,2)==k
    X2 = repmat(dot(X,X,1)',1,k);
    D = bsxfun(@plus,X2-2*X'*mu,dot(mu,mu,1));
    q = bsxfun(@times,D,1./sigma);  % M distance
    o = bsxfun(@times,log(1+bsxfun(@times,q,1./v)),-(v+d)/2);
    c = gammaln((v+d)/2)-gammaln(v/2)-d*log(pi*v.*sigma)/2;
    y = bsxfun(@plus,o,c);
else
    error('Parameters are mismatched.');
end


function P=ar1spectrum(ar1,period)
%%  AR1 power spectrum.
%
% power=ar1spectrum(ar1,period)
%
%
% (c) Aslak Grinsted 2002-2014
%

% -------------------------------------------------------------------------
%The MIT License (MIT)
%
%Copyright (c) 2014 Aslak Grinsted
%
%Permission is hereby granted, free of charge, to any person obtaining a copy
%of this software and associated documentation files (the "Software"), to deal
%in the Software without restriction, including without limitation the rights
%to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
%copies of the Software, and to permit persons to whom the Software is
%furnished to do so, subject to the following conditions:
%
%The above copyright notice and this permission notice shall be included in
%all copies or substantial portions of the Software.
%
%THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
%IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
%FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
%AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
%LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
%OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
%THE SOFTWARE.
%---------------------------------------------------------------------------

freq=1./period;
P=(1-ar1.^2)./(abs(1-ar1.*exp(-2*pi*i*freq))).^2; %http://www.madsci.org/posts/archives/may97/864012045.Eg.r.html
%fixed typo in numerical recipes


function pred_value = xval_simple_ols_loo(X, Y)
% Check: a very simple leave-one-out cross-validated regression
% pred_value = xval_simple_ols_loo(X, Y)
%
% pred_value: cross-validated predictions of outcome data
% X: n x variables matrix of predictors
% Y: n x 1 vector of outcomes
%
% Tor Wager, June 2010
%
% Go to any LASSO output directory and run this:
%
% maskInfo = iimg_read_img(fullfile(pwd, 'mask.img'), 2);
% dat{1} =  iimg_get_data(maskInfo, anticimages);
% pred_value = xval_simple_ols_loo(X, Y)

[N, k] = size(X);
pred_value = zeros(N, 1);

create_figure('test', 1, 2)

for i = 1:N

    % select training data
    Xi = X; Xi(i, :) = [];
    Yi = Y; Yi(i) = [];

    Xi = [ones(N-1, 1) Xi]; % add intercept

    % Make prediction
    b = pinv(Xi) * Yi;
    pred_value(i, 1) = [1 X(i, :)] * b;
     %pred_value(i, 1) = X(i, :) * b;

    create_figure('test', 1, 2, 1); subplot(1, 2, 1); plot(Xi*b, Yi, 'ko');
    plot(pred_value(i, 1), Y(i), 'ro', 'MarkerFaceColor', 'r');
    subplot(1, 2, 2);
   plot([ones(N, 1) X] * b, 'k');
   title('Black circles = training set; Red = holdout obs');
    %plot(X * b, 'k');
    drawnow;
    %pause(.1);

     %intercept_vals(i, 1) = b(1);
end

cm = colormap(jet(N));
figure; hold on;
for i = 1:N
    plot(pred_value(i), Y(i), 'ko', 'MarkerFaceColor', cm(i, :));
end
xlabel('Predicted outcome (xval)'); ylabel('Outcome');
title('Color = order in data series');

% figure; hold on;
% for i = 1:N
%     plot(intercept_vals(i), pred_value(i), 'ko', 'MarkerFaceColor', cm(i, :));
% end

%
% figure; hold on;
%     plot(pred_value(whorder(1:24)), Y(whorder(1:24)), 'bo', 'MarkerFaceColor', 'b');
% plot(pred_value(whorder(25:end)), Y(whorder(25:end)), 'ro', 'MarkerFaceColor', 'r');


function point_num = gp_se_size ( dim_num, level_max )

%*****************************************************************************80
%
%% GP_SE_SIZE: Gauss Patterson Slow Exponential Growth.
%
%  Discussion:
%
%    The Gauss Patterson Slow Exponential Growth family assumes that, for the
%    underlying 1D rules, a precision of 2*L+1 is needed at level L.  Therefore,
%    the lowest possible order Gauss-Patterson rule is chosen that will achieve
%    that precision.  This retains a combination of the advantages of
%    nestedness and high accuracy.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Output, integer POINT_NUM, the total number of unique
%    points in the grids.
%

%
%  Special case.
%
  if ( level_max < 0 )
    point_num = 0;
    return
  end

  if ( level_max == 0 )
    point_num = 1;
    return
  end
%
%  Count the points in the 1D rule.
%
  order_1d = zeros ( level_max+1, 1 );

  order_1d(0+1) = 1;
  for level = 1 : level_max
    p = 5;
    o = 3;
    while ( p < 2 * level + 1 )
      p = 2 * p + 1;
      o = 2 * o + 1;
    end
    order_1d(level+1) = o;
  end
%
%  Count the new points in the 1D rule.
%
  new_1d = zeros(level_max+1,1);

  new_1d(0+1) = 1;
  for level = 1 : level_max
    new_1d(level+1) = order_1d(level+1) - order_1d(level-1+1);
  end
%
%  Count the number of points by counting the number of new points
%  associated with each level vector.
%
  level_1d = zeros ( dim_num, 1 );

  point_num = 0;

  for level = 0 : level_max

    level_1d = [];
    more = 0;
    h = 0;
    t = 0;

    while ( 1 )

      [ level_1d, more, h, t ] = comp_next ( level, dim_num, level_1d, more, h, t );

      point_num = point_num + prod ( new_1d(level_1d(1:dim_num)+1) );

      if ( ~more )
        break
      end

    end

  end

  return
end


function quadrule_test06 ( )

%*****************************************************************************80
%
%% QUADRULE_TEST06 tests CHEBYSHEV2_COMPUTE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2009
%
%  Author:
%
%    John Burkardt
%
  order_max = 4;

  nfunc = func_set ( 'COUNT', 'DUMMY' );

  a = - 1.0;
  b =   1.0;
  nsub = 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUADRULE_TEST06\n' );
  fprintf ( 1, '  CHEBYSHEV2_COMPUTE sets up Gauss-Chebyshev type 2 quadrature;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Integration interval is [%f, %f]\n', a, b );
  fprintf ( 1, '  Number of subintervals is %d\n', nsub );
  fprintf ( 1, '  Quadrature order will vary.\n' );
  fprintf ( 1, '  Integrand will vary.\n' );
  fprintf ( 1, '  The weight function is sqrt ( 1 - X**2 )\n' );
  fprintf ( 1, '\n' );

  for ilo = 1 : 5 : nfunc

    ihi = min ( ilo + 4, nfunc );

    fprintf ( 1, '\n' );
    fprintf ( 1, '    ' );
    for i = ilo : ihi
      fprintf ( '%14s', fname(i) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '\n' );

    for order = 1 : order_max

      fprintf ( 1, '  %2d', order );

      for i = ilo : ihi

        func_set ( 'SET', i );

        [ xtab, weight ] = chebyshev2_compute ( order );

        result = 0.0;
        for j = 1 : order
          result = result + weight(j) * func ( xtab(j) );
        end

        fprintf ( 1, '  %12f', result );

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


function [ o, x, w ] = epn_glg_00_1 ( n, alpha )

%*****************************************************************************80
%
%% EPN_GLG_00_1 implements the "midpoint rule" for region EPN_GLG.
%
%  Discussion:
%
%    The rule has order O = 1.
%
%    The rule has precision P = 0.
%
%    EPN_GLG is the N-dimensional positive space [0,+oo)^N with generalized
%    Laguerre weight function:
%
%      w(alpha;x) = product ( 1 <= i <= n ) x(i)^alpha exp ( - x(i) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%
%    Input, real ALPHA, the exponent of X in the weight function.
%    -1.0 < ALPHA.
%
%    Input, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  if ( alpha <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EPN_GLG_00_1 - Fatal error!\n' );
    fprintf ( 1, '  ALPHA <= -1.0\n' );
    error ( 'EPN_GLG_00_1 - Fatal error!' );
  end

  expon = 0;
  volume = ep1_glg_monomial_integral ( expon, alpha );
  volume = volume ^ n;

  o = 1;

  x = zeros ( n, o );
  w = zeros ( o, 1 );

  k = 0;
%
%  1 point.
%
  k = k + 1;
  x(1:n,k) = 1.0;
  w(k) = volume;

  return
end


[x,y]=meshgrid([-3:0.2:3]);
z=(sin(x.*y)+eps)./(x.*y+eps);
surf(x,y,z)


function [ Pxx, Pyy, Pxy, coh, pha, freq ] = func_coherence( f1, f2, nfft, Fs, filt, n_overlap )

%
% Computing power spectrum, cross spectrum, coherence and phase
%
% All input parameters are equivalent to csd or the other spectrum
% related function in MATLAB.
%
% This program use csd.m in Signal Processing Toolbox.
%
% Input
%	f1 and f2	input data
%	nfft		number of data for FFT
%	Fs		sampling frequency
%	filt		filter vector, hanning(nfft/2)
%	n_overlap	number of overlap for smoothing
%
% Output
%	Pxx		spectrum of f1
%	Pyy		spectrum of f2
%	Pxy		cross spectrum between f1 and f2
%	coh		coherence
%	pha		phase
%	freq		frequency vector
%
%======================================================================
% Terms:
%
%       Distributed under the terms of the terms of the BSD License
%
% Copyright:
%
%       Nobuhito Mori
%           Disaster Prevention Research Institue
%           Kyoto University
%           mori@oceanwave.jp
%
%========================================================================

[Pxx,freq] = csd( f1, f1, nfft, Fs, filt, n_overlap );
[Pyy,freq] = csd( f2, f2, nfft, Fs, filt, n_overlap );
[Pxy,freq] = csd( f1, f2, nfft, Fs, filt, n_overlap );

Kxy  = real( Pxy );
Qxy  = imag( Pxy );
coh  = Pxy.*conj(Pxy)./(Pxx.*Pyy);
pha  = atan2( Qxy, Kxy );


function [] = test_l1_logistic_regression()

    clc;
    clear;
    close all;

    %rng('default');

    %% Set algorithms
    if 0
        algorithms = sgd_solver_list('ALL');
    else
        algorithms = {'SVRG', 'SAG', 'Adam', 'Reg-oBFGS-Lim','APG-BKT', 'APG-TFOCS-BKT'};
    end

    %% prepare dataset
    if 1
        % generate synthtic data
        d = 100;
        n = 1000;
        data = logistic_regression_data_generator(n, d);
        x_train = data.x_train;
        y_train = data.y_train;
        x_test = data.x_test;
        y_test = data.y_test;
        d = size(x_train,1);
        %w_opt = data.w_opt;
        lambda = 0.1;
    else
        % load pre-created synthetic data
        data = importdata('../data/logistic_regression/data_100d_10000.mat');
        x_train = data.x_train;
        y_train = data.y_train;
        x_test = data.x_test;
        y_test = data.y_test;
        d = size(x_train,1);
        n = length(y_train);
        w_opt = data.w_star;
        lambda = data.lambda;
    end

    %% define problem definitions
    problem = l1_logistic_regression(x_train, y_train, x_test, y_test, lambda);

    %% initialize
    w_init = rand(d,1);
    w_list = cell(length(algorithms),1);
    info_list = cell(length(algorithms),1);

    %% calculate solution
    %if norm(w_opt)
    %else
        % calculate solution
        options.w_init = w_init;
        options.max_iter = 100;
        w_opt = problem.calc_solution(options, 'ag');
    %end
    f_opt = problem.cost(w_opt);
    fprintf('f_opt: %.24e\n', f_opt);

    %% calculate solution
    f_opt = problem.cost(w_opt);
    fprintf('f_opt: %.24e\n', f_opt);

    %% perform algorithms
    for alg_idx=1:length(algorithms)
        fprintf('\n\n### [%02d] %s ###\n\n', alg_idx, algorithms{alg_idx});

        clear options;
        % general options for optimization algorithms
        options.w_init = w_init;
        options.tol_gnorm = 1e-10;
        options.max_iter = 100;
        options.max_epoch = 100;
        options.verbose = true;
        options.f_opt = f_opt;
        batch_size = 10;

        switch algorithms{alg_idx}
            case {'PG-BKT'}

                options.step_alg = 'backtracking';
                options.step_init_alg = 'bb_init';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'PG-TFOCS-BKT'}

                options.step_alg = 'tfocs_backtracking';
                options.step_init_alg = 'bb_init';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'APG-BKT'}

                options.step_alg = 'backtracking';
                options.step_init_alg = 'bb_init';
                [w_list{alg_idx}, info_list{alg_idx}] = ag(problem, options);

            case {'APG-TFOCS-BKT'}

                options.step_alg = 'tfocs_backtracking';
                options.step_init_alg = 'bb_init';
                [w_list{alg_idx}, info_list{alg_idx}] = ag(problem, options);

            case {'L-BFGS-BKT'}

                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'L-BFGS-WOLFE'}

                options.step_alg = 'strong_wolfe';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'L-BFGS-TFOCS'}

                options.step_alg = 'tfocs_backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'BFGS-TFOCS'}

                options.step_alg = 'tfocs_backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'Newton-CHOLESKY'}

                options.sub_mode = 'CHOLESKY';
                options.step_alg = 'backtracking';
                %options.step_alg = 'tfocs_backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = newton(problem, options);

            case {'NCG-BKT'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'backtracking';
                %options.step_alg = 'tfocs_backtracking';
                %options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = ncg(problem, options);

            case {'SGD'}

                options.batch_size = batch_size;
                options.step = 0.001 * options.batch_size;
                %options.step_alg = 'decay';
                options.step_alg = 'fix';

                [w_list{alg_idx}, info_list{alg_idx}] = sgd(problem, options);

            % Variance reduction (VR) varitns
            case {'SVRG'}

                options.batch_size = batch_size;
                options.step_init = 0.0001 * options.batch_size;
                options.step_alg = 'fix';

                [w_list{alg_idx}, info_list{alg_idx}] = svrg(problem, options);

            case {'SAG'}

                options.batch_size = batch_size;
                %options.step_init = 0.00005 * options.batch_size;
                options.step_init = 0.0001 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'SAG';

                [w_list{alg_idx}, info_list{alg_idx}] = sag(problem, options);

            case {'SAGA'}

                options.batch_size = batch_size;
                %options.step_init = 0.00005 * options.batch_size;
                options.step_init = 0.000001 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'SAGA';

                [w_list{alg_idx}, info_list{alg_idx}] = sag(problem, options);

            % AdaGrad variants
            case {'AdaGrad'}

                options.batch_size = batch_size;
                options.step_init = 0.02 * options.batch_size;
                options.step_alg = 'fix';
                options.epsilon = 0.00001;
                options.sub_mode = 'AdaGrad';

                [w_list{alg_idx}, info_list{alg_idx}] = adagrad(problem, options);

            case {'RMSProp'}

                options.batch_size = batch_size;
                options.step_init = 0.00001 * options.batch_size;
                options.step_alg = 'fix';
                options.epsilon = 0.00001;
                options.sub_mode = 'RMSProp';
                options.beta = 0.9;

                [w_list{alg_idx}, info_list{alg_idx}] = adagrad(problem, options);

            case {'AdaDelta'}

                options.batch_size = batch_size;
                options.step_init = 0.01 * options.batch_size;
                options.step_alg = 'fix';
                options.epsilon = 0.00001;

                options.sub_mode = 'AdaDelta';
                options.beta = 0.9;

                [w_list{alg_idx}, info_list{alg_idx}] = adagrad(problem, options);

            case {'Adam'}

                options.batch_size = batch_size;
                options.step_init = 0.00001 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'Adam';
                options.beta1 = 0.8;
                options.beta2 = 0.999;
                options.epsilon = 0.00001;

                [w_list{alg_idx}, info_list{alg_idx}] = adam(problem, options);

            case {'AdaMax'}

                options.batch_size = batch_size;
                options.step_init = 0.00001 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'AdaMax';
                options.beta1 = 0.8;
                options.beta2 = 0.999;
                options.epsilon = 0.00001;

                [w_list{alg_idx}, info_list{alg_idx}] = adam(problem, options);

            % Stochastic Quasi-Newton variants
            case {'SQN'}

                options.batch_size = batch_size;
                options.batch_hess_size = batch_size * 20;
                options.step_init = 0.001 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'SQN';
                options.L = 20;
                options.mem_size = 20;

                [w_list{alg_idx}, info_list{alg_idx}] = slbfgs(problem, options);

            case {'SVRG-SQN'}

                options.batch_size = batch_size;
                options.batch_hess_size = batch_size * 20;
                options.step_init = 0.01 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'SVRG-SQN';
                options.L = 20;
                options.mem_size = 20;

                [w_list{alg_idx}, info_list{alg_idx}] = slbfgs(problem, options);

            case {'SVRG-LBFGS'}

                options.batch_size = batch_size;
                options.batch_hess_size = batch_size * 20;
                options.step_init = 0.01 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'SVRG-LBFGS';
                options.mem_size = 20;

                [w_list{alg_idx}, info_list{alg_idx}] = slbfgs(problem, options);

            case {'SS-SVRG'}

                options.batch_size = batch_size;
                options.batch_hess_size = batch_size * 20;
                options.step_init = 0.0005 * options.batch_size;
                options.step_alg = 'fix';
                r = d-1;
                if r < 1
                    r = 1;
                end
                options.r = r;

                [w_list{alg_idx}, info_list{alg_idx}] = subsamp_svrg(problem, options);

            case {'oBFGS-Inf'}

                options.batch_size = batch_size;
                options.step_init = 0.0001 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'Inf-mem';
                options.regularized = false;

                [w_list{alg_idx}, info_list{alg_idx}] = obfgs(problem, options);

            case {'oBFGS-Lim'}

                options.batch_size = batch_size;
                options.step_init = 0.00001 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'Lim-mem';
                options.mem_size = 20;
                options.regularized = false;

                [w_list{alg_idx}, info_list{alg_idx}] = obfgs(problem, options);

            case {'Reg-oBFGS-Inf'}

                options.batch_size = batch_size;
                options.step_init = 0.0001 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'Inf-mem';
                options.regularized = true;
                options.delta = 0.1;

                [w_list{alg_idx}, info_list{alg_idx}] = obfgs(problem, options);

            case {'Reg-oBFGS-Lim'}

                options.batch_size = batch_size;
                options.step_init = 0.0001 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'Lim-mem';
                options.mem_size = 20;
                options.regularized = true;
                options.delta = 0.1;

                [w_list{alg_idx}, info_list{alg_idx}] = obfgs(problem, options);

            case {'Damp-oBFGS-Inf'} % SDBFGS

                options.batch_size = batch_size;
                options.step_init = 0.0001 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'Inf-mem';
                options.regularized = true;
                options.delta = 0.1;
                options.damped = true;

                [w_list{alg_idx}, info_list{alg_idx}] = obfgs(problem, options);

            case {'Damp-oBFGS-Lim'}

                options.batch_size = batch_size;
                options.step_init = 0.01 * options.batch_size;
                options.step_alg = 'fix';
                options.sub_mode = 'Lim-mem';
                options.regularized = true;
                options.delta = 0.1;
                options.damped = true;

                [w_list{alg_idx}, info_list{alg_idx}] = obfgs(problem, options);

           case {'IQN'}

                options.w_init = w_init;
                options.step_init = 1;
                options.step_alg = 'fix';

                [w_list{alg_idx}, info_list{alg_idx}] = iqn(problem, options);

            otherwise
                warn_str = [algorithms{alg_idx}, ' is not supported.'];
                warning(warn_str);
                w_list{alg_idx} = '';
                info_list{alg_idx} = '';
        end

    end

    fprintf('\n\n');

    %% plot all
    close all;

    % display iter vs cost/gnorm
    display_graph('iter','cost', algorithms, w_list, info_list);
    % display iter vs. l1 norm, i.e. the toral number of non-zero elements
    display_graph('iter','reg', algorithms, w_list, info_list);

    display_graph('grad_calc_count','optimality_gap', algorithms, w_list, info_list);

end