function [soln,eqn,info] = StokesRT0(node,elem,bdFlag,pde,option)
%% STOKESRT0 Stokes equations: the lowest order Raviart-Thomas element in 2D.
%
%  [soln,eqn,info] = StokesRT0(node,elem,bdFlag,pde,option)
%  uses the lowest order of Raviart-Thomas element (RT0) to approximate
%  velocity u and piecewise constant element (P0)  to approximate pressure
%  p, repectively.
%
%  We solve the following equation:
%       - grad div u + curl rot u + grad p  = f   in \Omega
%                                  - div u  = 0   in \Omega
%                                        u  = g   on \Gamma
%
% ifem StokesRT0doc
%
%  See also StokesBDM1B
%
% Based on a version by Ming Wang. Revised by Lin Zhong. Discussed with Jie
% Zhou and Long Chen. Further clean up by Long Chen.
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end

%% Data structure
elemunSort = elem;
[elem,bdFlag] = sortelem(elemunSort,bdFlag);
[elem2edge,edge] = dofedge(elem);
[Clambda,area,elemSign] = curlbasis(node,elem);
N = size(node,1); NT = size(elem,1); NE = size(edge,1);
Nu = NE; Np = NT; Ndof = Nu + Np;

%% Assemble matrices
t = cputime; % record assemble time
% Mv: Lumped mass matrix for vertex: P1 element
vecMv = accumarray([elem(:,1);elem(:,2);elem(:,3)],[area;area;area]/3,[N,1]);
% invMv = spdiags(1./vecMv,0,N,N);
Mv = spdiags(vecMv,0,N,N);

% Me: Mass matrix for RT0 element
Me = getmassmatvec(elem2edge,area,Clambda,'RT0');

%invMt: the inverse of Mass matrix for P0 element
invMt = spdiags(1./area,0,NT,NT);

% B: negative divergence operator
B = -icdmat(double(elem2edge),elemSign*[1 -1 1]);

% C: curl operator
C = icdmat(double(edge),[-1 1]);

% R: weak rot operator
R = spdiags(1./vecMv,0,N,N)*C'*Me;

% Vector Laplacian
A = B'*invMt*B + R'*Mv*R;

%% Assemble right hand side
locEdge = [2,3; 1 3; 1,2]; % ascend ordering
fu = zeros(Nu,1);% the right hand side of u
g = zeros(Np,1); % the right hand side of p
if ~isfield(pde,'f') || (isfield(pde,'f') && isreal(pde.f) && all(pde.f==0))
    pde.f = [];
end
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order is 3
end
if isfield(pde,'f') && ~isempty(pde.f)
    [lambda,w] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
    bt = zeros(NT,3);
    for p = 1:nQuad
        % quadrature points in the x-y-z coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        fp = pde.f(pxy);
        for k = 1:3
            i = locEdge(k,1); j = locEdge(k,2);
            % phi_k = lambda_iClambda_j - lambda_jClambda_i;
            phi_k = lambda(p,i)*Clambda(:,:,j)-lambda(p,j)*Clambda(:,:,i);
            rhs = dot(phi_k,fp,2);
            bt(:,k) = bt(:,k) + w(p)*rhs;
        end
    end
    bt = bt.*repmat(area,1,3);
    fu = accumarray(elem2edge(:),bt(:),[Nu 1]);
end
clear pxy fp bt rhs phi_k psi_k

%% Boundary condition and graddiv part of A
[u,p,ufreeDof,pDof,utbd] = getbdStokesRT0;
assembleTime = cputime - t;

%% Solve the system of linear equations
% set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 1e5  % Direct solver for small size systems
        solver = 'direct';
    else             % Multigrid-type  solver for large size systems
        solver = 'mg';
    end
else
    solver = option.solver;
end
% solve the system
% get submatrices of ufreeDof
A0 = A(ufreeDof,ufreeDof);
B0 = B(:,ufreeDof);
f0 = fu(ufreeDof);
g0 = g;
if strcmp(solver,'direct') && ~isempty(ufreeDof)
    t = cputime;
    bigA = [A0, B0'; ...
            B0, sparse(Np,Np)];
    bigF = [f0; g0];
    bigu = [u; p];
    bigFreeDof = [ufreeDof; Nu+pDof];
    bigu(bigFreeDof) = bigA(1:end-1,1:end-1)\bigF(1:end-1);
    u = bigu(1:Nu);
    p = bigu(Nu+1:end);
    info.solverTime = cputime - t;
elseif strcmp(solver,'mg')
%   option.solver = 'vcycle';
    option.solver  = 'WCYCLE';
    [u(ufreeDof),p,info] = mgstokesRT0(A0,B0,f0,g0,u,p,node,elemunSort,ufreeDof,option);
end

%% Post-process
if length(pDof)~=Np % p is unique up to a constant
    % impose the condition int(p)=0
    c = sum(p.*area)/sum(area);
    p = p - c;
end
w = R*u + utbd./vecMv;
psi = zeros(N,1);
% compute streamline function
if isfield(option,'stream') && option.stream
    [As,Ms] = assemblematrix(node,elem);
    [fixedNode,bdEdge,isBdNode] = findboundary(elem,bdFlag);
    freeNode = ~isBdNode;
    rhs = Ms*w;
    streamoption.freeDof = freeNode;
    psi(freeNode) = mg(As(freeNode,freeNode),rhs(freeNode),elemunSort,streamoption);
    % assume u\cdot n = 0.
end

%% Output information
info.assembleTime = assembleTime;

%% Output
soln = struct('u',u,'p',p,'w',w,'psi',psi);
eqn = struct('A',A0,'B',B0,'Me',Me,'Mv',Mv,'f',f0,'g',g0,...
             'edge',edge,'ufreeDof',ufreeDof,'pDof',pDof);
info.assembleTime = assembleTime;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbdStokesRT0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [u,p,ufreeDof,pDof,utbd] = getbdStokesRT0
        %% Boundary condition of Stokes equation: RT0-P0 elements

        % Initial set up
        utbd = zeros(N,1); % the line integral of u on the boundary (u.t, tau)|_\partial \Omega
        u = zeros(Nu,1);
        p = zeros(Np,1);
        ufreeDof = (1:Nu)';
        pDof = (1:Np-1)';

        if ~exist('bdFlag','var'), bdFlag = []; end
        if ~isfield(pde,'g_D'), pde.g_D = []; end
        if ~isfield(pde,'g_N'), pde.g_N = []; end
        if ~isfield(pde,'g_R'), pde.g_R = []; end
        if isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
            bdFlag = [];
        end

        % Find Dirichlet boundary dof: fixedDof and pDof
        isFixedDof = false(Nu,1);
        if ~isempty(bdFlag)
            isDirichlet(elem2edge(bdFlag(:)==1)) = true;
            Dirichlet = edge(isDirichlet,:);
            isFixedDof(isDirichlet) = true;
            fixedDof = find(isFixedDof);
            ufreeDof = find(~isFixedDof);
        end

        % Set up edge sign
        % edgeSign records the inconsistency of asecond orientation and
        % induced orientation for each boundary edges
        edgeSign = ones(NE,1);
        idx = (bdFlag(:,1) ~= 0 ) & (elemSign == -1) ; % the first edge is on boundary
        edgeSign(elem2edge(idx,1)) = -1;
        idx = (bdFlag(:,2) ~= 0 ) & (elemSign ==  1) ; % the second edge is on boundary
        edgeSign(elem2edge(idx,2)) = -1;
        idx = (bdFlag(:,3) ~= 0 ) & (elemSign == -1) ; % the third edge is on boundary
        edgeSign(elem2edge(idx,3)) = -1;

        % Compute the boundary integral
        if ~isempty(fixedDof) && ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && (pde.g_D == 0))
            % else no bddof or g_D = 0 (no modification needed)
            % 1. Normal component of u is imposed strongly
            if (isnumeric(pde.g_D) && length(pde.g_D) == NE)
                u(fixedDof) = pde.g_D(fixedDof);
            else
                u(fixedDof) = faceinterpolate(pde.g_D,node,edge(fixedDof,:),'RT0');
            end
            % 2. Tangential component of u is imposed weakly
            [lambdagD,wgD] = quadpts1(3);
            nQuadgD = size(lambdagD,1);
            % quadrat = cputime bases 1--3--2
            bdphi = lambdagD;
            ve = node(Dirichlet(:,2),:) - node(Dirichlet(:,1),:);
            ge = zeros(size(Dirichlet,1),2);
            int_left = zeros(size(Dirichlet,1),2);
            int_right = zeros(size(Dirichlet,1),2);
            for pp = 1:nQuadgD
                ppxy = lambdagD(pp,1)*node(Dirichlet(:,1),:) ...
                     + lambdagD(pp,2)*node(Dirichlet(:,2),:);
                gDp = pde.g_D(ppxy);
                int_left = int_left + wgD(pp)*gDp*bdphi(pp,1);
                int_right = int_right + wgD(pp)*gDp*bdphi(pp,2);
            end
            ge(:,1) = dot(int_left,ve,2).*edgeSign(fixedDof);
            ge(:,2) = dot(int_right,ve,2).*edgeSign(fixedDof);
            utbd = accumarray(Dirichlet(:), [ge(:,1); ge(:,2)],[N,1]);
        end

        % Modify the right hand side
        fu = fu - A*u - Me*(C*(utbd./vecMv));
         g = g - B*u;
         g = g - mean(g);
    end
end


function [prob]=mhprob3(cand,lambda,sbar,eps,Finv,n);

% compute the first part of the first exponential term
temp1=exp(-cand)-exp(-lambda);

% compute the second part of the first exponential term
% initiate the summation
temp2=0;
% loop over variables
   for jj=1:n
      % if jj=1 (first variable), the part finv*eps_i,t does not exist
      if jj==1
      temp3=(1/sbar(jj,1))*eps(1,1)^2;
      % if any other variable is considered, the term finv*eps_i,t must be taken into account
      else
      temp3=(1/sbar(jj,1))*(eps(1,1)+Finv{jj,1}'*eps(1:jj-1,1))^2;
      end
   % increment the summation
   temp2=temp2+temp3;
   end
% compute the (log of the) first exponential term
term1=-0.5*temp1*temp2;

% next compute the (log of the) second exponential term
term2=-(n/2)*(cand-lambda);

% compute the sum of the two terms to obtain the log of the acceptance prob
% and exponentiate it to obtain the actual acceptance prob
prob=min(1,exp(term1+term2));


function [res] = plotBeamformer(M, f, d, bfFilter, theta)
    c = 340;
    w = bfFilter;       % beamformer
    p = zeros(length(theta), length(f));
    for  j=1:length(theta)                    %scan angles
        a=exp(-1j*[0:M-1]'*2*pi*f*cos(theta(j))*d/c);
        p(j, :) = sum(w.*a);
    end
   % p=p';
    res = p;

    if length(f) == 1
        subplot(1, 2, 1);
        plot(theta/pi*180, abs(p.')); grid on;
        xlabel('Degree')
        subplot(1, 2, 2);
        polar(theta,abs(p.'))
    else
        subplot(1, 2, 1);
        mesh(f, theta/pi*180, abs(p))
        xlabel('Frequency(Hz)')
        ylabel('Degree')
        title('Beamformer Directivity')
        subplot(1, 2, 2);
        imagesc(abs(p).');
    end

end


function x_extract = extractsolution(momentdata,options)
%EXTRACTSOLUTIONS Tries to extract solutions from moment matrices
%
%
%  xoptimal = extractsolution(momentstructure)
%
%   xoptimal    : Extracted solutions
%
%   momentdata : Problem data, obtained from SOLVEMOMENT (or SOLVESDP)
%   options    : Options structure from SDPSETTINGS
%
%   See also SOLVEMOMENT, SDPSETTINGS

moment = momentdata.moment;
x = momentdata.x;
monomials = momentdata.monomials;
n = momentdata.n;
d = momentdata.d;

[U,S,V,ranks] = numranks(moment);

if options.moment.extractrank>0
    % We try a extraction from highest order moment no matter what
    flat = length(moment);
    ranks(flat) = options.moment.extractrank;
else
    % Find a flat extension
    flat = d+min(find(ranks(1+d:end)-ranks(1:end-d)==0));
end

if ~isempty(flat)

    % Find a basis
    r = ranks(flat);
    V = U{flat}(:,1:r)*sqrt(diag(diag(S{flat}(1:r,1:r))));
    if options.moment.rceftol >= 0
        [R,pivot] = rref(V',options.moment.rceftol);R = R';
    else
        % Try to find a reasonable tolerance by avoiding severly badly
        % conditioned R. Hack.., but seem to behave rather robustly.
        cV = cond(V);
        tol = 1e-10;
        [R,pivot] = rref(V',tol);R = R';
        while tol<1 & (cond(R)/cond(cV)>1e4)
            tol = tol*5;
            [R,pivot] = rref(V',tol);R = R';
        end
    end

    % Figure out multiplying matrices using YALMIP code
    w = monomials(pivot);
    for i = 1:n
        xw = x(i)*w;
        k = [];
        for j = 1:length(xw)
            k = [k;find(ismember(xw(j),monomials))];
        end
        N{i} = R(k,:);
    end

    % Things missing in the basis...
    if ~all(cellfun('prodofsize',N)==length(w)^2)
       x_extract = {[]};
       return;
    end

    % Create random convex combination
    rands = rand(n,1);rands = rands/sum(rands);
    M = 0;
    for i = 1:n
        M = M + rands(i)*N{i};
    end

    [Q,T] = schur(M);
    % Extract solution
    for i = 1:r
        for j = 1:n
            x_extract{i}(j,1) =  Q(:,i)'*N{j}*Q(:,i);
        end
    end

    % Refine solutions v-Rw = e(x)=0
    if options.moment.refine>0
        xtemp = double(x);
        e = monomials(1:size(R,1))-R*w;
        dedx = jacobian(e,x);
        for j = 1:r
            assign(x,x_extract{j});
            for i = 1:options.moment.refine
                assign(x,double(x)-double(dedx)\double(e));
            end
            x_extract{j} = double(x);
        end
        assign(x,xtemp);
    end

else
    x_extract = {};
end

% Somewhat more stable rank detection
% looking for sharp drops in singular value
function [U,S,V,ranks] = numranks(moment)
for i = 1:length(moment)
    [U{i},S{i},V{i}] = svd(moment{i});
    s = diag(S{i});
    decay = s(2:end)./(eps+s(1:end-1));
    r = min(find(decay<1e-3));
    if isempty(r)
        ranks(i) = rank(moment{i},1e-8);
    else
        ranks(i) = min(r,rank(moment{i},1e-8));
    end
end


function w = dualtree3D(x, J, Faf, af)

% 3D Dual-Tree Discrete Wavelet Transform
%
% USAGE:
%   w = dualtree3D(x, J, Faf, af)
% INPUT:
%   x - 3-D array
%   J - number of stages
%   Faf - first stage filters
%   af - filters for remaining stages
% OUPUT:
%   w{j}{i}{d} - wavelet coefficients
%        j = 1..J, i = 1..4, d = 1..7
%   w{J+1}{i} - lowpass coefficients
%        i = 1..4
% EXAMPLE:
%   x = rand(64,64,64);
%   J = 3;
%   [Faf, Fsf] = FSfarras;
%   [af, sf] = dualfilt1;
%   w = dualtree3D(x, J, Faf, af);
%   y = idualtree3D(w, J, Fsf, sf);
%   err = x - y;
%   max(max(max(abs(err))))
%
% WAVELET SOFTWARE AT POLYTECHNIC UNIVERSITY, BROOKLYN, NY
% http://taco.poly.edu/WaveletSoftware/

% normalization
x = x/2;

M = [
    1 1 1
    2 2 1
    2 1 2
    1 2 2
];

for i = 1:4
    f1 = M(i,1);
    f2 = M(i,2);
    f3 = M(i,3);
    [xi w{1}{i}] = afb3D(x, Faf{f1}, Faf{f2}, Faf{f3});
    for k = 2:J
        [xi w{k}{i}] = afb3D(xi, af{f1}, af{f2}, af{f3});
    end
    w{J+1}{i} = xi;
end

for k = 1:J
    for m = 1:7
        [w{k}{1}{m} w{k}{2}{m} w{k}{3}{m} w{k}{4}{m}] = ...
            pm4(w{k}{1}{m}, w{k}{2}{m}, w{k}{3}{m}, w{k}{4}{m});
    end
end


function centroids = kMeansInitCentroids(X, K)
%KMEANSINITCENTROIDS This function initializes K centroids that are to be
%used in K-Means on the dataset X
%   centroids = KMEANSINITCENTROIDS(X, K) returns K initial centroids to be
%   used with the K-Means on the dataset X
%

% You should return this values correctly
centroids = zeros(K, size(X, 2));

% ====================== YOUR CODE HERE ======================
% Instructions: You should set centroids to randomly chosen examples from
%               the dataset X
%

for i = 1:K
    centroids(i, :) = X(randperm(length(X) ,1), :);
end

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

end


function test_ft_plot_topo

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_plot_topo

x = randn(30,1);
y = randn(30,1);

% make something round between 0 and 1
val = -sqrt(x.^2 + y.^2);
val = val-min(val);
val = val./max(val);
isolines = 0:.1:1;

% 1 default style (surfiso) (without isolines is equal to style surf)
figure
ft_plot_topo(x, y, val);
hold on; plot(x, y, 'k*')
axis tight

% 2 style surfiso shading interp
figure
ft_plot_topo(x, y, val, 'style', 'surfiso', 'shading', 'interp');
hold on; plot(x, y, 'k*')
axis tight

% 3 default style (surfiso) with isolines
figure
ft_plot_topo(x, y, val, 'isolines', isolines);
hold on; plot(x, y, 'k*')
axis tight

% 4 style isofill with isolines
figure
ft_plot_topo(x, y, val, 'style', 'isofill', 'isolines', isolines);
hold on; plot(x, y, 'k*')
axis tight

%%% MASKING
% 5 style surf with binary mask
figure
ft_plot_topo(x, y, val, 'datmask', val>0.5, 'style', 'surf', 'clim', [0 1]);
hold on; plot(x, y, 'k*')
axis tight

% 6 style imsat with binary mask
figure
ft_plot_topo(x, y, val, 'datmask', val>0.5, 'style', 'imsat', 'clim', [0 1]);
hold on; plot(x, y, 'k*')
axis tight

% 7 style surf with continuous mask  (part of image will always be masked out because val is always 0 in one spot)
figure
ft_plot_topo(x, y, val, 'datmask', val, 'style', 'surf', 'clim', [0 1]);
hold on; plot(x, y, 'k*')
axis tight

% 8 style imsat with continuous mask  (part of image will always be masked out because val is always 0 in one spot)
figure
ft_plot_topo(x, y, val, 'datmask', val, 'style', 'imsat', 'clim', [0 1]);
hold on; plot(x, y, 'k*')
axis tight

% 9 style imsatiso with continuous mask and isolines (part of image will always be masked out because val is always 0 in one spot)
figure
ft_plot_topo(x, y, val, 'datmask', val, 'style', 'imsatiso', 'clim', [0 1], 'isolines', isolines);
hold on; plot(x, y, 'k*')
axis tight


function [connections]=findConnectionMatrix(mesh)
% Finds the sparse connection matrix for a mesh
%
%  connections=findConnectionMatrix(mesh)
%
% The mesh slots must be:
%     .uniqueVertices (nPoints,3)     : 3D coords of all mesh vertices
%     .uniqueFaceIndexList (nFaces,3) : triplets of indices specifies one corner of a face triangle
%
% The returned connections is a sparse matrix (nPoints*nPoints)
%
% AUTHOR: Wade
% DATE : Last modified 020701

nVerts=length(mesh.uniqueVertices);

% faceIndexList defines vertices that are connected to each other:
% If two vertices are in the same row, they're in the same triangle and
% therefore are connected

% The values here are going to be used to create scalars
% (connection_locations) that index into the connections matrix.
% The multiplication by nVerts makes it possible to add together
% a row and column value (see below).
%
r1=(mesh.uniqueFaceIndexList(:,1)-1)*nVerts;
r2=(mesh.uniqueFaceIndexList(:,2)-1);

r3=(mesh.uniqueFaceIndexList(:,3)-1);
r4=(mesh.uniqueFaceIndexList(:,2)-1)*nVerts;

% Calculate the connection_locations
% These are connections between (1,2), (1,3), and (2,3)
%
connect_locations=[ (r1+r2+1) ;(r1+r3+1); (r4+r3+1) ];

[cly,clx]=ind2sub([nVerts,nVerts],connect_locations);
connections = sparse(cly,clx,1,nVerts,nVerts);

%Now make it symmetric because if a is connected to b then b is connected to a.
connections=connections+(connections');
connections=(connections~=0);

return;


function W_nullpwd = sphNullformer_pwd(order, beam_dirs)
%SPHNULLFORMER_PWD Beamweights for a PWD beamformer at a specified direction,
%with nulls at others
%
%   For a set of K directions, computes the beamforming weights that creates
%   a PWD beamformer at each of the direction in the set, while placing
%   nulls at the rest.
%
%   Inputs:
%       order:  order of SH signals
%       beam_dirs:  Kx2 [azi elev] directions
%
%   Outputs:
%       W_nullpwd:  (order+1)^2xK matrix of beamweights, one set per
%           column for each beamforming direction
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPHNULLFORMER_PWD.M - 5/10/2016
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    beam_dirs2 = [beam_dirs(:,1) pi/2-beam_dirs(:,2)]; % convert from azi-elev to azi-incl

    % steering vectors in the SHD
    Y_nullpwd = getSH(order, beam_dirs2, 'real');

    % beamform at each direction and nullform at the rest
    W_nullpwd = pinv(Y_nullpwd);
end


function [c] = kron(a,b)
%Kronecker product of two QTT_Tuckers
%   [C]=KRON(A,B) computes Kronecker product of A and B, where A and B are
%   in the QTT-Tucker format
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

if ( isempty(a) )
  c=b;
  return
elseif ( isempty(b) )
  c=a;
  return
end

c = qtt_tucker;
c.dphys = a.dphys+b.dphys;
c.tuck = cell(c.dphys, 1);
c.core = kron(a.core, b.core);
c.tuck(1:a.dphys) = a.tuck;
c.tuck(a.dphys+1:c.dphys) = b.tuck;

end


function [ output_args ] = test_gmm_ex( input_args )
%TEST_GMM_HU_EX Summary of this function goes here
%   Detailed explanation goes here
close all;

dataset = '7';
[endmembers,I,Y,R_gt,A_gt,names,wl] = prepare_supervised_unmixing(dataset);
[rows,cols,B] = size(I);

% noise covariance matrix. it can be zero.
D = 0.001^2 * eye(B);

% smoothness and sparsity constraints on the abundances
options.beta1 = 0;
options.beta2 = 0;

% show intermediate results (1) or not (0)
options.show_fig = 1;

options.names = names;
options.D = D;

% project_mode can be
%   'image' - apply PCA on the pixels of the image
%   'endmembers' - concatenate the spectra in the library and use PCA on them
options.project_mode = 'image';

% threshold of convergence
options.convergence_thresh = 0.0001;

% calculate abundance maps
[A,R,w_jk,mu_jk,sigma_jk,extra] = gmm_hu_ex(I, endmembers, options);

% calculate endmembers per pixel
E = gmm_hu_endmember(I,A,D,w_jk,mu_jk,sigma_jk);

save('result_gmm.mat','A','R','E','w_jk','mu_jk','sigma_jk');

mdiff(A,A_gt);
show_abundances(A,rows,cols);
replay_scatter_abund(extra.frames_scatter, extra.frames_abund);

end


function [cj,ier]=nufft2d2(nj,xj,yj,iflag,eps,ms,mt,fk)
%NUFFT2D2: Nonuniform FFT in R^2 - Type 2.
%
%  [CJ,IER] = NUFFT2D2(NJ,XJ,YJ,IFLAG,EPS,MS,MT,FK);
%
%     cj(j) = SUM   fk(k1,k2) exp(+/-i k1 xj(j)) exp(+/-i k2 yj(j))
%             k1,k2
%                            for j = 1,...,nj
%
%     where -ms/2 <= k1 <= (ms-1)/2, -mt/2 <= k2 <= (mt-1)/2
%
%     If (iflag .ge.0) the + sign is used in the exponential.
%     If (iflag .lt.0) the - sign is used in the exponential.
%
%  Input parameters:
%
%     nj     number of output values   (integer)
%     xj,yj  location of output values (real *8 array)
%     iflag  determines sign of FFT (see above)
%     eps    precision request  (between 1.0e-15 and 1.0e-1)
%     ms     number of Fourier modes given  [ -ms/2: (ms-1)/2 ]
%     mt     number of Fourier modes given  [ -mt/2: (mt-1)/2 ]
%     fk     Fourier coefficient values (complex *16 array)
%
%  Output parameters:
%
%     cj     output values (complex *16 array)
%     ier    error return code
%            ier = 0  => normal execution.
%            ier = 1  => precision eps requested is out of range.
%
%

cj=zeros(nj,1)+1i*zeros(nj,1);
ier=0;

mex_id_ = 'nufft2d2f90(i int[x], i double[], i double[], io dcomplex[], i int[x], i double[x], i int[x], i int[x], i dcomplex[], io int[x])';
[cj, ier] = nufft2d(mex_id_, nj, xj, yj, cj, iflag, eps, ms, mt, fk, ier, 1, 1, 1, 1, 1, 1);


%% Compute error statistics on groundtruth vs NovAtel
distance = ((ground_truth(:,1)-gps_x).^2 + (ground_truth(:,2)-gps_y).^2).^0.5;
raw_rms = rms(distance);
raw_max = max(distance);
fprintf('\nRMS 2D Raw: %0.4fm\n',raw_rms)
fprintf('Max 2D Raw: %0.4fm\n',raw_max)

%% Compute error statistics on groundtruth vs filter output
xyz = out_profile(:,2:4);
llh = ecef2lla(xyz);
[x,y] = deg2utm(llh(:,1),llh(:,2));
x = x-min_x;
y = y-min_y;
h = -llh(:,3);

distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;
filter_rms = rms(distance);
filter_max = max(distance);
fprintf('RMS 2D Filter: %0.4fm, %0.2f%% change\n',filter_rms,(raw_rms-filter_rms)/raw_rms*100);
fprintf('Max 2D Filter: %0.4fm, %0.2f%% change\n',filter_max,(raw_max-filter_max)/raw_max*100);

%% 3D Stats
distance = ((ground_truth(:,1)-gps_x).^2 + (ground_truth(:,2)-gps_y).^2 + (ground_truth(:,3)-gps_h).^2).^0.5;
raw_rms = rms(distance);
raw_max = max(distance);
fprintf('\nRMS 3D Raw: %0.4fm\n',raw_rms)
fprintf('Max 3D Raw: %0.4fm\n',raw_max)
distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2 + (ground_truth_full(:,3)-h).^2).^0.5;
filter_rms = rms(distance);
filter_max = max(distance);
fprintf('RMS 3D Filter: %0.4fm, %0.2f%% change\n',filter_rms,(raw_rms-filter_rms)/raw_rms*100);
fprintf('Max 3D Filter: %0.4fm, %0.2f%% change\n',filter_max,(raw_max-filter_max)/raw_max*100);

%% Rotation stats


function [mat, dim] = spm_get_matdim(img, vx, bb)
% Voxel-to-world matrix and image dimensions from image or bbox and vox-dim
%
% FORMAT [mat, dim] = spm_get_matdim(img, vx, bb)
%
% img - filename of image to use as reference (defaults to SPM's TPM.nii)
% vx  - [1 x 3] vector of voxel dimensions (mm).
% bb  - [2 x 3] array of the min and max X, Y, and Z coordinates (mm),
%       i.e. bb = [minX minY minZ; maxX maxY maxZ].
%
% mat - [4 x 4] matrix mapping voxel coordinates to world (mm) coordinates
% dim - [1 x 3] vector of image dimensions (number of voxels)
%       (both as in output from spm_vol)
%
%       Note that the output mat will correspond to the same orientation
%       as SPM's canonical templates (transverse and vx(1) forced negative)
%       if either or both bb and vx are specified (finite), but otherwise
%       will keep the orientation of the reference image.
%__________________________________________________________________________
% Copyright (C) 2013 Wellcome Trust Centre for Neuroimaging

% Ged Ridgway
% $Id: spm_get_matdim.m 5374 2013-03-29 17:26:24Z ged $

if nargin < 3, vx  = nan(1, 3); end
if nargin < 2, bb  = nan(2, 3); end
if nargin < 1, img = '';        end

% Use MNI space by default, based on tissue priors
try
    vol = spm_vol(img);
    if isempty(vol), error('Failed to read volume %s', img), end
catch
    vol = spm_vol(fullfile(spm('dir'), 'tpm', 'TPM.nii'));
end
mat = vol(1).mat;
dim = vol(1).dim;

valid_bb = all(isfinite(bb(:)));
valid_vx = all(isfinite(vx));
if ~valid_bb && ~valid_vx
    return
end

% User has specified one or both of bb or vx, over-ride appropriately
[BB VX] = spm_get_bbox(vol(1));
if ~valid_bb, bb = BB; end
if ~valid_vx, vx = VX; end

% Determine mat and dim from bb and vx, assuming "canonical" orientation
% (i.e. transverse with negative first voxel dimension)
vx  = [-1 1 1] .* abs(vx);
mn  = vx .* min(bb ./ repmat(vx, 2, 1)); % "first" voxel's mm coordinates
mx  = vx .* round(max(bb ./ repmat(vx, 2, 1))); % "last voxel's mm coords
% matrix that maps voxel [1 1 1] to mn
mat = spm_matrix([mn 0 0 0 vx]) * spm_matrix([-1 -1 -1]);
% dim such that mat * [dim 1]' == [mx 1]'
dim = mat \ [mx 1]';
dim = round(dim(1:3)');


function [ value, ifault ] = ppnd ( p )

%*****************************************************************************80
%
%% PPND produces the normal deviate value corresponding to lower tail area = P.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by J Beasley, S Springer.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    J Beasley, S Springer,
%    Algorithm AS 111:
%    The Percentage Points of the Normal Distribution,
%    Applied Statistics,
%    Volume 26, Number 1, 1977, pages 118-121.
%
%  Parameters:
%
%    Input, real P, the value of the cumulative probability
%    densitity function.  0 < P < 1.
%
%    Output, real VALUE, the normal deviate value with the property that
%    the probability of a standard normal deviate being less than or
%    equal to PPND is P.
%
%    Output, integer IFAULT, error flag.
%    0, no error.
%    1, P <= 0 or P >= 1.  PPND is returned as 0.
%
  a0 = 2.50662823884;
  a1 = -18.61500062529;
  a2 = 41.39119773534;
  a3 = -25.44106049637;
  b1 = -8.47351093090;
  b2 = 23.08336743743;
  b3 = -21.06224101826;
  b4 = 3.13082909833;
  c0 = -2.78718931138;
  c1 = -2.29796479134;
  c2 = 4.85014127135;
  c3 = 2.32121276858;
  d1 = 3.54388924762;
  d2 = 1.63706781897;
  split = 0.42;

  ifault = 0;
%
%  0.08 < P < 0.92
%
  if ( abs ( p - 0.5 ) <= split )

    r = ( p - 0.5 ) * ( p - 0.5 );

    value = ( p - 0.5 ) * ( ( ( ...
        a3   * r ...
      + a2 ) * r ...
      + a1 ) * r ...
      + a0 ) / ( ( ( ( ...
        b4   * r ...
      + b3 ) * r ...
      + b2 ) * r ...
      + b1 ) * r ...
      + 1.0 );
%
%  P < 0.08 or P > 0.92,
%  R = min ( P, 1-P )
%
  elseif ( 0.0 < p && p < 1.0 )

    if ( 0.5 < p )
      r = sqrt ( - log ( 1.0 - p ) );
    else
      r = sqrt ( - log ( p ) );
    end

    value = ( ( ( ...
        c3   * r ...
      + c2 ) * r ...
      + c1 ) * r ...
      + c0 ) / ( ( ...
        d2   * r ...
      + d1 ) * r ...
      + 1.0 );

    if ( p < 0.5 )
      value = - value;
    end
%
%  P <= 0.0 or 1.0 <= P
%
  else

    ifault = 1;
    value = 0.0;

  end

  return
end


% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function output = SA_spears(objective, inputs)
% SIMULATED_ANNEALING_SPEARS: Function runs simulated annealing algorithm using the
% SPEARS algorithm for optimizing binary functions (MAX-SAT). The reference with
% the description of the algorithm and the parameters can be found in:
%
% An Experimental Evaluation of Fast Approximation Algorithms for the Maximum
% Satisfiability Problem by: MATTHIAS POLOCZEK and DAVID P. WILLIAMSON

% Extract n_vars
n_vars = inputs.n_vars;

% Set temperature limits
max_temp = 10;
min_temp = 0.01;

% Set counter and T
counter  = 0;
T = max_temp;

% Set initial condition and evaluate objective function
new_x   = sample_models(1,n_vars);
new_obj = objective(new_x);

% Set best variables
best_x   = new_x;
best_obj = new_obj;

% Declare vectors to save solutions
model_iter = zeros(0,n_vars);
obj_iter   = zeros(0,1);
time_iter  = zeros(0,1);

% Run simulated annealing
while(T >= min_temp)

    % Increment counter
    counter = counter + 1;
    sa_iter = tic;

    % Decrease T according to cooling schedule
    T = T*exp(-1/n_vars);

    %% Compute change from flipping each variable
    for i=1:n_vars

        % compute change in objective from flipping bit
        temp_x  = new_x; temp_x(i) = 1 - temp_x(i);
        temp_df = objective(temp_x) - new_obj;

        % Compute probability of flipping variable
        p = 1/(1 + exp(-temp_df/T));
        if rand < p
            new_x = temp_x;
        end

    end

    % Evaluate objective function at current solution
    new_obj = objective(new_x);

    % Update best solution
    if new_obj < best_obj
        best_x = new_x;
        best_obj = new_obj;
    end

    % save solution
    model_iter = [model_iter; best_x];
    obj_iter   = [obj_iter; best_obj];
    time_iter  = [time_iter; toc(sa_iter)];

end

% save outputs
output = struct;
output.objVals  = obj_iter;
output.optModel = model_iter;
output.runTime  = time_iter;

end


% cplxdual2D_plots
% DISPLAY 2D WAVELETS OF cplxdual2D.M

J = 4;
L = 3*2^(J+1);
N = L/2^J;
[Faf, Fsf] = FSfarras;
[af, sf] = dualfilt1;
x = zeros(2*L,6*L);
w = cplxdual2D(x, J, Faf, af);
w{J}{1}{2}{2}(N/2,N/2+0*N) = 1;
w{J}{1}{1}{3}(N/2,N/2+1*N) = 1;
w{J}{1}{2}{1}(N/2,N/2+2*N) = 1;
w{J}{1}{1}{1}(N/2,N/2+3*N) = 1;
w{J}{1}{2}{3}(N/2,N/2+4*N) = 1;
w{J}{1}{1}{2}(N/2,N/2+5*N) = 1;
w{J}{2}{2}{2}(N/2+N,N/2+0*N) = 1;
w{J}{2}{1}{3}(N/2+N,N/2+1*N) = 1;
w{J}{2}{2}{1}(N/2+N,N/2+2*N) = 1;
w{J}{2}{1}{1}(N/2+N,N/2+3*N) = 1;
w{J}{2}{2}{3}(N/2+N,N/2+4*N) = 1;
w{J}{2}{1}{2}(N/2+N,N/2+5*N) = 1;
y = icplxdual2D(w, J, Fsf, sf);
y = [y; sqrt(y(1:L,:).^2+y(L+[1:L],:).^2)];
figure(1)
clf
imagesc(y);
title('2D Dual-Tree Complex Wavelets')
axis image
axis off
colormap(gray(128))
print -djpeg95 cplxdual2D_plots


function theta = GFM_rand_init( tree, M, min_prob, min_std )

% Initial values for GFM EM algorithm
% Use the 'small' EM algorithm of Fan & Xia to initialize the 'big' EM
% algorithm of Crouse et al/Durand et al.
%
% Syntax:
%   theta = GFM_rand_init( tree, M, min_prob, min_std )
%
% Input:
%   tree  : Wavelet transform from DWT2_TO_TREE with L levels and D
%           dimensions
%
%   M     : The number of mixtures.
%
%	min_p : Mininimum value of the probabilities
%
%	min_s : Mininimum value of the standard devs
%
%
% Output:
%   theta : L-by-3-by-D cell with parameters. For the third dimension row l
%           are the parameters for level l:
%           state probs, transition probs, standard deviations
%
%
% See also: GFM_INIT, GFM_EM_WRAPPER, GFM_EM

L = length(tree) - 1;
D = length(tree{2});

% Initialize output
theta = cell( L, 3, D );

for l = 1:L
    for d = 1:D
		% State probs
        p = max( min_prob, rand(1,2) );
        theta{l, 1, d} = p / sum(p);

		if l > 1
			% Trans probs
    		p = max( min_prob, rand(2) );
    		theta{l, 2, d} = p ./ repmat( sum(p), [M 1] );

			% Update state probs to be consistent with the trans probs
			theta{l,1,d} = theta{l-1,1,d} * theta{l,2,d}';
		end

		% Standard devs
        theta{l, 3, d} = max( min_std, rand(1,M)*std(tree{l+1}{d}(:)) );
    end
end

end


function [u,p,w,edge,eqn,info] = StokesRT0(node,elem,bdFlag,pde,option)
%% STOKESRT0 Stokes equations: the lowest order Raviart-Thomas element in 2D.
%
%  [u,p,w,rotuI,Me,Mv,eqn,info] = StokesRT0(node,elem,bdFlag,pde,option)
%  uses the lowest order of Raviart-Thomas element (RT0) to approximate
%  velocity u and piecewise constant element (P0)  to approximate pressure
%  p, repectively.
%
%  We solve the following equation:
%       - grad div u + curl rot u + grad p  = f   in \Omega
%                                  - div u  = 0   in \Omega
%                                        u  = g   on \Gamma
%
% ifem StokesRT0doc
%
%  See also StokesBDM1B
%
% Based on a version by Ming Wang. Revised by Lin Zhong. Discussed with Jie
% Zhou and Long Chen. Further clean up by Long Chen.

if ~exist('option','var'), option = []; end

%% Data structure
elemunSort = elem;
[elem,bdFlag] = sortelem(elemunSort,bdFlag);
[elem2edge,edge] = dofedge(elem);
[Clambda,area,elemSign] = curlbasis(node,elem);
N = size(node,1); NT = size(elem,1); NE = size(edge,1);
Nu = NE; Np = NT; Ndof = Nu + Np;

%% Assemble matrix
tic; % record assemble time
% Mv: Lumped mass matrix for vertex: P1 element
vecMv = accumarray([elem(:,1);elem(:,2);elem(:,3)],[area;area;area]/3,[N,1]);
% invMv = spdiags(1./vecMv,0,N,N);
Mv = spdiags(vecMv,0,N,N);

% Me: Mass matrix for RT0 element
Me = getmassmatvec(elem2edge,area,Clambda,'RT0');

%invMt: the inverse of Mass matrix for P0 element
invMt = spdiags(1./area,0,NT,NT);

% B: negative divergence operator
B = -icdmat(double(elem2edge),elemSign*[1 -1 1]);

% C: curl operator
C = icdmat(double(edge),[-1 1]);

% R: weak rot operator
R = spdiags(1./vecMv,0,N,N)*C'*Me;

% Vector Laplacian
A = B'*invMt*B + R'*Mv*R;

%% Assemble right hand side
locEdge = [2,3; 1 3; 1,2]; % ascend ordering
fu = zeros(Nu,1);% the right hand side of u
g = zeros(Np,1); % the right hand side of p
if ~isfield(pde,'f') || (isfield(pde,'f') && isreal(pde.f) && all(pde.f==0))
    pde.f = [];
end
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order is 3
end
if isfield(pde,'f') && ~isempty(pde.f)
    [lambda,w] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
    bt = zeros(NT,3);
    for p = 1:nQuad
        % quadrature points in the x-y-z coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        fp = pde.f(pxy);
        for k = 1:3
            i = locEdge(k,1); j = locEdge(k,2);
            % phi_k = lambda_iClambda_j - lambda_jClambda_i;
            phi_k = lambda(p,i)*Clambda(:,:,j)-lambda(p,j)*Clambda(:,:,i);
            rhs = dot(phi_k,fp,2);
            bt(:,k) = bt(:,k) + w(p)*rhs;
        end
    end
    bt = bt.*repmat(area,1,3);
    fu = accumarray(elem2edge(:),bt(:),[Nu 1]);
end
clear pxy fp bt rhs phi_k psi_k

%% Boundary condition and graddiv part of A
[u,p,ufreeDof,pDof,utbd] = getbdStokesRT0;
assembleTime = toc;

%% Solve the system of linear equations
% set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 1e5  % Direct solver for small size systems
        solver = 'direct';
    else             % Multigrid-type  solver for large size systems
        solver = 'mg';
    end
else
    solver = option.solver;
end
% solve the system
% get submatrices of ufreeDof
A0 = A(ufreeDof,ufreeDof);
B0 = B(:,ufreeDof);
f0 = fu(ufreeDof);
g0 = g;
if strcmp(solver,'direct') && ~isempty(ufreeDof)
    tic;
    bigA = [A0, B0'; ...
            B0, sparse(Np,Np)];
    bigF = [f0; g0];
    bigu = [u; p];
    bigFreeDof = [ufreeDof; Nu+pDof];
    bigu(bigFreeDof) = bigA(1:end-1,1:end-1)\bigF(1:end-1);
    u = bigu(1:Nu);
    p = bigu(Nu+1:end);
    info.solverTime = toc;
elseif strcmp(solver,'mg')
%   option.solver = 'vcycle';
    option.solver  = 'WCYCLE';
    [u(ufreeDof),p,info] = mgstokesRT0(A0,B0,f0,g0,u,p,node,elemunSort,ufreeDof,option);
end

%% Post-process
if length(pDof)~=Np % p is unique up to a constant
    % impose the condition int(p)=0
    c = sum(p.*area)/sum(area);
    p = p - c;
end
% w = invMv*(C'*Me*u + utbd);
w = R*u + utbd./vecMv;

%% Output information
eqn = struct('A',A0,'B',B0,'f',f0,'g',g0,'ufreeDof',ufreeDof,'pDof',pDof,'Me',Me,'Mv',Mv);
info.assembleTime = assembleTime;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbdStokesRT0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [u,p,ufreeDof,pDof,utbd] = getbdStokesRT0
        %% Boundary condition of Stokes equation: RT0-P0 elements

        % Initial set up
        utbd = zeros(N,1); % the line integral of u on the boundary (u.t, tau)|_\partial \Omega
        u = zeros(Nu,1);
        p = zeros(Np,1);
        ufreeDof = (1:Nu)';
        pDof = (1:Np-1)';

        if ~exist('bdFlag','var'), bdFlag = []; end
        if ~isfield(pde,'g_D'), pde.g_D = []; end
        if ~isfield(pde,'g_N'), pde.g_N = []; end
        if ~isfield(pde,'g_R'), pde.g_R = []; end
        if isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
            bdFlag = [];
        end

        % Find Dirichlet boundary dof: fixedDof and pDof
        isFixedDof = false(Nu,1);
        if ~isempty(bdFlag)
            isDirichlet(elem2edge(bdFlag(:)==1)) = true;
            Dirichlet = edge(isDirichlet,:);
            isFixedDof(isDirichlet) = true;
            fixedDof = find(isFixedDof);
            ufreeDof = find(~isFixedDof);
        end

        % Set up edge sign
        % edgeSign records the inconsistency of asecond orientation and
        % induced orientation for each boundary edges
        edgeSign = ones(NE,1);
        idx = (bdFlag(:,1) ~= 0 ) & (elemSign == -1) ; % the first edge is on boundary
        edgeSign(elem2edge(idx,1)) = -1;
        idx = (bdFlag(:,2) ~= 0 ) & (elemSign ==  1) ; % the second edge is on boundary
        edgeSign(elem2edge(idx,2)) = -1;
        idx = (bdFlag(:,3) ~= 0 ) & (elemSign == -1) ; % the third edge is on boundary
        edgeSign(elem2edge(idx,3)) = -1;

        % Compute the boundary integral
        if ~isempty(fixedDof) && ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && (pde.g_D == 0))
            % else no bddof or g_D = 0 (no modification needed)
            % 1. Normal component of u is imposed strongly
            if (isnumeric(pde.g_D) && length(pde.g_D) == NE)
                u(fixedDof) = pde.g_D(fixedDof);
            else
                u(fixedDof) = faceinterpolate(pde.g_D,node,edge(fixedDof,:),'RT0');
            end
            % 2. Tangential component of u is imposed weakly
            [lambdagD,wgD] = quadpts1(3);
            nQuadgD = size(lambdagD,1);
            % quadratic bases 1--3--2
            bdphi = lambdagD;
            ve = node(Dirichlet(:,2),:) - node(Dirichlet(:,1),:);
            ge = zeros(size(Dirichlet,1),2);
            int_left = zeros(size(Dirichlet,1),2);
            int_right = zeros(size(Dirichlet,1),2);
            for pp = 1:nQuadgD
                ppxy = lambdagD(pp,1)*node(Dirichlet(:,1),:) ...
                     + lambdagD(pp,2)*node(Dirichlet(:,2),:);
                gDp = pde.g_D(ppxy);
                int_left = int_left + wgD(pp)*gDp*bdphi(pp,1);
                int_right = int_right + wgD(pp)*gDp*bdphi(pp,2);
            end
            ge(:,1) = dot(int_left,ve,2).*edgeSign(fixedDof);
            ge(:,2) = dot(int_right,ve,2).*edgeSign(fixedDof);
            utbd = accumarray(Dirichlet(:), [ge(:,1); ge(:,2)],[N,1]);
        end

        % Modify the right hand side
        fu = fu - A*u - Me*(C*(utbd./vecMv));
         g = g - B*u;
         g = g - mean(g);
    end
end


% timewarp()   - Given two event marker vectors, computes a matrix
%                that can be used to warp a time series so that its
%                evlatencies match newlatencies. Values of the warped
%                timeserie that falls between two frames in the original
%                timeserie will be linear interpolated.
% Usage:
%   >> warpmat = timewarp(evlatency, newlatency)
%
% Necessary inputs:
%   evlatency  - [vector] event markers in the original time series, in frames
%                Markers must be ordered by increasing frame latency.
%                If you want to warp the entire time-series, make sure
%                the first (1) and last frames are in the vector.
%   newlatency - [vector] desired warped event time latencies. The original
%                time series will be warped so that the frame numbers of its
%                events (see evlatency above) match the frame numbers in
%                newlatency. newlatency frames must be sorted by ascending
%                latency. Both vectors must be the same length.
%
% Optional outputs:
%      warpmat - [matrix] Multiplying this matrix with the original
%                time series (column) vectors performs the warping.
%
% Example:
%      % In 10-frame vectors, warp frames 3 and 5 to frames 4 and 8,
%      % respectively. Generate a matrix to warp data epochs in
%      % variable 'data' of size (10,k)
%      >> warpmat = timewarp([1 3 5  10], [1 4 8 10])
%      >> warped_data = warpmat*data;
%
% Authors: Jean Hausser, SCCN/INC/UCSD, 2006
%
% See also: angtimewarp(), phasecoher(), erpimage()
%
function M=timewarp(evLatency, newLatency)
  M = [0];
  if min(sort(evLatency) == evLatency) == 0
    error('evLatency should be in ascending order');
    return;
  end
  if min(sort(newLatency) == newLatency) == 0
    error('newLatency should be in ascending order');
    return;
  end
  if length(evLatency) ~= length(newLatency)
    error('evLatency and newLatency must have the same length.');
    return;
  end
  if length(evLatency) < 2 | length(newLatency) < 2
    error(['There should be at least two events in evlatency and ' ...
          'newlatency (e.g., "begin" and "end")'] );
    return;
  end
  if evLatency(1) ~= 1
    disp(['Assuming old and new time series beginnings are synchronized.']);
    disp(['Make sure you have defined an ending event in both the old and new time series!']);
    evLatency(end+1)=1;
    newLatency(end+1)=1;
    evLatency = sort(evLatency);
    newLatency = sort(newLatency);
  end

  t = 1:max(evLatency);

  for k=1:length(evLatency)-1
    for i=evLatency(k):evLatency(k+1)-1
      tp(i) = (t(i)-evLatency(k)) * ...
              (newLatency(k+1) - newLatency(k))/...
              (evLatency(k+1) - evLatency(k)) + ...
              newLatency(k);
    end
  end

  % Check what's going on at tp(max(newLatency)), should equal t(max(evLatency))
  tp(max(evLatency)) = max(newLatency);
  ts = tp-min(newLatency)+1;

% $$$   M = sparse(max(newLatency)-min(newLatency)+1, max(evLatency));
  M = zeros(max(newLatency)-min(newLatency)+1, max(evLatency));

  k = 0;
  for i=1:size(M,1)
    while i > ts(k+1)
      k = k+1;
    end
% $$$     k = k-1;

    if k == 0
      % Check wether i == ts(1) and i == 1
      % In that case, M(1,1) = 1
      M(1,1) = 1;
    else
      M(i,k) = 1 - (i-ts(k))/(ts(k+1)-ts(k));
      M(i,k+1) = 1 - (ts(k+1)-i)/(ts(k+1)-ts(k));
    end
  end


function [X, A] = PCA_stochastic(A, k)
% Example of stochastic gradient algorithm in Manopt on a PCA problem.
%
% PCA (principal component analysis) on a dataset A of size nxd consists
% in solving
%
%   minimize_X  f(X) = -.5*norm(A*X, 'fro')^2 / n,
%
% where X is a matrix of dimension dxk with orthonormal columns. This
% is equivalent to finding k dominant singular vectors of A, or k top
% eigenvectors of A'*A.
%
% If n is large, this computation can be expensive. Thus,  stochastic
% gradient algorithms take the point of view that f(X) is a sum of many (n)
% terms: each term involves only one of the n rows of A.
%
% To make progress, it may be sufficient to optimize with respect to a
% subset of the terms at each iteration. This way, each individual
% iteration can be very cheap. In particular, individual operations have
% cost independent of n, because f or its gradient need never be evaluated
% completely (or at all in the case of f.)
%
% Stochastic gradient algorithms (this implementation in particular) are
% sensitive to proper parameter tuning. See in code.

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Bamdev Mishra and Nicolas Boumal, Sept. 6, 2017
% Contributors:
%
% Change log:
%

    % If none is given, generate a random data set: n samples in R^d
    if ~exist('A', 'var') || isempty(A)
        d = 1000;
        n = 100000;
        fprintf('Generating data...');
        A = randn(n, d)*diag([[15 10 5], ones(1, d-3)]);
        fprintf(' done (size: %d x %d).\n', size(A));
    else
        [n, d] = size(A);
    end

    % Pick a number of component to compute
    if ~exist('k', 'var') || isempty(k)
        k = 3;
    end

    % We are looking for k orthonormal vectors in R^d: Stiefel manifold.
    problem.M = stiefelfactory(d, k);

    % The cost function to minimize is a sum of n terms. This parameter
    % must be set for stochastic algorithms.
    problem.ncostterms = n;

    % We do not need to specify how to compute the value of the cost
    % function (stochastic algorithms never use this). All we need is to
    % specify how to compute the gradient of the cost function, where the
    % sum is restricted to a subset of the terms (a sample). Notice that we
    % specify a partial Euclidean gradient (hence the 'e' in partialegrad).
    % This way, Manopt will automatically convert the Euclidean vector into
    % a proper Riemannian partial gradient, in the tangent space at X.
    % In particular, if sample = 1:n, then the partial gradient corresponds
    % to the actual (complete) gradient.
    problem.partialegrad = @partialegrad;
    function G = partialegrad(X, sample)

        % X is an orthonormal matrix of size dxk
        % sample is a vector if indices between 1 and n: a subset
        % Extract a subset of the dataset
        Asample = A(sample, :);

        % Compute the gradient of f restricted to that sample
        G = -Asample'*(Asample*X);
        G = G / n;

    end

    % If one wants to use checkgradient to verify one's work, then it is
    % necessary to specify the cost function as well, as below.
    % problem.cost = @(X) -.5*norm(A*X, 'fro')^2 / n;
    % checkgradient(problem); pause;

    % To have the solver record statistics every x iterations, set
    % options.checkperiod to x. This will record simple quantities which
    % are almost free to compute (namely, elapsed time and step size of the
    % last step.) To record more sophisticated quantities, you can use
    % options.statsfun as usual. Time spent computing these statistics is
    % not counted in times reported in the info structure returned by the
    % solver.
    options.checkperiod = 10;
    options.statsfun = statsfunhelper('metric', @(X) norm(A*X, 'fro'));

    % Set the parameters for the solver: stochastic gradient algorithms
    % tend to be quite sensitive to proper tuning, especially regarding
    % step size selection. See the solver's documentation for details.
    options.maxiter = 200;
    options.batchsize = 10;
    % options.stepsize_type = 'decay';
    options.stepsize_init = 1e2;
    options.stepsize_lambda = 1e-3;
    options.verbosity = 2;

    % Run the solver
    [X, info] = stochasticgradient(problem, [], options);

    % Plot the special metric recorded by options.statsfun
    plot([info.iter], [info.metric], '.-');
    xlabel('Iteration #');
    ylabel('Frobenius norm of A*X');
    title('Convergence of stochasticgradient on stiefelfactory for PCA');

    % Add to that plot a reference: the globally optimal value attained if
    % the true dominant singular vectors are computed.
    fprintf('Running svds... ');
    t = tic();
    [V, ~] = svds(A', k);
    fprintf('done: %g [s] (note: svd may be faster)\n', toc(t));
    hold all;
    bound = norm(A*V, 'fro');
    plot([info.iter], bound*ones(size([info.iter])), '--');
    hold off;

    legend('Algorithm', 'SVD bound', 'Location', 'SouthEast');

end


function [Tx,Ty,Tz,nr,A] = surfcv(x,y,z,f,const);
%SURFCV 3-D surface of constant value plot.
%   SURFCV(X,Y,Z,F,CONST) draws a surface of constant value
%   for a function of three variables:  F(X,Y,Z) = CONST .
%   The arrays X,Y,Z define the coordinates for F and must
%   be monotonic and 3-D plaid (as if produced by meshgrid).
%   The surface is calculated by linear interpolation of the
%   values of F and is drawn with triangular patches using
%   FILL3.  The color of each patch is determined by its
%   orientation with respect to a specified direction.
%   F must be an M-by-N-by-P volume array.
%
%   SURFCV(F,CONST) assumes X=1:N, Y=1:M, Z=1:P.
%
%   H = SURFCV(...) returns a column vector of handles
%   to PATCH objects, one handle per patch.
%
%   [TX,TY,TZ] = SURFCV(X,Y,Z,F,CONST); returns 3-by-K
%   arrays specifying x,y,z coordinates of the vertices
%   of the triangular patches, where K is the number
%   of patches.  FILL3(TX,TY,TZ,C) can be used to draw
%   the surface.
%
%   [TX,TY,TZ,NR] = SURFCV(...) also returns 3-by-K
%   arrays of normal vectors for each patch which
%   can be used to compute lighting conditions of the
%   surface.  Vectors are normalized to unity and
%   point in the direction of increasing F.
%
%   [TX,TY,TZ,NR,A] = SURFCV(...) returns the area
%   of each patch.
%
%   Example 1: To visualize the surface
%              1./(X.^2+Y.^2) +  1./(Y.^2+Z.^2) + 1./(Z.^2+X.^2) = 2
%              over the range -3 < X < 3, -3 < Y < 3, -3 < Z < 3:
%
%      [x,y,z] = meshgrid(-3:.4:3, -3:.4:3, -3:.4:3);
%      f = 1./(x.^2+y.^2) +  1./(y.^2+z.^2) + 1./(z.^2+x.^2);
%      const = 2;
%      surfcv(x,y,z,f,const);
%
%   Example 2: The routine does not require the grid to be Cartesian
%              and can handle a more general problem of the form:
%                 F(U(X,Y,Z),V(X,Y,Z),W(X,Y,Z)) = CONST
%              This allows to change surface appearance not only
%              by modifying the function F, but also by deforming
%              the grid:
%
%      [x,y,z] = meshgrid(-3:.4:3, -3:.4:3, -3:.4:3);
%      f = 1./(x.^2+y.^2) +  1./(y.^2+z.^2) + 1./(z.^2+x.^2);
%      const = 2;
%      z = z + 0.2.*(x.^2 - y.^2);      %  Grid deformation
%      surfcv(x,y,z,f,const);
%

%  Copyright (c) 1997 Ruslan L. Davidchack
%                     University of Kansas, Lawrence, KS
%                     e-mail: ruslan@ukans.edu
%                     URL:    http://www.ukans.edu/home/ruslan
%      created:  Sep 29, 1997;
%      modified: Dec 23, 1998;

%Short description of the algorithm.
%   Function F is defined on the M-by-N-by-P 3-D grid which
%   divides the space into (M-1)*(N-1)*(P-1) cubes.
%   F specifies values of the function at the vertices.
%   The surface of constant value cuts a patch through a
%   cube if F < CONST for some vertices of the cube and
%   F > CONST for others.
%     Note: F is slightly modified on the input to assure
%           that it does not exactly equal CONST on a vertex,
%           and thus a vertex of a surface element belongs to
%           only one edge (see below).
%   Every vertex of the patch lays on the edge of the
%   cube and its coordinates can be determined by linear
%   interpolation between the values of the function on the
%   ends of the edge. Two vertices of a patch are connected
%   if they lay on the edges that belong to the same face of
%   a cube. If a patch has more than three vertices, it is
%   subdivided into triangular elements by additional
%   connections between vertices that do not belong to the
%   same face.
%   The idea of the algorithm is to group the edges which
%   contain vertices of the surface patches into pairs according
%   to their affiliation with the faces (there can be only 2 or 4
%   patch vertices on a face).  Then the edge pairs are grouped
%   according to their affiliation with the cubes.  The "cube-edge"
%   array, thus created, is then decomposed into triplets of patch
%   vertices.  There can be more than one patch per cube and
%   the algorithm handles this case as well.
%Naming convensions
%   ve - vertex, ed - edge, fa - face, cu - cube
%   nv - number of vertices, ne - number of edges, etc.
%   ce - coordinates of edges, cf - coordinates of faces, etc.
%   ie - indices of edges, ia - indices of faces, etc.

%Simple case to follow the algorithm
%      [x,y,z] = meshgrid(1:4, 1:3, 1:2); const = 15;
%      f = x.^2 + y.^2 + z.^2;
%Different cool surfaces:
% 1.   [x,y,z] = meshgrid(-3:.2:3, -3:.2:3, -3:.2:1.6); const = 7;
%      f = 1./(x.^2+y.^2)+1./(y.^2+z.^2)+1./(z.^2+x.^2)+x.^2+y.^2+z.^2;
%
% 2.   [x,y,z] = meshgrid(-2:.2:2, -2:.2:2, -2:.2:2);  const = 0;
%      f = (x.^2 + y.^2 + z.^2).^2 - 4*(x.^2 - y.^2 - z.^2);
%
% 3.   [x,y,z] = meshgrid(-2:.2:2, -2:.2:2, -2:.2:2);  const = 0;
%      f = x.^2 + y.^2 - z.^2.*(2 - z)./(2 + z);

  if nargin == 2,
    f = x; const = y;
    [x,y,z] = meshgrid(1:size(f,2),1:size(f,1),1:size(f,3));
  elseif nargin ~=5
    disp('Wrong number of input arguments'); return;
  end,

%Make sure F == CONST is never true on the grid
  f1 = f; rngf = max(max(max(f))) - min(min(min(f)));
  if ~isfinite(rngf), rngf = 1; end,
  f1(find(f == const)) = const + 1e-12*rngf;

%Number of vertices in each direction:  nv = [M N P]
%   Total number of vertices = prod(nv)
  nv = size(f1);  iv = reshape(1:prod(nv),nv);
  [ix,iy,iz] = meshgrid(1:nv(2),1:nv(1),1:nv(3));

%Number of edges of each orientation:
%			ne = [(M-1)*N*P  M*(N-1)*P  M*N*(P-1)]
%   Total number of edges = sum(ne)
  ne = prod(nv) - prod(nv([2 1 1;3 3 2]));

%Determine indices of vertices that belong to each edge
%   Indexing of vertices is the same as in f(:)
  ed = zeros(sum(ne),2);
%   Edges parallel to y-axis
  ed((1:ne(1)),1) = reshape(iv(1:nv(1)-1,:,:),ne(1),1);
  ed((1:ne(1)),2) = ed((1:ne(1)),1) + 1;
%   Edges parallel to x-axis
  ie = (1:ne(2)) + ne(1);
  ed(ie,1) = reshape(iv(:,1:nv(2)-1,:),ne(2),1);
  ed(ie,2) = ed(ie,1) + nv(1);
%   Edges parallel to z-axis
  ie = (1:ne(3)) + ne(1) + ne(2);
  ed(ie,1) = (1:ne(3))';
  ed(ie,2) = ed(ie,1) + nv(1)*nv(2);

%Select edges crossed by the surface
  se = find(prod(f1(ed)' - const) < 0)';
  if isempty(se),
    disp('There is no surface in the specified range');
    if nargout > 0, Tx = []; end;
    if nargout > 2, Ty = []; Tz = []; end;
    if nargout > 3, nr = []; end;
    if nargout > 4, A = []; end;
    return; end;
  ed = ed(se,:);

%Restore coordinates of the selected edges
  ce = zeros(length(se),4);
  ce(:,1:3) = [ix(ed(:,1)) iy(ed(:,1)) iz(ed(:,1))];
  ce(:,4) = (se > ne(1)) + (se > ne(1)+ne(2)) + 1;

%Determine coordinates of the patch vertices
  dd = diff(f1(ed)')';  i1 = find(dd < 0);
  dd = (const - f1(ed(:,1)))./dd;
  np = zeros(3,length(se));  np(1,:) = diff(x(ed)');
  np(2,:) = diff(y(ed)');    np(3,:) = diff(z(ed)');
  xv = np(1,:)'.*dd + x(ed(:,1));
  yv = np(2,:)'.*dd + y(ed(:,1));
  zv = np(3,:)'.*dd + z(ed(:,1));
  np(:,i1) = -np(:,i1);

  ie1 = find(ce(:,4) == 1); ie2 = find(ce(:,4) == 2);
  ie3 = find(ce(:,4) == 3);
  ie = cumsum([length(ie1); length(ie2); length(ie3)]);

%Find faces containing the selected edges (maximum of 4 faces per edge)
  cf = repmat([(1:length(se))' ce],1,4);
%Edge-to-face coordinate transformation matrix
  etof = [0 0 0 0 0   0 -1  0  0 0   0 0 0 0  2   0  0  0 -1  2;...
	  0 0 0 0 0   0  0  0 -1 0   0 0 0 0 -1   0  0 -1  0 -1;...
          0 0 0 0 0   0  0 -1  0 0   0 0 0 0 -1   0 -1  0  0 -1];
  ie = diff([0;ie]);
  cf = cf + [repmat(etof(1,:),ie(1),1);repmat(etof(2,:),ie(2),1);...
             repmat(etof(3,:),ie(3),1)];
  cf = reshape(cf',5,4*length(se));

%Discard faces that are outside the range of x,y,z
  cf(:,find(prod(cf) == 0)) = [];  cf = cf';
  i1 = find(cf(:,5) ~= 2);  cf(i1(find(cf(i1,3) > nv(1)-1)),:) = [];
  i2 = find(cf(:,5) < 3);   cf(i2(find(cf(i2,2) > nv(2)-1)),:) = [];
  i3 = find(cf(:,5) > 1);   cf(i3(find(cf(i3,4) > nv(3)-1)),:) = [];

%Find indices of the selected faces
  i1 = find(cf(:,5)==1); i2 = find(cf(:,5)==2); i3 = find(cf(:,5)==3);
%Number of faces of each orientation:
%                ne = [(M-1)*(N-1)*P  M*(N-1)*(P-1)  (M-1)*N*(P-1)]
  nf = [nv(3)*(nv(2)-1)*(nv(1)-1) nv(1)*(nv(2)-1)*(nv(3)-1)...
        nv(2)*(nv(3)-1)*(nv(1)-1)];
  fa = zeros(size(cf,1),1);
  fa(i1) = cf(i1,3)+((cf(i1,2)-1)+(cf(i1,4)-1)*(nv(2)-1))*(nv(1)-1);
  fa(i2) = cf(i2,3)+((cf(i2,2)-1)+(cf(i2,4)-1)*(nv(2)-1))*nv(1)+nf(1);
  fa(i3) = cf(i3,3)+((cf(i3,2)-1)+(cf(i3,4)-1)*nv(2))*(nv(1)-1)+...
	   nf(1) + nf(2);
  [fa ia] = sort(fa);

%!!!!!!!! Check whether fac values come in pairs.
%   Each face can have only 2 or 4 selected edges.
%   The check below can be removed after debugging process
%   is completed
if mod(length(fa),2) ~= 0,
  disp('Odd number!'); return,
else
  if sum(diff(reshape(fa,2,length(fa)/2))) > 0,
  disp('Odd number!'); return, end, end;
%!!!!!!!!

%Combine face coord. with indices of edges that belong to the faces
  cf = [fa(1:2:end) reshape(cf(ia,1),2,length(fa)/2)' ...
        cf(ia(1:2:end),2:5)];

%Find cubes containing selected faces
  i1 = find(cf(:,7)==1); i2 = find(cf(:,7)==2); i3 = find(cf(:,7)==3);
  cc = repmat(cf(:,2:6),1,2);
  ftoc = [zeros(3,7) [0 0 -1;0 -1 0;-1 0 0]];
  cc = cc + [repmat(ftoc(1,:),length(i1),1);...
   repmat(ftoc(2,:),length(i2),1);repmat(ftoc(3,:),length(i3),1)];
  cc = reshape(cc',5,size(cf,1)*2);
  cc(:,find(prod(cc) == 0 | cc(4,:) > nv(1)-1 | cc(3,:) > nv(2)-1 |...
            cc(5,:) > nv(3)-1)) = [];  cc = cc';

  cu = cc(:,4) + ((cc(:,3)-1) + (cc(:,5)-1)*(nv(2)-1))*(nv(1)-1);
  [cu ic] = sort(cu);
%Combine indices of selected cubes with indices of edges that belong
%   to them into a cube-edge index array
  cc = [cu cc(ic,1:2)];

%Combine edges into triplets composing surface patches
  ip = [];   cc = sortrows(cc);
  while 1,
%Indices of patches
    ic = find(diff([0;cc(:,1);0]) ~= 0);
%Indices of triangular patches
    ic3 = ic(find(diff(ic) == 3));
%Indices of patches with more than 3 vertices
    icm = ic(find(diff(ic) > 3));

%Select triangular patches and take them out of the cube-edge array
    ic(end) = [];   ip = [ip [cc(ic,2:3)';cc(ic+1,3)']];
    if ~isempty(ic3),
      irm = repmat(ic3,1,3) + repmat(0:2,length(ic3),1);
    else irm = []; end,
    irm = [irm(:);icm];   cc(icm+1,2) = cc(icm,3);   cc(irm,:) = [];

%Sort new cube-edge index array or quit if it is empty
    if ~isempty(cc),  cc = sortrows(cc);  else,
      disp(['Number of patches = ' num2str(size(ip,2))]);
      break; end

%Find cases when there is more than one patch per cube
    irm = find(sum(abs(diff(cc)')) == 0);
    if ~isempty(irm),
      %disp(['More than one patch in ' int2str(length(irm)) ' cubes']);
      irm = repmat(irm,2,1)+repmat([0;1],1,length(irm)); irm = irm(:);
      cc(irm,:) = []; end,

  end;

  Tx = xv(ip);  Ty = yv(ip);  Tz = zv(ip);

%This is to compute area of each patch and the normal vector
  x2 = Tx(2,:)-Tx(1,:); y2 = Ty(2,:)-Ty(1,:); z2 = Tz(2,:)-Tz(1,:);
  x3 = Tx(3,:)-Tx(1,:); y3 = Ty(3,:)-Ty(1,:); z3 = Tz(3,:)-Tz(1,:);
  if nargout == 5
     A = sqrt((x2.*y3-x3.*y2).^2 + (y2.*z3-y3.*z2).^2 +...
              (z2.*x3-z3.*x2).^2)./2; end
  nr = [y2.*z3-y3.*z2; z2.*x3-z3.*x2; x2.*y3-x3.*y2];
  nr = nr./repmat(sqrt(sum(nr.^2)),3,1);
  i1 = find(sum(nr.*np(:,ip(1,:))) < 0);
  nr(:,i1) = -nr(:,i1);
  i2 = ip(2,i1); ip(2,i1) = ip(3,i1); ip(3,i1) = i2;
  Tx = xv(ip);  Ty = yv(ip);  Tz = zv(ip);
  if nargout < 3,
    va = [-2 3 1];
% Colored according to the patch orientation
    C = va*nr;
% Colored according to the patch distance from the origin
%    C = sqrt(Tx.^2 + Ty.^2 + Tz.^2);
    clf;  H = fill3(Tx,Ty,Tz,C);  axis image;
    set(gca,'xlim',[min(min(Tx)) max(max(Tx))],...
            'ylim',[min(min(Ty)) max(max(Ty))],...
            'zlim',[min(min(Tz)) max(max(Tz))],'box','on');
    view(va);
    Tx = H;
  end
  if nargout == 0, clear Tx, end
  if nargout < 4, clear nr, end


function trigamma_values_test ( )

%*****************************************************************************80
%
%% TRIGAMMA_VALUES_TEST demonstrates the use of TRIGAMMA_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIGAMMA_VALUES_TEST:\n' );
  fprintf ( 1, '  TRIGAMMA_VALUES stores values of\n' );
  fprintf ( 1, '  the TriGamma function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X               FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = trigamma_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function u = u_function ( time )

%*****************************************************************************80
%
%% U_FUNCTION evaluates the time-dependent boundary values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 December 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real TIME, the current time.
%
%    Output, real U(2), the boundary values at the left and right endpoints.
%
  ua =   sin ( pi * time );
  ub = - sin ( pi * time );

  u = [ ua; ub ];

  return
end


Q = [1 1];                      % State weights
R = 0.5;                        % du weights
Ru = 0.5;                       % u weights
B = [0; 1];
C = eye(Nvar);
D = 0;
LB = -20*ones(N,1);        % Lower bound of control input
UB = 20*ones(N,1);         % Upper bound of control input


function pascalScore = computePascalScore(bb1,bb2)
%compute the Pascal score of the bb1, bb2 (intersection/union)

intersectionArea = computeIntersectionArea(bb1,bb2);
pascalScore = intersectionArea/(computeArea(bb1)+computeArea(bb2)-intersectionArea);
return


% Fig. 6.48   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clear all;
%close all;
clf

num=[20 1];
den=[1 0 0];
w=logspace(-3,1,100);
[m,p]=bode(num,den,w);
loglog(w,m);
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.48 Compensated open-loop transfer function.');
bodegrid;


function h = genHazFun(t,b)

% Sample general hazard function; depends on three parameters

h = b(1) + b(2)*(t./b(3)).*exp(-(t./b(3)));


function sVF = grad(sF, varargin)
% calculates the gradient of a spherical harmonic
%
% Syntax
%   sVF = grad(sF) % returns the gradient as a spherical vector field
%   g = grad(sF, v) % return the gradient in point v as vector3d
%
% Input
%  sF - @S2FunHarmonic
%  v - @vector3d
%
% Output
%  sVF - @sphericalVectorFieldHarmonic
%    g - @vector3d
%

if nargin > 1
  sF = [sF.drho; sF.dthetasin];
  v = varargin{1};
  y = eval(sF, v);
  sVF = ...
    y(:, 1)./sin(v.theta).^2.*S2VectorField.rho(v)+ ...
    y(:, 2) .* S2VectorField.theta(v);

  sVF(isnan(sVF)) = vector3d([0 0 0]);

else
  sF = [sF.drho; sF.dtheta];
  sVF = S2VectorFieldHarmonic(sF);
end

end


function [coef, bias, variance] = regressMajorityPercentage(smaps, pv, ph, imsegs)
% estimate the percentage of a region occupied by the majority label, given
% some estimate of the superpixel label likelihoods

ndata = 0;
for f = 1:numel(smaps)
    for m = 1:size(smaps{f}, 2)
        ndata = ndata + max(smaps{f}(:, m));
    end
    plab{f} = [pv{f}(:, 1) repmat(pv{f}(:, 2), [1 5]).*ph{f} pv{f}(:, 3)];
end

disp(num2str(ndata))

y = zeros(ndata, 1); % label
x = ones(ndata, 3); % statistics for regression
c = 0;
if 1
for f = 1:numel(smaps)
    npix = imsegs(f).npixels(:);
    labels = imsegs(f).labels(:);

    for m = 1:size(smaps{f}, 2)
        for s = 1:max(smaps{f}(:, m))
            ind = find(smaps{f}(:, m)==s);

            mclab = 0;
            mcprc = 0;
            for k = 1:7
                prc = sum((labels(ind)==k).*npix(ind))/(sum(npix(ind).*(labels(ind)>0))+1E-10);
                if prc > mcprc
                    mcprc = prc;
                    mclab = k;
                end
            end
            if mclab > 0
                y(c+s) = mcprc;
                x(c+s, 1) = sum(plab{f}(ind, mclab).*npix(ind))/sum(npix(ind));
                [tmp, maxlab] = max(plab{f}(ind, :), [], 2);
                x(c+s, 2) = sum((maxlab==mclab).*npix(ind)) / sum(npix(ind));
                x(c+s, 3) = numel(ind);
            end
        end
        c = c + s;
    end
end
ind = find(y==0);
y(ind) = [];
x(ind, :) = [];
save '../data/tmp.mat' x y
else
    load '../data/tmp.mat'
end

thresh1 = [0.05:0.1:0.95];
tx = sum(repmat(x(:, 1), [1 numel(thresh1)])  >= repmat(thresh1, [size(x,1) 1]), 2);
hist(tx)
for k =1:numel(thresh1)
    ind = find(tx==k);
    px(k) = mean(y(ind));
    count(k) = numel(ind);
    x(ind, 1) = px(k);
end
disp(num2str(px))
disp(num2str(count))

thresh2 = [0.05:0.1:0.95];
tx = sum(repmat(x(:, 2), [1 numel(thresh2)])  >= repmat(thresh2, [size(x,1) 1]), 2);
hist(tx)
for k =1:numel(thresh2)
    ind = find(tx==k);
    px(k) = mean(y(ind));
    count(k) = numel(ind);
    x(ind, 2) = px(k);
end
disp(num2str(px))
disp(num2str(count))

thresh3 = [1 2 3 4 5 7 10 15 20 25 35 50 100 150];
tx = sum(repmat(x(:, 3), [1 numel(thresh3)])  >= repmat(thresh3, [size(x,1) 1]), 2);
hist(tx)
for k =1:numel(thresh3)
    ind = find(tx==k);
    px(k) = mean(y(ind));
    count(k) = numel(ind);
    x(ind, 3) = px(k);
end
disp(num2str(px))
disp(num2str(count))

ylog = log((y+1E-5)./(1-y+1E-5));
xlog = log((x+1E-5)./(1-x+1E-5));

coef = fminunc(@(a) objective(a, xlog, y), [1 1 1 0]');

%coef = robustfit(xlog, ylog, 'logistic');
%coef = regress(ylog, [ones([size(xlog, 1) 1])  xlog]);
%coef = regress(ylog, xlog);

%ylogEst = coef(1) + xlog*coef(2:end);
ylogEst = coef(end) + xlog*coef(1:end-1);
size(ylogEst)
yEst = 1./(1+exp(-ylogEst));
disp(num2str([mean(yEst) mean(y)]))
bias = mean(yEst-y);
variance = var(yEst-y);
disp(mean(abs(yEst-y)))

function val = objective(coef, xlog, y)
ylogEst = coef(end) + xlog*coef(1:end-1);
yEst = 1./(1+exp(-ylogEst));
val = sum((yEst-y).^2);
%disp([sqrt(val/numel(y)) coef'])


function om_constants

% astrodynamic and utility constants

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global dtr rtd mu mmu smu omega req flat j2 aunit

dtr = pi / 180.0;

rtd = 180.0 / pi;

% earth gravitational constant (km**3/sec**2)

mu = 398600.436233;

% moon gravitational constant (km**3/sec**2)

mmu = 4902.800076;

% sun gravitational constant (km**3/sec**2)

smu = 132712440040.944;

% earth inertial rotation rate (radians/second)

omega = 7.292115486e-5;

% earth equatorial radius (kilometers)

req = 6378.1363;

% earth flattening factor (non-dimensional)

flat = 1.0 / 298.257;

% earth oblateness gravity coefficient (non-dimensional)

j2 = 0.00108263;

% astronomical unit (kilometers)

aunit = 149597870.691;


function rgb = ar2rgb(omega,radius,grayValue,varargin)
% compute rgb values from angle and radius
%
% Input
%  omega     -
%  radius    -
%  grayValue -
%
% Output
%  rgb       -

L = (radius(:) - 0.5) .* grayValue(:) + 0.5;

S = grayValue(:) .* (1-abs(2*radius(:)-1)) ./ (1-abs(2*L-1));
S(isnan(S))=0;

[h,s,v] = hsl2hsv(omega(:),S(:),L(:));

% the following lines correct for small yellow and cyan range in normal hsv
% space
if ~check_option(varargin,'noHueCorrection')

  z = linspace(0,1,1000);

  r = 0;f = 0.5 + exp(- 200.*(mod(z-r+0.5,1)-0.5).^2);
  b = 0.6666;f = f + exp(- 200.*(mod(z-b+0.5,1)-0.5).^2);
  g = 0.3333;f = f + exp(- 200.*(mod(z-g+0.5,1)-0.5).^2);

  f = f./sum(f);
  f = cumsum(f);
  h = interp1(z,f,h);
end

rgb = reshape(hsv2rgb(h,s,v),[],3);

end

% some testing code for this correctiom
%
% h = linspace(0,1);
% s = ones(size(h));
% v = ones(size(h));
% rgb = reshape(hsv2rgb(h,s,v),1,[],3);
% imagesc(rgb), axis off
%
%
%


function featuresS = ngtdmToScalarFeatures(s,p,numVoxels)
% function featuresS = ngtdmToScalarFeatures(s,p,numVoxels)
%
% APA, 3/15/2017

% Coarseness
featuresS.coarseness = 1/(sum(s .* p) + 1e-6);

% Contrast
Ng = sum(p > 0);
numLevels = length(p);
indV = (1:numLevels)';
term1 = 0;
term2 = 0;
for lev = 1:numLevels
    term1 = term1 + ...
        sum(p .* circshift(p,lev) .* (indV-circshift(indV,lev)).^2);
    term2 = term2 + s(lev);
end
featuresS.contrast = 1/Ng/(Ng-1) * term1 * term2 / numVoxels;

% Busyness
denom = 0;
for lev = 1:numLevels
    pShiftV = circshift(p,lev);
    indShiftV = circshift(indV,lev);
    usePv = p > 0;
    usePshiftV = pShiftV > 0;
    denom = denom + ...
        sum(usePv .* usePshiftV .* abs(p .* indV - pShiftV .* indShiftV));
end
featuresS.busyness = sum(p .* s) / denom;

% Complexity
complxty = 0;
for lev = 1:numLevels
    pShiftV = circshift(p,lev);
    sShiftV = circshift(s,lev);
    indShiftV = circshift(indV,lev);
    usePv = p > 0;
    usePshiftV = pShiftV > 0;
    term1 = abs(indV - indShiftV);
    term2 = usePv .* usePshiftV .* (p .* s + pShiftV .* sShiftV)...
        ./ (p + pShiftV + eps);
    complxty = complxty + sum(term1 .* term2);
end
featuresS.complexity = complxty / numVoxels;

% Texture strength
strength = 0;
for lev = 1:numLevels
    pShiftV = circshift(p,lev);
    indShiftV = circshift(indV,lev);
    usePv = p > 0;
    usePshiftV = pShiftV > 0;
    term = sum(usePv .* usePshiftV .* (p + pShiftV) .* (indV - indShiftV).^2);
    strength = strength + term;
end
featuresS.strength = strength / sum(s);


function err_total = error_depth_list(param_dist,xcn_list,xpn_list,R,T,X_shape_list,ind_list);

N_view = length(ind_list);

err_total = [];

N_pts = zeros(1,N_view);

for kk = 1:N_view,

   xcn = xcn_list{kk};
   xpn = xpn_list{kk};
   ind = ind_list{kk};

   xpn = xpn([1 3],:);

   X_shape = X_shape_list{kk};

X_new = depth_compute(xcn,xpn,[param_dist],R,T);

N_pt_calib = size(xcn,2);

% UnNormalized shape extraction:

X_shape2 = X_new;
X_shape2 = X_shape2 - (X_shape2(:,1)*ones(1,N_pt_calib));

% map the second vector at [1;0;0]:

omu = -cross([1;0;0],X_shape2(:,2));
omu = acos((dot([1;0;0],X_shape2(:,2)))/norm(X_shape2(:,2)))*(omu / norm(omu));
Ru = rodrigues(omu);

X_shape2 = Ru* X_shape2;

omu2 = -cross([0;1;0],[0;X_shape2(2:3,ind)]);
omu2 = acos((dot([0;1;0],[0;X_shape2(2:3,ind)]))/norm([0;X_shape2(2:3,ind)]))*(omu2 / norm(omu2));
Ru2 = rodrigues(omu2);

X_shape2 = Ru2* X_shape2;

% Error:

err_shape = X_shape2(:,2:end) - X_shape(:,2:end);

err_shape = err_shape(:);

N_pts(kk) = N_pt_calib;

err_total = [ err_total ; err_shape ];

end;

%err_depth = Z_new - Z_ref;


function catalan_row_next_test ( )

%*****************************************************************************80
%
%% CATALAN_ROW_NEXT_TEST tests CATALAN_ROW_NEXT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CATALAN_ROW_NEXT_TEST\n' );
  fprintf ( 1, '  CATALAN_ROW_NEXT computes a row of Catalan''s triangle.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  First, compute row 7:\n' );

  ido = 0;
  i = 7;
  c = [];
  c = catalan_row_next ( ido, i, c );
  fprintf ( 1, '  %2d  ', i );
  for j = 0 : i
    fprintf ( 1, '  %4d', c(j+1) );
  end
  fprintf ( 1, '\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now compute rows one at a time:\n' );
  fprintf ( 1, '\n' );

  ido = 0;
  c = [];

  for i = 0 : n
    c = catalan_row_next ( ido, i, c );
    ido = 1;
    fprintf ( 1, '  %2d  ', i );
    for j = 0 : i
      fprintf ( 1, '  %4d', c(j+1) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function[yf,mf]=yearfrac(num)
%YEARFRAC  Converts a DATENUM into 'year.fraction' and 'month.fraction'.
%
%   YF=YEARFRAC(NUM) where NUM is an array of dates in Matlab's 'datenum'
%   format, returns the fraction of the year at each date.
%
%   FLOOR(YF) returns the year.  Note that the actual number of days in
%   each year is used, including leap years.
%
%   YF=YEARFRAC(NUM) where NUM is a cell array of numeric arrays, also
%   works.  YF is then a cell array of the same size as NUM.
%
%   [YF,MF]=YEARFRAC(NUM) also returns MF, the fraction of the current
%   month at each date.  FLOOR(MF) is the standard month number.
%
%   Note NaNs and Infs in NUM are passed through to the same values in YF.
%
%   See also DATENUM, DATEVEC.
%
%   Usage: yf=yearfrac(num);
%          [yf,mf]=yearfrac(num);
%   _________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 1998--2020 J.M. Lilly --- type 'help jlab_license' for details

if ~iscell(num)
    if strcmpi(num, '--t')
        yf2num('--t'),return
    end
end

if iscell(num)
    for i=1:length(num)
        if ~isempty(num{i})
            [yf{i,1},mf{i,1}]=yearfrac1(num{i});
        else
            yf{i}=[];
            mf{i}=[];
        end
    end
else
    [yf,mf]=yearfrac1(num);
    yf=reshape(yf,size(num));
    mf=reshape(mf,size(num));
end

function[yf,mf]=yearfrac1(num)
yf=[];
if ~isempty(num)
    index=find(isnan(num));
    if ~isempty(index)
        num(index)=0;
    end
    infindex=find(isinf(num));
    if ~isempty(infindex)
        num(infindex)=0;
    end

    [y,mo,d,h,mi,s] = datevec(num);

    %Number of days in this year?
    nd=datenum(y,12,31)-datenum(y-1,12,31);
    na=datenum(y,mo,d,h,mi,s)-datenum(y-1,12,31)-1;
    %The minus one is because for Jan 1, I add nothing to year.fraction

    yf=y+na./nd;

    %Number of days in this month?
    nd=datenum(y,mo+1,1)-datenum(y,mo,1);  %Yes, this also works if mo = 12, for January
    mf=mo+((d-1)+h/24)./nd;

    if ~isempty(index)
      yf(index)=nan;
      mf(index)=nan;
    end
    if ~isempty(infindex)
      yf(infindex)=inf;
      mf(infindex)=inf;
    end
end

function[num]=yf2num(yf)
%YF2NUM  Convert date in 'year.fraction' format to 'datenum' format.
%
%   YF2NUM(YF) where YF is an array of dates in 'year.fraction' format
%   returns the array in Matlab's 'datenum' format.
%
%   See also YEARFRAC, DATENUM, DATEVEC.
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2000--2009 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(yf, '--t')
  yf2num_test,return
end

y=floor(yf);

%Number of days in this year?
d0=datenum(y-1,12,31);
d1=datenum(y,12,31);
nd=d1-d0;
num=d0+nd.*(yf-y)+1;

function[]=yf2num_test

yearf=(1850:(1/360):2004)';
num=yf2num(yearf);
yearf2=yearfrac(num);
bool=maxmax(abs(yearf-yearf2))<1e-10;
reporttest('YF2NUM and YEARFRAC, daily resolution, 1e-10 cutoff',bool)

yearf=(1990:(1/360/24):2004)';
%yearf=(1850:(1/360/24):2004)';
num=yf2num(yearf);
yearf2=yearfrac(num);
bool=maxmax(abs(yearf-yearf2))<1e-10;
reporttest('YF2NUM and YEARFRAC, hourly resolution, 1e-10 cutoff',bool)


function six_j_values_test ( )

%*****************************************************************************80
%
%% SIX_J_VALUES_TEST demonstrates the use of SIX_J_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SIX_J_VALUES_TEST:\n' );
  fprintf ( 1, '  SIX_J_VALUES returns values of \n' );
  fprintf ( 1, '  the Wigner 6J coefficient.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '      J1      J2      J3      J4      J5      J6        SIX_J\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, j1, j2, j3, j4, j5, j6, fx ] = six_j_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %6.2f  %6.2f  %6.2f  %6.2f  %6.2f  %6.2f  %24.16f\n', ...
      j1, j2, j3, j4, j5, j6, fx );

  end

  return
end


% MAIN - Pendulum
%
% Demonstrates simple swing-up for a single pendulum with a torque motor.
%

clc; clear;
addpath ../../

% Physical parameters of the pendulum
p.k = 1;  % Normalized gravity constant
p.c = 0.1;  % Normalized damping constant

% User-defined dynamics and objective functions
problem.func.dynamics = @(t,x,u)( dynamics(x,u,p) );
problem.func.pathObj = @(t,x,u)( u.^2 );

% Problem bounds
problem.bounds.initialTime.low = 0;
problem.bounds.initialTime.upp = 0;
problem.bounds.finalTime.low = 0.5;
problem.bounds.finalTime.upp = 2.5;

problem.bounds.state.low = [-2*pi; -inf];
problem.bounds.state.upp = [2*pi; inf];
problem.bounds.initialState.low = [0;0];
problem.bounds.initialState.upp = [0;0];
problem.bounds.finalState.low = [pi;0];
problem.bounds.finalState.upp = [pi;0];

problem.bounds.control.low = -5; %-inf;
problem.bounds.control.upp = 5; %inf;

% Guess at the initial trajectory
problem.guess.time = [0,1];
problem.guess.state = [0, pi; pi, pi];
problem.guess.control = [0, 0];

% Select a solver:
problem.options.method = 'trapezoid';
problem.options.defaultAccuracy = 'medium';

% Solve the problem
soln = optimTraj(problem);
t = soln.grid.time;
q = soln.grid.state(1,:);
dq = soln.grid.state(2,:);
u = soln.grid.control;

% Plot the solution:
figure(1); clf;

subplot(3,1,1)
plot(t,q)
ylabel('q')
title('Single Pendulum Swing-Up');

subplot(3,1,2)
plot(t,dq)
ylabel('dq')

subplot(3,1,3)
plot(t,u)
ylabel('u')


function [B,twom]=multicatdir_f(A,gamma,omega)
%MULTICATDIR_F  returns multilayer Leicht-Newman modularity matrix for categorical directed layers, function handle version
%
% Version: 2.2.0
% Date: Thu 11 Jul 2019 12:25:42 CEST
%
%   Input: A: Cell array of NxN adjacency matrices for each layer of a
%          categorical directed multilayer network
%          gamma: intralayer resolution parameter
%          omega: interlayer coupling strength
%
%   Output: B: function handle where B(i) returns the ith column of
%          [NxT]x[NxT] flattened modularity tensor for the
%           multilayer network with uniform ordinal coupling (T is
%           the number of layers of the network)
%           twom: normalisation constant
%
%   Example of usage: [B,twom]=multicatdir_f(A,gamma,omega);
%          [S,Q]= genlouvain(B); % see iterated_genlouvain.m and
%          postprocess_categorical_multilayer.m for how to improve output
%          multilayer partition
%          Q=Q/twom;
%          S=reshape(S,N,T);
%
%   [B,twom] = MULTICATDIR_F(A,GAMMA, OMEGA) with A a cell array of square
%   matrices of equal size each representing an directed network "layer"
%   computes the Leicht-Newman multilayer modularity matrix B using the
%   quality function described in Mucha et al. 2010, with intralayer
%   resolution parameter GAMMA, and with interlayer coupling OMEGA
%   connecting all-to-all categorical layers. Once the mulilayer modularity
%   matrix is computed, optimization can be performed by the generalized
%   Louvain code GENLOUVAIN or ITERATED_GENLOUVAIN. The output B can be used
%   with other heuristics, provided the same mapping is used to go from the
%   multilayer tensor to the multilayer flattened matrix. That is, the
%   node-layer tuple (i,s) is mapped to i + (s-1)*N. [Note that we can
%   define a mapping between a multilayer partition S_m stored as an N by T
%   matrix and the corresponding flattened partition S stored as an NT by 1
%   vector. In particular S_m = reshape(S,N,T) and S = S_m(:).]
%
%   See also
%       genlouvain heuristics:      GENLOUVAIN, ITERATED_GENLOUVAIN
%       multilayer wrappers:        MULTICAT, MULTICAT_F, MULTIORD
%       other heuristics:           SPECTRAL23
%       Kernighan-Lin improvement:  KLNB
%
%   Notes:
%     The matrices in the cell array A are assumed to be square,
%     and of equal size.  These assumptions are not checked here.
%
%     For smaller systems, it is potentially more efficient (and easier) to
%     directly use the sparse quality/modularity matrix B in MULTICAT. For
%     large systems with undirected layer networks, use MULTICAT_F.
%
%     This code serves as a template and can be modified for situations
%     with other wrinkles (e.g., different intralayer null models,
%     different numbers of nodes from layer-to-layer, or systems which are
%     both multiplex and longitudinal).  That is, this code is only a
%     starting point; it is by no means exhaustive.
%
%     By using this code, the user implicitly acknowledges that the authors
%     accept no liability associated with that use.  (What are you doing
%     with it anyway that might cause there to be a potential liability?!?)
%
%   References:
%     Blondel, Vincent D., Jean-Loup Guillaume, Renaud Lambiotte, and
%     Etienne Lefebvre, "Fast unfolding of communities in large networks,"
%     Journal of Statistical Mechanics: Theory and Experiment, P10008
%     (2008).
%
%     Fortunato, Santo, "Community detection in graphs," Physics Reports
%     486, 75-174 (2010).
%
%     Good, Benjamin H., Yves-Alexandre de Montjoye, and Aaron Clauset,
%     "Performance of modularity maximization in practical contexts,"
%     Physical Review E 81, 046106 (2010).
%
%     Mucha, Peter J., Thomas Richardson, Kevin Macon, Mason A. Porter, and
%     Jukka-Pekka Onnela. "Community Structure in Time-Dependent,
%     Multiscale, and Multiplex Networks," Science 328, 876-878 (2010).
%
%     Elizabeth A. Leicht and Mark E. J. Newman. "Community structure in
%     Directed Networks", Physical Review Letters 100, 118703 (2008).
%
%     Porter, M. A., J. P. Onnela, and P. J. Mucha, "Communities in
%     networks," Notices of the American Mathematical Society 56, 1082-1097
%     & 1164-1166 (2009).
%
%   Acknowledgments:
%     Thank you to Dani Bassett, Jesse Blocher, Bruce Rogers, and Simi Wang
%     for their collaborative help which led to significant cleaning up
%     of earlier versions of our multilayer community detection codes.

if nargin<2||isempty(gamma)
    gamma=1;
end

if nargin<3||isempty(omega)
    omega=1;
end

N=length(A{1});
T=length(A);

if length(gamma)==1
    gamma=repmat(gamma,T,1);
end

m=zeros(T,1);
for i=1:T
    m(i)=sum(A{i}(:));
end
A=blkdiag(A{:});
kout=sum(A,1);
koutmat=sparse(1:(N*T),kron(1:T,ones(1,N)),kout);
kin=sum(A,2);
kinmat=sparse(1:(N*T),kron(1:T,ones(1,N)),kin);
A=(A+A')./2;
all2all = N*[(-T+1):-1,1:(T-1)];
A = A + omega*spdiags(ones(N*T,2*T-2),all2all,N*T,N*T);

B=@(i) A(:,i)-gamma(ceil(i./(N+eps))).*(kout(i).*kinmat(:,ceil(i./(N+eps)))+kin(i).*koutmat(:,ceil(i./(N+eps))))./(2*m(ceil(i./(N+eps))));

twom=sum(m)+omega*2*N*(T-1);
end


% Initialization
clear all; close all; clc;
gen = spx.data.synthetic.MultiToneSignalGenerator();
gen.TotalDuration = 0.35;
% Sampling frequency
gen.SamplingFrequency = 1000;
% Frequencies (Hz)
gen.Frequencies = [113 247 327 413];
% Each frequency will be present in a segment of the
% overall signal duration.
% The segment is described by its origin and its duration.
gen.Origins = [0 0 0.030 0.150];
gen.Durations = [0.350 0.050 0.200 0.200];
% Amplitudes for each frequency
gen.Amplitudes = [1 1.7 1.9 1.8];

[x , t] = gen.run();
plot(t, x);