function [H, pValue, KSstatistic] = kstest_2s_2d(x1, x2, alpha)

% kstest_2s_2d - FUNCTION Two-sample Two-diensional Kolmogorov-Smirnov Test
%
% Usage:[H, pValue, KSstatistic] = kstest_2s_2d(x1, x2 <, alpha>)
%
% The paired-sample Kolmogorov-Smirnov test is a statistical test used to
% determine whether two sets of data arise from the same or different
% distributions.  The null hypothesis is that both data sets were drawn
% from the same continuous distribution.
%
% The algorithm in this function is taken from Peacock [1].
%
% 'x1' is an [Nx2] matrix, each row containing a two-dimensional sample.
% 'x2' is an [Mx2] matrix, each row likewise containing a two-dimensional
% sample. The optional argument 'alpha' is used to set the desired
% significance level for rejecting the null hypothesis.
%
% 'H' is a logical value: true indicates that the null hypothesis should be
% rejected.  'pValue' is an estimate for the P value of the test statistic.
% 'KSstatistic' is the raw value for the test statistic ('D' in [1]).
%
% In contrast to kstest2, this function can only perform a two-tailed test.
% This is because Peacock does not provide a method for estimating P in the
% one-tailed case [1].  Suggestions for a one-tailed test are welcome.
%
% References: [1] J. A. Peacock, "Two-dimensional goodness-of-fit testing
%  in astronomy", Monthly Notices Royal Astronomy Society 202 (1983)
%  615-627.
%    Available from: http://articles.adsabs.harvard.edu/cgi-bin/nph-iarticle_query?1983MNRAS.202..615P&defaultprint=YES&filetype=.pdf
%

% Author: Dylan Muir (From kstest_2s_2d by Qiuyan Peng @ ECE/HKUST) Date:
% 13th October, 2012

%%

if nargin < 2
   error('stats:kstest2:TooFewInputs','At least 2 inputs are required.');
end

%%
%
% x1,x2 are both 2-column matrices
%

if ((size(x1,2)~=2)||(size(x2,2)~=2))
   error('stats:kstest2:TwoColumnMatrixRequired','The samples X1 and X2 must be two-column matrices.');
end
n1 = size(x1,1);
n2 = size(x2,1);

%%
%
% Ensure the significance level, ALPHA, is a scalar
% between 0 and 1 and set default if necessary.
%

if (nargin >= 3) && ~isempty(alpha)
   if ~isscalar(alpha) || (alpha <= 0 || alpha >= 1)
      error('stats:kstest2:BadAlpha',...
         'Significance level ALPHA must be a scalar between 0 and 1.');
   end
else
   alpha  =  0.05;
end

%%
%
% Calculate F1(x) and F2(x), the empirical (i.e., sample) CDFs.
%

% - A function handle to perform comparisons in all possible directions
fhCounts = @(x, edge)([(x(:, 1) >= edge(1)) & (x(:, 2) >= edge(2))...
   (x(:, 1) <= edge(1)) & (x(:, 2) >= edge(2))...
   (x(:, 1) <= edge(1)) & (x(:, 2) <= edge(2))...
   (x(:, 1) >= edge(1)) & (x(:, 2) <= edge(2))]);

KSstatistic = -inf;

for iX = 1:(n1+n2)
   % - Choose a starting point
   if (iX<=n1)
      edge = x1(iX,:);
   else
      edge = x2(iX-n1,:);
   end

   % - Estimate the CDFs for both distributions around this point
   vfCDF1 = sum(fhCounts(x1, edge)) ./ n1;
   vfCDF2 = sum(fhCounts(x2, edge)) ./ n2;

   % - Two-tailed test statistic
   vfThisKSTS = abs(vfCDF1 - vfCDF2);
   fKSTS = max(vfThisKSTS);

   % - Final test statistic is the maximum absolute difference in CDFs
   if (fKSTS > KSstatistic)
      KSstatistic = fKSTS;
   end
end

%% Peacock Z calculation and P estimation

n      =  n1 * n2 /(n1 + n2);
Zn = sqrt(n) * KSstatistic;
Zinf = Zn / (1 - 0.53 * n^(-0.9));
pValue = 2 * exp(-2 * (Zinf - 0.5).^2);

% Clip invalid values for P
if (pValue > 0.2)
   pValue = 0.2;
end

H = (pValue <= alpha);


% MTSP_GA Multiple Traveling Salesmen Problem (M-TSP) Genetic Algorithm (GA)
%   Finds a (near) optimal solution to the M-TSP by setting up a GA to search
%   for the shortest route (least distance needed for the salesmen to travel
%   to each city exactly once and return to their starting locations)
%
% Summary:
%     1. Each salesman travels to a unique set of cities and completes the
%        route by returning to the city he started from
%     2. Each city is visited by exactly one salesman
%
% Input:
%     XY (float) is an Nx2 matrix of city locations, where N is the number of cities
%     DMAT (float) is an NxN matrix of city-to-city distances or costs
%     NSALESMEN (scalar integer) is the number of salesmen to visit the cities
%     MINTOUR (scalar integer) is the minimum tour length for any of the salesmen
%     POPSIZE (scalar integer) is the size of the population (should be divisible by 8)
%     NUMITER (scalar integer) is the number of desired iterations for the algorithm to run
%     SHOWPROG (scalar logical) shows the GA progress if true
%     SHOWRESULT (scalar logical) shows the GA results if true
%
% Output:
%     OPTROUTE (integer array) is the best route found by the algorithm
%     OPTBREAK (integer array) is the list of route break points (these specify the indices
%         into the route used to obtain the individual salesman routes)
%     MINDIST (scalar float) is the total distance traveled by the salesmen
%
% Route/Breakpoint Details:
%     If there are 10 cities and 3 salesmen, a possible route/break
%     combination might be: rte = [5 6 9 1 4 2 8 10 3 7], brks = [3 7]
%     Taken together, these represent the solution [5 6 9][1 4 2 8][10 3 7],
%     which designates the routes for the 3 salesmen as follows:
%         . Salesman 1 travels from city 5 to 6 to 9 and back to 5
%         . Salesman 2 travels from city 1 to 4 to 2 to 8 and back to 1
%         . Salesman 3 travels from city 10 to 3 to 7 and back to 10
%
% Example:
%     n = 35;
%     xy = 10*rand(n,2);
%     nSalesmen = 5;
%     minTour = 3;
%     popSize = 80;
%     numIter = 5e3;
%     a = meshgrid(1:n);
%     dmat = reshape(sqrt(sum((xy(a,:)-xy(a',:)).^2,2)),n,n);
%     [optRoute,optBreak,minDist] = mtsp_ga(xy,dmat,nSalesmen,minTour, ...
%         popSize,numIter,1,1);
%
% Example:
%     n = 50;
%     phi = (sqrt(5)-1)/2;
%     theta = 2*pi*phi*(0:n-1);
%     rho = (1:n).^phi;
%     [x,y] = pol2cart(theta(:),rho(:));
%     xy = 10*([x y]-min([x;y]))/(max([x;y])-min([x;y]));
%     nSalesmen = 5;
%     minTour = 3;
%     popSize = 80;
%     numIter = 1e4;
%     a = meshgrid(1:n);
%     dmat = reshape(sqrt(sum((xy(a,:)-xy(a',:)).^2,2)),n,n);
%     [optRoute,optBreak,minDist] = mtsp_ga(xy,dmat,nSalesmen,minTour, ...
%         popSize,numIter,1,1);
%
% Example:
%     n = 35;
%     xyz = 10*rand(n,3);
%     nSalesmen = 5;
%     minTour = 3;
%     popSize = 80;
%     numIter = 5e3;
%     a = meshgrid(1:n);
%     dmat = reshape(sqrt(sum((xyz(a,:)-xyz(a',:)).^2,2)),n,n);
%     [optRoute,optBreak,minDist] = mtsp_ga(xyz,dmat,nSalesmen,minTour, ...
%         popSize,numIter,1,1);
%
% See also: tsp_ga, mtspf_ga, mtspo_ga, mtspof_ga, mtspofs_ga, mtspv_ga, distmat
%
% Author: Joseph Kirk
% Email: jdkirk630@gmail.com
% Release: 1.5
% Release Date: 11/07/11
function varargout = mtsp_ga(xy,dmat,nSalesmen,minTour,popSize,numIter,showProg,showResult)

% Process Inputs and Initialize Defaults
nargs = 8;
for k = nargin:nargs-1
    switch k
        case 0
            xy = 10*rand(40,2);
        case 1
            N = size(xy,1);
            a = meshgrid(1:N);
            dmat = reshape(sqrt(sum((xy(a,:)-xy(a',:)).^2,2)),N,N);
        case 2
            nSalesmen = 5;
        case 3
            minTour = 3;
        case 4
            popSize = 80;
        case 5
            numIter = 5e3;
        case 6
            showProg = 1;
        case 7
            showResult = 1;
        otherwise
    end
end

% Verify Inputs
[N,dims] = size(xy);
[nr,nc] = size(dmat);
if N ~= nr || N ~= nc
    error('Invalid XY or DMAT inputs!')
end
n = N;

% Sanity Checks
nSalesmen = max(1,min(n,round(real(nSalesmen(1)))));
minTour = max(1,min(floor(n/nSalesmen),round(real(minTour(1)))));
popSize = max(8,8*ceil(popSize(1)/8));
numIter = max(1,round(real(numIter(1))));
showProg = logical(showProg(1));
showResult = logical(showResult(1));

% Initializations for Route Break Point Selection
nBreaks = nSalesmen-1;
dof = n - minTour*nSalesmen;          % degrees of freedom
addto = ones(1,dof+1);
for k = 2:nBreaks
    addto = cumsum(addto);
end
cumProb = cumsum(addto)/sum(addto);

% Initialize the Populations
popRoute = zeros(popSize,n);         % population of routes
popBreak = zeros(popSize,nBreaks);   % population of breaks
popRoute(1,:) = (1:n);
popBreak(1,:) = rand_breaks();
for k = 2:popSize
    popRoute(k,:) = randperm(n);
    popBreak(k,:) = rand_breaks();
end

% Select the Colors for the Plotted Routes
pclr = ~get(0,'DefaultAxesColor');
clr = [1 0 0; 0 0 1; 0.67 0 1; 0 1 0; 1 0.5 0];
if nSalesmen > 5
    clr = hsv(nSalesmen);
end

% Run the GA
globalMin = Inf;
totalDist = zeros(1,popSize);
distHistory = zeros(1,numIter);
tmpPopRoute = zeros(8,n);
tmpPopBreak = zeros(8,nBreaks);
newPopRoute = zeros(popSize,n);
newPopBreak = zeros(popSize,nBreaks);
if showProg
    pfig = figure('Name','MTSP_GA | Current Best Solution','Numbertitle','off');
end
for iter = 1:numIter
    % Evaluate Members of the Population
    for p = 1:popSize
        d = 0;
        pRoute = popRoute(p,:);
        pBreak = popBreak(p,:);
        rng = [[1 pBreak+1];[pBreak n]]';
        for s = 1:nSalesmen
            d = d + dmat(pRoute(rng(s,2)),pRoute(rng(s,1)));
            for k = rng(s,1):rng(s,2)-1
                d = d + dmat(pRoute(k),pRoute(k+1));
            end
        end
        totalDist(p) = d;
    end

    % Find the Best Route in the Population
    [minDist,index] = min(totalDist);
    distHistory(iter) = minDist;
    if minDist < globalMin
        globalMin = minDist;
        optRoute = popRoute(index,:);
        optBreak = popBreak(index,:);
        rng = [[1 optBreak+1];[optBreak n]]';
        if showProg
            % Plot the Best Route
            figure(pfig);
            for s = 1:nSalesmen
                rte = optRoute([rng(s,1):rng(s,2) rng(s,1)]);
                if dims > 2, plot3(xy(rte,1),xy(rte,2),xy(rte,3),'.-','Color',clr(s,:));
                else plot(xy(rte,1),xy(rte,2),'.-','Color',clr(s,:)); end
                title(sprintf('Total Distance = %1.4f, Iteration = %d',minDist,iter));
                hold on
            end
            hold off
        end
    end

    % Genetic Algorithm Operators
    randomOrder = randperm(popSize);
    for p = 8:8:popSize
        rtes = popRoute(randomOrder(p-7:p),:);
        brks = popBreak(randomOrder(p-7:p),:);
        dists = totalDist(randomOrder(p-7:p));
        [ignore,idx] = min(dists); %#ok
        bestOf8Route = rtes(idx,:);
        bestOf8Break = brks(idx,:);
        routeInsertionPoints = sort(ceil(n*rand(1,2)));
        I = routeInsertionPoints(1);
        J = routeInsertionPoints(2);
        for k = 1:8 % Generate New Solutions
            tmpPopRoute(k,:) = bestOf8Route;
            tmpPopBreak(k,:) = bestOf8Break;
            switch k
                case 2 % Flip
                    tmpPopRoute(k,I:J) = tmpPopRoute(k,J:-1:I);
                case 3 % Swap
                    tmpPopRoute(k,[I J]) = tmpPopRoute(k,[J I]);
                case 4 % Slide
                    tmpPopRoute(k,I:J) = tmpPopRoute(k,[I+1:J I]);
                case 5 % Modify Breaks
                    tmpPopBreak(k,:) = rand_breaks();
                case 6 % Flip, Modify Breaks
                    tmpPopRoute(k,I:J) = tmpPopRoute(k,J:-1:I);
                    tmpPopBreak(k,:) = rand_breaks();
                case 7 % Swap, Modify Breaks
                    tmpPopRoute(k,[I J]) = tmpPopRoute(k,[J I]);
                    tmpPopBreak(k,:) = rand_breaks();
                case 8 % Slide, Modify Breaks
                    tmpPopRoute(k,I:J) = tmpPopRoute(k,[I+1:J I]);
                    tmpPopBreak(k,:) = rand_breaks();
                otherwise % Do Nothing
            end
        end
        newPopRoute(p-7:p,:) = tmpPopRoute;
        newPopBreak(p-7:p,:) = tmpPopBreak;
    end
    popRoute = newPopRoute;
    popBreak = newPopBreak;
end

if showResult
% Plots
    figure('Name','MTSP_GA | Results','Numbertitle','off');
    subplot(2,2,1);
    if dims > 2, plot3(xy(:,1),xy(:,2),xy(:,3),'.','Color',pclr);
    else plot(xy(:,1),xy(:,2),'.','Color',pclr); end
    title('City Locations');
    subplot(2,2,2);
    imagesc(dmat(optRoute,optRoute));
    title('Distance Matrix');
    subplot(2,2,3);
    rng = [[1 optBreak+1];[optBreak n]]';
    for s = 1:nSalesmen
        rte = optRoute([rng(s,1):rng(s,2) rng(s,1)]);
        if dims > 2, plot3(xy(rte,1),xy(rte,2),xy(rte,3),'.-','Color',clr(s,:));
        else plot(xy(rte,1),xy(rte,2),'.-','Color',clr(s,:)); end
        title(sprintf('Total Distance = %1.4f',minDist));
        hold on;
    end
    subplot(2,2,4);
    plot(distHistory,'b','LineWidth',2);
    title('Best Solution History');
    set(gca,'XLim',[0 numIter+1],'YLim',[0 1.1*max([1 distHistory])]);
end

% Return Outputs
if nargout
    varargout{1} = optRoute;
    varargout{2} = optBreak;
    varargout{3} = minDist;
end

    % Generate Random Set of Break Points
    function breaks = rand_breaks()
        if minTour == 1 % No Constraints on Breaks
            tmpBreaks = randperm(n-1);
            breaks = sort(tmpBreaks(1:nBreaks));
        else % Force Breaks to be at Least the Minimum Tour Length
            nAdjust = find(rand < cumProb,1)-1;
            spaces = ceil(nBreaks*rand(1,nAdjust));
            adjust = zeros(1,nBreaks);
            for kk = 1:nBreaks
                adjust(kk) = sum(spaces == kk);
            end
            breaks = minTour*(1:nBreaks) + cumsum(adjust);
        end
    end
end


function [PR, PN, PC, PL] = subspaceProjector(A, printLevel, sub_space)
% Returns the matrix for projection onto the subspace of matrix A,
% specified by `sub_space`
%   * 'all'
%   * 'R' row space         i.e. range(A')
%   * 'N' nullspace         i.e. null(A)
%   * 'C' column space      i.e. range(A)
%   * 'L' left nullspace    i.e. null(A')
%
% Let M denote the Moore-Penrose pseudoinverse of the A and the subscripts are the following
% `_R` row space        i.e. range(A')
% `_N` nullspace        i.e. null(A)
% `_C` column space     i.e. range(A)
% `_L` left nullspace   i.e. null(A')
%
% Let
%
% .. math::
%    v   &= v_R + v_N \\
%    v_R &= M A v = PR v \\
%    v_N &= (I - M A) v = PN v
%
% Let
%
% .. math::
%    u   &= u_C + u_L \\
%    u_C &= A M u = PC u \\
%    u_L &= (I - A M) u = PL u
%
% Examples:
%
% Given :math:`A v = b`, then :math:`v_R = M b`
%
% Given :math:`A^Tu = q`, then :math:`u_C = M^T q`
%
% USAGE:
%
%    [PR, PN, PC, PL] = subspaceProjector(model, printLevel, sub_space)
%
% INPUT:
%    A          `m x n` matrix
%
% OPTIONAL INPUTS:
%    printLevel:          {(1), 0}, 1 = print diagnostics, 0 = silent
%    sub_space:           returns projection matrices onto all or one select
%
%                           * sub_space
%                           * 'all'
%                           * 'R' row space
%                           * 'N' nullspace
%                           * 'C' column space
%                           * 'L' left nullspace
%
% OUTPUTS:
%    [PR, PN, PC, PL]:    matrices for projection onto the row, null,
%                         column and left nullspace of A, respectively
%
% .. Author:
%       - 10 July 2009 : Ronan Fleming. First Version.
%       - 10 Aug  2009 : Changed to use Micheal Saunders faster approach
%       -    Jan  2018 : Changed to take a matrix A

if ~exist('printLevel','var')
    printLevel=1;
end

if ~exist('sub_space','var')
    sub_space='all';
end

[nMet,nRxn]=size(A);

archstr = computer('arch');
switch archstr
    case {'glnx86','glnxa64'}
        %A = U1*D1*V1'
        if printLevel
            fprintf('%s','Calculating SVD ...');
            tic
        end
        %Michael Saunders code
        [U1,D1,V1,r] = subspaceSVD(A);
        if printLevel
            fprintf('%s\n',[' finished. toc = ' num2str(toc)]);
        end
        PR=[];PN=[];PC=[];PL=[];
        if strcmp(sub_space,'R')
            PR=V1*V1';
        elseif strcmp(sub_space,'N')
            PN=eye(nRxn) - V1*V1';
        elseif strcmp(sub_space,'C')
            PC=U1*U1';
        elseif strcmp(sub_space,'L')
            PL=eye(nMet) - U1*U1';
        elseif strcmp(sub_space,'all')
            PR=V1*V1';
            PN=eye(nRxn) - V1*V1';
            PC=U1*U1';
            PL=eye(nMet) - U1*U1';
        end
    otherwise
        %for other architectures calculate the Moore-Penrose Pseudoinverse
        if printLevel
            fprintf('%s','Calculating the Moore-Penrose Pseudoinverse...');
            tic
        end
        M=pinv(full(A));
        if printLevel
            fprintf('%s\n',[' finished. toc = ' num2str(toc)]);
        end

        PR=[];PN=[];PC=[];PL=[];
        if strcmp(sub_space,'R')
            PR=M*A;
        elseif strcmp(sub_space,'N')
            PN=eye(nRxn)-M*A;
        elseif strcmp(sub_space,'C')
            PC=A*M;
        elseif strcmp(sub_space,'L')
            PL=eye(nMet)-A*M;
        elseif strcmp(sub_space,'all')
            PR=M*A;
            PN=eye(nRxn)-M*A;
            PC=A*M;
            PL=eye(nMet)-A*M;
        end
end


function [pID,pN] = FDR(p,q)
% :Usage:
% ::
%
%     pt = FDR(p,q)
%
% :Inputs:
%
%   **p:**
%        vector of p-values
%
%   **q:**
%        False Discovery Rate level
%
% :Outputs:
%
%   **pID:**
%        p-value threshold based on independence or positive dependence
%
%   **pN:**
%        Nonparametric p-value threshold
%
% ..
%    % @(#)FDR.m	1.3 Tom Nichols 02/01/18
%
%    The checking code below was added by Tor Wager
% ..

    p(isnan(p)) = [];

    if any(p == 0)
        disp('******************************************')
        disp('Warning! Some p-values are zero.')
        disp('FDR.m will interpret these as ineligible voxels.')
        disp('If these are valid p-values, they should have some not-exactly-zero value.')
        p(p == 0) = [];
        disp('******************************************')

    end

    p = sort(p(:));
    V = length(p);
    I = (1:V)';

    cVID = 1;
    cVN = sum(1./(1:V));

    pID = p(max(find(p<=I/V*q/cVID)));
    pN = p(max(find(p<=I/V*q/cVN)));

    return


% jCell:  Tools for operating on cell arrays of column vectors
%
% Basic mathematical operations
%   cellabs    - Absolute value of each element in a cell array.
%   cellmax    - Maximum of each element in a cell array.
%   cellmin    - Minimum of each element in a cell array.
%   cellsum    - Sum of each element a cell array, possibly weighted.
%   cellmean   - Mean value of each element a cell array, possibly weighted.
%   cellstd    - Standard deviation of each element a cell array, possibly weighted.
%   cellmed    - Median value of each element a cell array.
%   cellreal   - Real part of each element in a cell array.
%   cellimag   - Imaginary part of each element in a cell array.
%   cellpair   - Complex pairing for elements in a cell array.
%   celllog10  - Base ten logarithm of each element in a cell array.
%   celladd    - Addition acting on each element in a cell array.
%   cellmult   - Multiplication acting on each element in a cell array.
%   celldiv    - Division acting on each element in a cell array.
%   celldot    - Dot product for arrays of column vectors.
%
% Reshaping, indexing, and sizes
%   cell2col   - Converts cell arrays of numeric arrays into 'column-appended' form.
%   col2cell   - Converts 'column-appended' data into cell arrays of numeric arrays.
%   cellindex  - Applies a cell array of indices to a cell array of column vectors.
%   cellchunk  - Converts cell array data into uniform length 'chunks'.
%   cellength  - Length of each element in a cell array.
%   cellsize   - Size of each element in a cell array along specified dimension.
%   cellget    - Indexes a cell array of numerical arrays by ID number.
%   cellimit   - Limits the ranges of times in a cell array of numerical arrays.
%
% Data processing
%   cellstrip  - Strips INF values from the beginnings or ends of cell arrays.
%   cellsplit  - Splits cell arrays of numeric arrays at data gaps.
%   cellprune  - Removes all empty cells, or cells less than a specified length.
%   cellpack   - Removes all INF values from cell arrays of numeric arrays.
%   cellfill   - Fills missing data marked by INFs in a cell array.
%   cellgrid   - Interpolate a cell array of numeric arrays onto a regular grid.
%   cellfirst  - Returns the first element of each entry in a cell array.
%
% Plotting
%   cellplot   - Rapidly plot all element of a cell array of numeric arrays.
%
% See also jVarfun.
help jCell


function geometry_test1836 ( )

%*****************************************************************************80
%
%% GEOMETRY_TEST1836 tests SPHERE_EXP2IMP_ND.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 July 2011
%
%  Author:
%
%    John Burkardt
%
  n = 3;

  p = [ ...
    4.0, 2.0, 3.0;
    1.0, 5.0, 3.0;
    1.0, 2.0, 6.0;
   -2.0, 2.0, 3.0 ]';
  pc_true = [ 1.0, 2.0, 3.0 ]';
  r_true = 3.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'GEOMETRY_TEST1836\n' );
  fprintf ( 1, '  SPHERE_EXP2IMP_ND: explicit sphere => implicit form;\n' );

  r8mat_transpose_print ( n, n + 1, p, '  Initial form of explicit sphere:' );

  [ r, pc ] = sphere_exp2imp_nd ( n, p );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Computed form of implicit sphere:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Imputed radius = %f\n', r );
  fprintf ( 1, '  True radius =    %f\n', r_true );

  r8vec_print ( n, pc, '  Imputed center' );

  r8vec_print ( n, pc_true, '  True center' );

  return
end


function [ n_data, n, fn ] = i4_factorial_values ( n_data )

%*****************************************************************************80
%
%% I4_FACTORIAL_VALUES returns values of the factorial function.
%
%  Discussion:
%
%    0! = 1
%    I! = Product ( 1 <= J <= I ) I
%
%    In Mathematica, the function can be evaluated by:
%
%      n!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the argument of the function.
%
%    Output, integer FN, the value of the function.
%
  n_max = 13;

  fn_vec = [ ...
            1, ...
            1, ...
            2, ...
            6, ...
           24, ...
          120, ...
          720, ...
         5040, ...
        40320, ...
       362880, ...
      3628800, ...
     39916800, ...
    479001600 ];

  n_vec = [ ...
     0,  1,  2,  3, ...
     4,  5,  6,  7, ...
     8,  9, 10, 11, ...
    12 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    fn = 0;
  else
    n = n_vec(n_data);
    fn = fn_vec(n_data);
  end

  return
end


% Detailed description of the factor data structure and related functions
% -----------------------------------------------------------------------
% We will use structures to implement the factor datatype. The code
%
%   phi = struct('var', [3 1 2], 'card', [2 2 2], 'val', ones(1, 8));
%
% creates a factor over variables X_3, X_1, X_2, which are all binary
% valued, because phi.card(1) (the cardinality of X_3, |Val(X_3)|) is 2,
% and likewise for X_1 and X_2. phi has been initialized so that
% phi(X_3, X_1, X_2) = 1 for any assignment to the variables.
%
% A factor's values are stored in a row vector in the .val field
% using an ordering such that the left-most variables as defined in the
% .var field cycle through their values the fastest. More concretely, for
% the factor phi defined above, we have the following mapping from variable
% assignments to the index of the row vector in the .val field:
%
% -+-----+-----+-----+-------------------+
%  | X_3 | X_1 | X_2 | phi(X_3, X_1, X_2)|
% -+-----+-----+-----+-------------------+
%  |  1  |  1  |  1  |     phi.val(1)    |
% -+-----+-----+-----+-------------------+
%  |  2  |  1  |  1  |     phi.val(2)    |
% -+-----+-----+-----+-------------------+
%  |  1  |  2  |  1  |     phi.val(3)    |
% -+-----+-----+-----+-------------------+
%  |  2  |  2  |  1  |     phi.val(4)    |
% -+-----+-----+-----+-------------------+
%  |  1  |  1  |  2  |     phi.val(5)    |
% -+-----+-----+-----+-------------------+
%  |  2  |  1  |  2  |     phi.val(6)    |
% -+-----+-----+-----+-------------------+
%  |  1  |  2  |  2  |     phi.val(7)    |
% -+-----+-----+-----+-------------------+
%  |  2  |  2  |  2  |     phi.val(8)    |
% -+-----+-----+-----+-------------------+
%
%
% We have provided the AssignmentToIndex and IndexToAssignment functions
% that compute the mapping between the assignments A and the variable indices I,
% given D, the cardinality of the variables. Concretely, given a factor phi, if
% phi.val(I) corresponds to the assignment A, i.e. phi(X = A) = phi.val(I) then
%
%   I = AssignmentToIndex(A, D)
%   A = IndexToAssignment(I, D)
%
% For instance, for the factor phi as defined above, with the assignment
%
%    A = [2 1 2]
%
% to X_3, X_1 and X_2 respectively (as defined by phi.var = [3 1 2]), I = 6
% as phi.val(6) corresponds to the value of phi(X_3 = 2, X_1 = 1, X_2 = 2).
% Thus, AssignmentToIndex([2 1 2], [2 2 2]) returns 6, and conversely,
% IndexToAssignment(6, [2 2 2]) returns the vector [2 1 2]. The second
% argument in the function calls corresponds to the cardinality of the
% sample factor phi, phi.card, which is [2 2 2].
%
% More generally, the assignment vector A is a row vector that corresponds
% to assignments to the variables in a factor, with an understanding that the
% variables for which the assignments refer to are given by the .var field
% of the factor.
%
% Giving AssignmentToIndex a matrix A, one assignment per row, will cause it
% to return a vector of indices I, such that I(k) is the index
% corresponding to the assignment in A(k, :) (row k).
%
% Similarly, giving IndexToAssignment a vector I of indices will yield a
% matrix A of assignments, one per row, such that A(k, :) (the kth row of A)
% corresponds to the assignment mapped to by index I(k).
%
% Getting and setting values to factors
% -------------------------------------
%
% We have provided the convenience functions GetValueOfAssignment and
% SetValueOfAssignment so you do not need to manipulate the .val field
% directly for getting and setting values.
%
% For instance, calling
%
%   GetValueOfAssignment(phi, [1 2 1])
%
% yields the value phi(X_3 = 1, X_1 = 2, X_2 = 1). Again, the variables for
% which the assignments refer to are given by the .var field of the factor.
%
% Similarly, executing
%
%    phi = SetValueOfAssignment(phi, [2 2 1], 6)
%
% causes the value of phi(X_3 = 2, X_1 = 2, X_2 = 1) to be set to 6. Note
% that because MATLAB/Octave passes function arguments by value (not by
% reference), SetValueOfAssignment *does not modify* the factor that you
% passed in. Instead, it returns a new factor with the modified value for
% the specified assignment; this is why we reassigned phi to the result of
% SetValueOfAssignment.
%
% More details about these functions are provided in their respective .m
% files.

% Sample Factors and Outputs
% --------------------------
%
% In the following, we have provided you with some sample input factors, as
% well as the output that you should receive when various operations are
% performed on these factors. You may find these sample inputs and outputs
% helpful in debugging your implementation. For instance, FACTORS.PRODUCT
% is the factor you should get when you execute
%
%   FactorProduct(FACTORS.INPUT(1), FACTORS.INPUT(2))
%
% These sample factors define a simple chain Bayesian network over binary
% variables: X_1 -> X_2 -> X_3
%

% FACTORS.INPUT(1) contains P(X_1)
FACTORS.INPUT(1) = struct('var', [1], 'card', [2], 'val', [0.11, 0.89]);

% FACTORS.INPUT(2) contains P(X_2 | X_1)
FACTORS.INPUT(2) = struct('var', [2, 1], 'card', [2, 2], 'val', [0.59, 0.41, 0.22, 0.78]);

% FACTORS.INPUT(3) contains P(X_3 | X_2)
FACTORS.INPUT(3) = struct('var', [3, 2], 'card', [2, 2], 'val', [0.39, 0.61, 0.06, 0.94]);

% Factor Product
% FACTORS.PRODUCT = FactorProduct(FACTORS.INPUT(1), FACTORS.INPUT(2));
% The factor defined here is correct to 4 decimal places.
FACTORS.PRODUCT = struct('var', [1, 2], 'card', [2, 2], 'val', [0.0649, 0.1958, 0.0451, 0.6942]);

% Factor Marginalization
% FACTORS.MARGINALIZATION = FactorMarginalization(FACTORS.INPUT(2), [2]);
FACTORS.MARGINALIZATION = struct('var', [1], 'card', [2], 'val', [1 1]);

% Observe Evidence
% FACTORS.EVIDENCE = ObserveEvidence(FACTORS.INPUT, [2 1; 3 2]);
FACTORS.EVIDENCE(1) = struct('var', [1], 'card', [2], 'val', [0.11, 0.89]);
FACTORS.EVIDENCE(2) = struct('var', [2, 1], 'card', [2, 2], 'val', [0.59, 0, 0.22, 0]);
FACTORS.EVIDENCE(3) = struct('var', [3, 2], 'card', [2, 2], 'val', [0, 0.61, 0, 0]);

% Compute Joint Distribution
% FACTORS.JOINT = ComputeJointDistribution(FACTORS.INPUT);
FACTORS.JOINT = struct('var', [1, 2, 3], 'card', [2, 2, 2], 'val', [0.025311, 0.076362, 0.002706, 0.041652, 0.039589, 0.119438, 0.042394, 0.652548]);

% Compute Marginal
%FACTORS.MARGINAL = ComputeMarginal([2, 3], FACTORS.INPUT, [1, 2]);
FACTORS.MARGINAL = struct('var', [2, 3], 'card', [2, 2], 'val', [0.0858, 0.0468, 0.1342, 0.7332]);

%p = ComputeJointDistribution(FACTORS.INPUT)


function B = blockrot90(A, p, q, k)
%BLOCKROT90 Perform 90 degree block rotation on arrays.
%
%   BLOCKROT90(A,P,Q) is the 90 degree anticlockwise rotation of all P-by-Q
%   submatrices of the matrix A.
%
%   BLOCKROT90(A,P,Q,K) is the K*90 degree rotation of all P-by-Q submatrices
%   of A, K = +-1,+-2,...
%
%   BLOCKROT90 is vectorized along higher dimensions, so the same operations is
%   performed on all 2-D slices A(:,:,I,J,K,...).
%
%   See also ROT90.

%   Author:      Peter J. Acklam
%   Time-stamp:  2003-10-13 13:25:20 +0200
%   E-mail:      pjacklam@online.no
%   URL:         http://home.online.no/~pjacklam

   nargsin = nargin;
   error(nargchk(3, 4, nargsin));

   z = size(A);
   if nargsin < 4
      k = 1;
   else
      if any(size(k) ~= 1)
         error('K must be a scalar.');
      end
      k = k - 4*floor(k/4);     % map k to {0,1,2,3}
   end

   r = z(1)/p;
   s = z(2)/q;
   if (r ~= round(r)) | (s ~= round(s))
      error('A can not be subdivided into P-by-Q submatrices.');
   end

   u = [ p r q prod(z)/(z(1)*q) ];
   v = [ q*r p*s z(3:end) ];

   switch k
      case 1                    % 90 degrees anticlockwise
         B = reshape(A, u);
         B = reshape(permute(B(:,:,q:-1:1,:), [ 3 2 1 4 ]), v);
      case 2                    % 180 degrees
         B = reshape(A, u);
         B = reshape(B(p:-1:1,:,q:-1:1,:), z);
      case 3                    % 90 degrees clockwise
         B = reshape(A, u);
         B = reshape(permute(B(p:-1:1,:,:,:), [ 3 2 1 4 ]), v);
      otherwise                 % 0 degrees
         B = A;
   end


function x = ffwt ( n, x )

%*****************************************************************************80
%
%% FFWT performs an in-place fast Walsh transform.
%
%  Discussion:
%
%    This routine performs a fast Walsh transform on an input series X
%    leaving the transformed results in X.
%    X is dimensioned N, which must be a power of 2.
%    The results of this Walsh transform are in sequency order.
%
%    The output sequence could be normalized by dividing by N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2011
%
%  Author:
%
%    Ken Beauchamp
%
%  Reference:
%
%    Ken Beauchamp,
%    Walsh functions and their applications,
%    Academic Press, 1975,
%    ISBN: 0-12-084050-2,
%    LC: QA404.5.B33.
%
%  Parameters:
%
%    Input, integer N, the number of items in X.
%    N must be a power of 2.
%
%    Input, real X(N), the data to be transformed.
%
%    Output, real X(N), the transformed data.
%
  m = i4_log_2 ( n );

  for i = 1 : m
    two_power(i) = 2^( m - i );
  end

  for l = 1 : m

    nz = 2^( l - 1 );
    nzi = 2 * nz;
    nzn = floor ( n / nzi );
    nz2 = floor ( nz / 2 );
    if ( nz2 == 0 )
      nz2 = 1;
    end

    for i = 1 : nzn

      js = ( i - 1 ) * nzi;
      z = 1.0;

      for ii = 1 : 2

        for j = 1 : nz2
          js = js + 1;
          j2 = js + nz;
          hold = x(js) + z * x(j2);
          z = - z;
          x(j2) = x(js) + z * x(j2);
          x(js) = hold;
          z = - z;
        end

        if ( l == 1 )
          break
        end

        z = - 1.0;

      end

    end
  end
%
%  Bit reversal section.
%
  nw = 0;
  for k = 1 : n
%
%  Choose correct index and switch elements if not already switched.
%
    if ( k < nw + 1 )
      hold = x(nw+1);
      x(nw+1) = x(k);
      x(k) = hold;
    end
%
%  Bump up series by 1.
%
    for i = 1 : m

      ii = i;
      if ( nw < two_power(i) )
        break
      end
      mw = floor ( nw / two_power(i) );
      mw1 = floor ( mw / 2 );
      if ( mw <= 2 * mw1 )
        break
      end

      nw = nw - two_power(i);

    end

    nw = nw + two_power(ii);

  end

  return
end


function [all_theta] = oneVsAll(X, y, num_labels, lambda)
%ONEVSALL trains multiple logistic regression classifiers and returns all
%the classifiers in a matrix all_theta, where the i-th row of all_theta
%corresponds to the classifier for label i
%   [all_theta] = ONEVSALL(X, y, num_labels, lambda) trains num_labels
%   logistic regression classifiers and returns each of these classifiers
%   in a matrix all_theta, where the i-th row of all_theta corresponds
%   to the classifier for label i

% Some useful variables
m = size(X, 1);
n = size(X, 2);

% You need to return the following variables correctly
all_theta = zeros(num_labels, n + 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the following code to train num_labels
%               logistic regression classifiers with regularization
%               parameter lambda.
%
% Hint: theta(:) will return a column vector.
%
% Hint: You can use y == c to obtain a vector of 1's and 0's that tell you
%       whether the ground truth is true/false for this class.
%
% Note: For this assignment, we recommend using fmincg to optimize the cost
%       function. It is okay to use a for-loop (for c = 1:num_labels) to
%       loop over the different classes.
%
%       fmincg works similarly to fminunc, but is more efficient when we
%       are dealing with large number of parameters.
%
% Example Code for fmincg:
%
%     % Set Initial theta
%     initial_theta = zeros(n + 1, 1);
%
%     % Set options for fminunc
%     options = optimset('GradObj', 'on', 'MaxIter', 50);
%
%     % Run fmincg to obtain the optimal theta
%     % This function will return theta and the cost
%     [theta] = ...
%         fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), ...
%                 initial_theta, options);
%

for c = 1:num_labels
    initial_theta = zeros(n + 1, 1);
    options = optimset('GradObj', 'on', 'MaxIter', 50);
    [theta] = ...
         fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), ...
                 initial_theta, options);
    all_theta(c, :) = theta(:)';
end

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

end


function b=bitstream(l,p)

% b=bitstream(l,p)
%
% Return a stream of l bits where the probability of a '1' is p.
% p defaults to 0.5.

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

error(nargchk(1,2,nargin));
error(chk_param(l,'l','scalar','integer','real','>=',0));
if (nargin<2)
  p=0.5;
end
error(chk_param(p,'p','scalar','real','>=',0,'<=',1));

b=double(rand(1,l)<p);


clc;
clearvars;
close all;
rng default;

% subspace dimension
D = 4;
% ambient dimension
M = 10;
% Number of subspaces
K = 2;
import spx.data.synthetic.subspaces.random_subspaces;
bases = random_subspaces(M, K, D);
A = bases{1};
B = bases{2};
G = A' * B
sigmas = svd(G)'
largest_product = sigmas(1)
smallest_angle_rad  = acos(largest_product)
smallest_angle_deg = rad2deg(smallest_angle_rad)

K = 6;
bases = random_subspaces(M, K, D);
angles = spx.la.spaces.smallest_angles_deg(bases)
angles = spx.matrix.off_diag_upper_tri_elements(angles)'

angles = spx.la.spaces.smallest_angles_rad(bases)
angles = spx.la.spaces.smallest_angles_cos(bases)


%SCALEM Compute scaling map
%
%   W = SCALEM(A,TYPE)
%   W = A*SCALEM([],TYPE)
%   W = A*SCALEM(TYPE)
%
% INPUT
%   A     Dataset
%   TYPE  Type of scaling (optional; default: the class priors weighted
%         mean of A is shifted to the origin)
%
% OUTPUT
%   W  Scaling mapping
%
% DESCRIPTION
% Computes a scaling map W, whose type depends on the parameter T:
%
% [], 'c-mean'  - The mean of A is shifted to the origin. Class priors are taken
%                 into account.
% 'mean'        - The mean of A is shifted to the origin. This is computed for
%                 the data as given in A, neglecting class priors.
% 'c-variance'  - The mean of A  is shifted to the origin and the average class
%                 variances (within-scatter) are normalized.  Class priors are
%                 taken into account.
% 'variance'    - The mean of A is shifted to the origin and the total variances
%                 of all features are scaled to 1. This is computed for the
%                 data as given in A, neglecting class priors.
% '2-sigma'     - For each feature f, the 2-sigma interval, [f-2*std(f),f+2*std(f)]
%                 is rescaled to [0,1]. Values outside this domain are clipped.
%                 Class priors are taken into account.
% 'domain'      - The domain for all features in the dataset A is set to [0,1].
%                 This is computed for the data as given in A, neglecting class priors.
%
% The map W may be applied to a new dataset B by B*W.
%
% SEE ALSO (PRTools Guide)
% PRMAPPING, DATASET

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: scalem.m,v 1.9 2009/11/27 08:52:02 duin Exp $

function W = scalem(varargin)

  argin = shiftargin(varargin,'char');
  argin = setdefaults(argin,[],'');
  if mapping_task(argin,'definition')
  	W = define_mapping(argin,'untrained');
    return
  end

  [a,t] = deal(argin{:});
  %DXD Make the naming even better:
  dname = 'Scaling mapping';
  switch t
  case 'mean'
	  dname = 'zero-mean';
  case {'','c-mean'}
	  dname = 'zero-mean (+class priors)';
  case 'variance'
	  dname = 'unit-var';
  case 'c-variance'
	  dname = 'unit-var (+class priors)';
  case '2-sigma'
	  dname = '0-1 scaling (+-2 standard dev.)';
  case 'domain'
	  dname = '0-1 scaling';
  end
  % No data, return an untrained mapping.
	if (nargin < 1) | isempty(a)
		W = prmapping('scalem',{t});
		W = setname(W,dname);
		return;
	end

	[a,c,lablist] = cdats(a,1);
	[m,k] = size(a);

	switch t
	case {'','c-mean'}
    u = meancov(a);
		u =getprior(a)*(+u);
		s = ones(1,k);
		clip = 0;

	case 'mean'
		a = remclass(a);
		u = mean(a,1);
		s = ones(1,k);
		clip = 0;

	case 'c-variance'
		p = getprior(a);
		U = meancov(a);
		u = p*(+U);
		s = zeros(1,k);
		for j=1:c
			s = s + p(j)*var(seldat(a,j)*cmapm(+U(j,:),'shift'));
		end
		s = sqrt(s);
		clip = 0;

	case 'variance'
		%[u,G] = meancov(+a);
		%s = sqrt(diag(G))';
  	u = mean(a,1);
		s = std(a,0,1);
		clip = 0;

	case '2-sigma'
		p = getprior(a);
		U = meancov(a);
		u = p*(+U);
		s = zeros(1,k);
		for j=1:c
			s = s + p(j)*var(seldat(a,j));
		end
		s = sqrt(s);
		s = 4*s;
		u = u-0.5*s;
		clip = 1;

	case 'domain'
		mx = max(a,[],1)+eps;
		mn = min(a,[],1)-eps;
		u = mn;
		s = (mx - mn)*(1+eps);
		clip = 0;

	otherwise
		error('Unknown option')
	end

	J = find(s==0);
	s(J) = ones(size(J)); 				% No scaling for zero variances.

	W = cmapm({u,s,clip},'scale');
	W.size_out = a.featsize;
	W = setlabels(W,getfeatlab(a));

return;


function [F,L,Qc,H,Pinf,dF,dQc,dPinf,params] = cf_matern32_to_ss(magnSigma2, lengthScale)
% CF_MATERN32_TO_SS - Matern covariance functions, nu = 3/2, to state space
%
% Syntax:
%   [F,L,Qc,H,Pinf,dF,dQc,dPinf,params] = cf_matern32_to_ss(magnSigma2, lengthScale)
%
% In:
%   magnSigma2  - Matern magnitude scale parameter (default: 1)
%   lengthScale - Matern distance scale parameter (default: 1)
%
% Out:
%   F           - Feedback matrix
%   L           - Noise effect matrix
%   Qc          - Spectral density of white noise process w(t)
%   H           - Observation model matrix
%   Pinf        - Covariance of the stationary process
%   dF          - Derivatives of F w.r.t. parameters
%   dQc         - Derivatives of Qc w.r.t. parameters
%   dPinf       - Derivatives of Pinf w.r.t. parameters
%   params      - Input and output parameter information
%
% Description:
%   This function converts one-dimensional covariance functions of
%   the Matern class to state space models. The covariance function
%   parametrization is as follows
%
%     k(tau) = magnSigma2 (1+sqrt(3) |tau|/lengthScale) exp(-sqrt(3) |tau|/lengthScale),
%
%   where magnSigma2 is the magnitude scale parameter, lengthScale the
%   distance scale parameter, and tau the time difference between states,
%   tau = t-t'.
%     This function takes the covariance function parameters as inputs and
%   outputs the corresponding state space model matrices. The state space
%   model is given as follows in terms of a stochastic differential
%   equation
%
%      df(t)/dt = F f(t) + L w(t),
%
%   where w(t) is a white noise process with spectral denisty Qc. The
%   observation model for discrete observation y_k of f(t_k) at step k,
%   is as follows
%
%      y_k = H f(t_k) + r_k, r_k ~ N(0, R),
%
%   where r_k is the Gaussian measurement noise with covariance R.
%     Pinf is the stationary covariance, where the value of Pinf(i,j),
%   is defined as follows
%
%      Pinf(i,j) = E[(f_i(t)-E[f_i(t)])(f_j(t)-E[f_j(t)])],
%
%   where f_i(t) is component i of state vector f(t).
%     Derivatives: All have same form. For example, dF has the following
%   form:
%
%       dF(:,:,1) = dF/d(magnSigma2 = input parameter_1),
%       dF(:,:,i) = dF/d(input parameter_i).
%
% References:
%
%   [1] Simo Sarkka, Arno Solin, Jouni Hartikainen (2013).
%       Spatiotemporal learning via infinite-dimensional Bayesian
%       filtering and smoothing. IEEE Signal Processing Magazine,
%       30(4):51-61.
%
%   [2] Jouni Hartikainen and Simo Sarkka (2010). Kalman filtering and
%       smoothing solutions to temporal Gaussian process regression
%       models. Proceedings of IEEE International Workshop on Machine
%       Learning for Signal Processing (MLSP).
%
% See also:
%   COV_MATERN32, SPEC_MATERN32
%
% Copyright:
%   2012-2014   Arno Solin
%   2013-2014   Jukka Koskenranta
%
%  This software is distributed under the GNU General Public
%  License (version 3 or later); please refer to the file
%  License.txt, included with the software, for details.

%% Apply defaults

  % Check if magnSigma2 is given
  if nargin < 1 || isempty(magnSigma2), magnSigma2 = 1; end

  % Check if lengthScale is given
  if nargin < 2 || isempty(lengthScale), lengthScale = 1; end

%% Form state space model

  % Derived constants
  lambda = sqrt(3)/lengthScale;

  % Feedback matrix
  F = [0,          1;
       -lambda^2,  -2*lambda];

  % Noise effect matrix
  L = [0;   1];

  % Spectral density
  Qc = 12*sqrt(3)/lengthScale^3*magnSigma2;

  % Observation model
  H = [1,   0];

%% Stationary covariance

  % Calculate Pinf only if requested
  if nargout > 4,

    Pinf = [magnSigma2, 0;
            0,          3*magnSigma2/lengthScale^2];

  end

%% Calculate derivatives

  % Calculate derivatives only if requested
  if nargout > 5

    % Derivative of F w.r.t. parameter magnSigma2
    dFmagnSigma2    =  [0,  0;
                        0,  0];

    % Derivative of F w.r.t parameter lengthScale
    dFlengthScale   =  [0,                  0;
                        6/lengthScale^3,    2*sqrt(3)/lengthScale^2];
    % Derivative of Qc w.r.t. parameter magnSigma2
    dQcmagnSigma2   =   12*sqrt(3)/lengthScale^3;

    % Derivative of Qc w.r.t. parameter lengthScale
    dQclengthScale  =   -3*12*sqrt(3)/lengthScale^4*magnSigma2;

    % Derivative of Pinf w.r.t. parameter magnSigma2
    dPinfmagnSigma2 = [1,0;
                       0,3/lengthScale^2];

    % Derivative of Pinf w.r.t. parameter lengthScale
    dPinflengthScale = [0,0;
                        0,-6*magnSigma2/lengthScale^3];

    % Stack all derivatives
    dF = zeros(2,2,2);
    dQc = zeros(1,1,2);
    dPinf = zeros(2,2,2);

    dF(:,:,1) = dFmagnSigma2;
    dF(:,:,2) = dFlengthScale;
    dQc(:,:,1) = dQcmagnSigma2;
    dQc(:,:,2) = dQclengthScale;
    dPinf(:,:,1) = dPinfmagnSigma2;
    dPinf(:,:,2) = dPinflengthScale;

  end

%% Return parameter names

  % Only return if requested
  if nargout > 8

    % Stationarity
    p.stationary = true;

    % Input parameter information
    p.in{1}.name = 'magnSigma2';  p.in{1}.default = 1; p.in{1}.opt = true;
    p.in{2}.name = 'lengthScale'; p.in{2}.default = 1; p.in{2}.opt = true;

    % Return parameter setup
    params = p;

  end


function tests = test_spx_checks
  tests = functiontests(localfunctions);
end

function setupOnce(testCase)
    import spx.matrix;
end

function test_is_square(testCase)
    import spx.matrix;
    a = randn(3, 4);
    verifyFalse(testCase, matrix.is_square(a));
    verifyTrue(testCase, matrix.is_square(zeros(3, 3)));
end

function test_is_symmetric(testCase)
    import spx.matrix;
    a  = randn(3, 4);
    verifyFalse(testCase, matrix.is_symmetric(a));
    a = [1 2; 3 4];
    verifyFalse(testCase, matrix.is_symmetric(a));
    a = [1 2; 2 4];
    verifyTrue(testCase, matrix.is_symmetric(a));
    a = [1 2i; 2i 4];
    verifyTrue(testCase, matrix.is_symmetric(a));
    a = [1 2i; -2i 4];
    verifyFalse(testCase, matrix.is_symmetric(a));
end

function test_is_hermitian(testCase)
    import spx.matrix;
    a  = randn(3, 4);
    verifyFalse(testCase, matrix.is_hermitian(a));
    a = [1 2; 3 4];
    verifyFalse(testCase, matrix.is_hermitian(a));
    a = [1 2; 2 4];
    verifyTrue(testCase, matrix.is_hermitian(a));
    a = [1 2i; 2i 4];
    verifyFalse(testCase, matrix.is_hermitian(a));
    a = [1 2i; -2i 4];
    verifyTrue(testCase, matrix.is_hermitian(a));
end

function test_is_pd(testCase)
    import spx.matrix;
    a = randn(3, 3);
    verifyFalse(testCase, matrix.is_positive_definite(a));
    a  = [
    2 -1 0;
    -1 2 -1;
    0 -1 2];
    verifyTrue(testCase, matrix.is_positive_definite(a));
end


function R = rotz(alpha)

    % ROTZ computes a rotation along the z axis (rotation matrix).
    %
    % FORMAT: R = rotz(alpha)
    %
    % INPUT:  - alpha = angle in radians
    %
    % OUTPUT: - R = [3 * 3] rotation matrix along z axis
    %
    % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava
    %
    %          all authors are with the Italian Istitute of Technology (IIT)
    %          email: name.surname@iit.it
    %
    % Genoa, Dec 2017
    %

    %% --- Initialization ---

   R = [cos(alpha), -sin(alpha), 0;
        sin(alpha),  cos(alpha), 0;
        0,           0,          1];

end


function accuracy = ldrc(TrainSet, TestSet, train_num, test_num, class_num, reduce_dimension, options)
% Linear discriminant regression classificatoin (LDRC) algorithm
%
% Inputs:
%       TrainSet            train sets of size dxn, where d is dimension and n is number of sets
%       TestSet             test sets of size dxn, where d is dimension and n is number of sets
%       test_num            numner of test sets
%       class_num           numner of classes
%       options             options
% Output:
%       accuracy            classification accurary
%
% References:
%       S.-M. Huang and J.-F. Yang,
%       "Linear discriminant regression classification for face recognition,"
%       IEEE Signal Processing Letters, vol.20, no.1, pp.91-94, 2013.
%
%
% Created by H.Kasai on July 05, 2017

    % extract options
    if ~isfield(options, 'verbose')
        verbose = false;
    else
        verbose = options.verbose;
    end

    if verbose
        fprintf('# LDRC: calculate projection matrix U ...');
    end
    Eb = 0;
    Ew = 0;
    for j = 1 : train_num

        j_class = TrainSet.y(1, j);

        % calcuate Ew
        intra_class_index = find(TrainSet.y == j_class);
        intra_class_index(intra_class_index == j) = [];
        X_intra_wo_j = TrainSet.X(:, intra_class_index);        % X^{intra}
        for k = 1 : size(X_intra_wo_j, 2)
            error_intra = TrainSet.X(:,j) - X_intra_wo_j(:,k);
            Ew = Ew + error_intra * error_intra';               % Eq.(14)
        end

        % calcuate Eb
        inter_class_index = find(TrainSet.y ~= j_class);
        X_inter = TrainSet.X(:, inter_class_index);             % X^{inter}

        for k = 1 : size(X_inter, 2)
            error_inter = TrainSet.X(:,j) - X_inter(:,k);
        end
        Eb = Eb + error_inter * error_inter';                   % Eq.(13)
    end

    % calc
    Ew = Ew/train_num;                                          % Eq.(14)
    Eb = Eb/(train_num * (class_num-1));                        % Eq.(13)
    epsilon = 0.001;
    Ew = Ew + epsilon*eye(size(Ew,2));

    [V, ~] = eig(Eb, Ew);                                       % Eq.(18)
    for i = 1:reduce_dimension
        U(:,i) = V(:,size(V,2)+1-i);
    end
    if verbose
        fprintf('done\n');
    end

    % reduce dimention
    TrainSet.X_red  = U' * TrainSet.X;
    TestSet.X_red   = U' * TestSet.X;

    % prepare projection matrix (hat matrix)
    H = cell(1, class_num);
    for i = 1 : class_num
        class_index = find(TrainSet.y == i);

        X_i = TrainSet.X_red(:, class_index);
        alpha_i = pinv(X_i' * X_i) *  X_i';
        H{i} = X_i * alpha_i;

        if verbose
            fprintf('# LDRC: calc Hi for class : %03d/%03d (samples: %03d)\n', i, class_num, length(class_index));
        end
    end

    % prepare predicted label array
    identity = zeros(1, test_num);

    % predict the class
    for j = 1 : test_num

        dis = zeros(1, class_num);
        y = TestSet.X_red(:, j);
        for i = 1 : class_num
            y_pred = H{i} * y;
            dis(1, i) = norm(y - y_pred);
        end
        [~, label] = min(dis);
        identity(j) = label;

        if verbose
            correct = (label == TestSet.y(1, j));
            fprintf('# LDRC: test:%03d, predict class: %03d --> ground truth :%03d (%d)\n', j, label, TestSet.y(1, j), correct);
        end
    end

    % calculate accuracy
    correct_num = sum(identity == TestSet.y);
    accuracy = correct_num / test_num;
end


function [ vX, mX ] = SolveLsL1ComplexIrls( mA, vB, lambdaFctr, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX, mX ] = SolveLsL1ComplexIrls( mA, vB, lambdaFctr, numIterations )
% Solves the 0.5 * || A x - b ||_2 + \lambda || x ||_1 problem using Fixed
% Point Iteration Method. The model allows A, b and x to be Complex.
% Input:
%   - mA                -   Model Matrix.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double' (Complex).
%                           Range: (-inf, inf).
%   - vB                -   Input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double' (Complex).
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L1 Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia Fixed Point Iteration Method - https://en.wikipedia.org/wiki/Fixed-point_iteration.
% Remarks:
%   1.  A
% Known Issues:
%   1.  A
% TODO:
%   1.  B
% Release Notes:
%   -   1.0.000     07/11/2016
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

paramEps = 1e-9;

% vX  = mAA \ vAb;
vX  = pinv(mA) * vB; %<! Dealing with "Fat Matrix"

mAA = mA' * mA;
vAb = mA' * vB;
% mI = eye(size(mA, 1));

mX = zeros([size(vX, 1), numIterations]);
mX(:, 1) = vX;

for ii = 2:numIterations
    vX = (mAA + (lambdaFctr * diag(1./ (abs(vX) + paramEps)))) \ vAb;

    % Doesn't work (Bad conditioning)
    % mX = diag(abs(vX));
    % vX = (mX * mA') * (((mA * mX * mA.') + (lambdaFctr * mI)) \ vB);

    mX(:, ii) = vX;
end

end


function [val grad]=test_func(x)

val=(x-2).^2;
grad=2*(x-2);


function thiem_test
% determination of transmissivity  T                    Holzbecher October 2005
% using Thiem's solution
% and fzero for automatic estimation
global rfit sfit reach Q

% specify fitting data
rfit = [0.8 30 90 215];                     % observation well distance
sfit = [2.236 1.088 0.716 0.25];            % observated steady state drawdowns
Q = 788;                                    % pumping rate
rwell = 1;                                  % well radius
reach = 500;                                % reach
T = 500;                                    % guess for transmissivity
test = 0;                                   % test run flag

% -------------------------------------------------------------------------
if test == 1               % input pumptest 'Oude Korendijk'
    rfit = [0.8 30 90 215]; sfit = [2.236 1.088 0.716 0.25];
    Q = 788; rwell = 0.5; T = 400; reach = 500;
end
options = optimset('Display','iter','TolFun',1e-8);
T = fzero(@myfun,T,options);
res = sum(Q*log(rfit/reach)/T/2/pi+sfit);
display (['Residual = ' num2str(res)]);
r = linspace (rwell,reach,100);
h = Q*log(r/reach)/T/2/pi;
plot (rfit,-sfit,'o',r,h,'-');
legend ('measured','ideally',4);
xlabel ('r'); ylabel ('h');
title ('Thiem Solution');
text (10*rwell,-0.1*max(sfit),['T = ' num2str(T)]);

function f = myfun(T);
global rfit sfit reach Q

% calculate Thiem solution
h = Q*log(rfit/reach)/T/2/pi;

% specify function f to vanish
f = (h+sfit)*h'/T;


function [int, keepindex, repindex] = mesh_laplacian_interp(lap, index);

% MESH_LAPLACIAN_INTERP: Computes the zero Laplacian interpolation matrix
%
% Useage:   [int, keepindex, repindex] = mesh_laplacian_interp(lap, index)
%
% This function calculates an interpolation matrix that provides
% the coefficients for the calculation of potential values at all
% unknown vertices of a mesh, given known potential values at
% a subset of the mesh vertices (at 'index').  The interpolation
% solution is constrained by a minimal norm of the Laplacian
% of the mesh.  See the reference below for details.
%
% 'lap' is the laplacian matrix for the full mesh (see mesh_laplacian)
% 'int' is the matrix which interpolates from the points in 'index'
% to the full mesh.  'index' is a row vector of indices into a
% subset of the vertices used to calculate 'lap'.  This subset
% is where the electric potential is known and usually corresponds
% to the given electrode vertices, eg:
%
% index = dsearchn(scalpvert,elecvert)';
%
% If 'index' contains repeated indices, only the unique indices
% are useful.  The 'keepindex' array can be used to select these.
% The 'repindex' array is the repeated indices.
%
% Interpolations can be done using matrix 'int', eg:
%
% [int, keepindex, repindex] = mesh_laplacian_interp(lap,index);
% if isempty(repindex),
%   Vint = int * Vknown;
% else
%   Vint = int * Vknown(keepindex);
% end
%
% This implements interpolation method B (p. 336) of
% Oostendorp T, Oosterom A & Huiskamp G (1989),
% Interpolation on a triangulated 3D surface.
% Journal of Computational Physics, 80: 331-343.
%

% Licence:  GNU GPL, no implied or express warranties
% History:  (c) 04/2002 Robert Oostenveld
%           - agreed to release 'lapint' under GNU GPL
%           04/2002, Darren.Weber@flinders.edu.au
%           - introduced check for index replications and
%             adjusted calculations/output accordingly
%           - converted lap to sparse matrix and solution
%             of interpolation matrix with \ operator
%           - accepts sparse lap input and returns sparse int
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if size(lap,1)~=size(lap,2), error('MESH_LAPLACIAN_INTERP: lap matrix is not square'); end

if issparse(lap), lap = full(lap); end

tic

if size(index,1)~=1, index = index'; end

% Remove any replicate indices from 'index'
[KnownIndex, i, i] = union(index,index);
if ~isequal(length(KnownIndex),length(index)),
    fprintf('\a\n\nMESH_LAPLACIAN_INTERP: Warning:\n\nTrimming duplicate values from index, use keepindex!\n\n');
end
keepindex = sort(i);
repindex = setdiff(1:length(index),sort(i));

KnownIndex = index(keepindex); % unsort KnownIndex
clear index

k = length(KnownIndex);
n = length(lap);

fprintf('MESH_LAPLACIAN_INTERP: Calc Interpolation matrix for %5d to %5d vertices...',k,n);

% find 'unknown' indices of lap matrix
UnknownIndex = setdiff(1:n, KnownIndex);

% reshuffle rows & columns of lap matrix
lapi = [KnownIndex, UnknownIndex];
lap = lap(lapi, :); % rows
lap = lap(:, lapi); % columns

% Segregate known/unknown portions of lap
k = length(KnownIndex);
n = length(lap);

L11 = lap(1:k    ,1:k    );
L12 = lap(1:k    ,(k+1):n);
L21 = lap((k+1):n,1:k    );
L22 = lap((k+1):n,(k+1):n);

clear lap lapi; % tidy up some memory

% Convert to sparse for quicker computation
A = sparse([L12; L22]);
B = sparse([L11; L21]);

clear L11 L12 L21 L22;  % tidy up some memory

%int = -pinv(A) * B;  % cannot use pinv with sparse matrix
int = -A \ B;

% Convert result back to full matrix
int = full(int);

% append the interpolating piece to the identity matrix
% these take care of the known potentials
int = [eye(k); int];

% reshuffle the columns of the interpolating matrix
[tmp, order] = sort(KnownIndex);
int = int(:,order);

% reshuffle the rows of the interpolating matrix
[tmp, order] = sort([KnownIndex, UnknownIndex]);
int = int(order, :);

int = sparse(int);

t = toc;
fprintf('done (%6.2f sec).\n',t);

return


function [y,deriv] = log_distance_mv2df(w,input_data,new_dim)
% This is an MV2DF. See MV2DF_API_DEFINITION.readme.
%
% The function projects each column of input_data to a subspace and then
% computes log distance from a centroid. The input_data is fixed, but
% the projection and centroid parameters are variable.
%
%   W = reshape(w);
%   y.' = log sum((W(:,1:end-1).'*input_data - W(:,end)).^2,1)
%
%      W is the augmented matrix [M c] where M maps an input  vector
%      to a lower dimensional space and c is the centroid in
%      the lower dimensional space.
%
% Parameters:
%     w: the vectorized version of the W matrix
%     input_data: is an K-by-T matrix of input vectors of length K, for
%     each of T trials.
%     new_dim: the dimension of vectors in the lower dimensional space.
%

if nargin==0
    test_this();
    return;
end

if isempty(w)
    [dim, num_trials] = size(input_data);
    map = @(w) map_this(w,input_data,dim,new_dim);
    transmap = @(w) transmap_this(w,input_data,num_trials,new_dim);
    delta = linTrans(w,map,transmap);
    y = logsumsquares_fh(new_dim,1,delta);
    return;
end

if isa(w,'function_handle')
    f = log_distance_mv2df([],input_data,new_dim);
    y = compose_mv(f,w,[]);
    return;
end

f = log_distance_mv2df([],input_data,new_dim);
if nargout==1
    y = f(w);
else
    [y,deriv] = f(w);
end

function y = map_this(w,input_data,dim,new_dim)
% V = [input_data; ones(1,num_trials)];
W = reshape(w,new_dim,dim+1);
y = bsxfun(@minus,W(:,1:end-1)*input_data,W(:,end));
y = y(:);

function dx = transmap_this(dy,input_data,num_trials,new_dim)
dY = reshape(dy,new_dim,num_trials);
% V = [input_data; ones(1,num_trials)];
% Vt = V.';
% dX = dY*Vt;
dYt = dY.';
dYtSum = sum(dYt,1);
dX = [input_data*dYt;-dYtSum].';
dx = dX(:);

function test_this()
K = 5;
N = 10;
P = 3;
M = randn(P,N);
c = randn(P,1);
W = [M c];
w = W(:);
input_data = randn(N,K);

f = log_distance_mv2df([],input_data,P);
test_MV2DF(f,w);


function[phi] = orth_proj(P)
%%% Funciton to compute the projection of a vector orthogonal to the
%%% subspace spanned by a matrix

%%%         Inputs
%%%     P - basis matrix
%%%     x - input vector

%%%         Outputs
%%%     phi- orthogonal projection matrix
%%%     y - output vector

[nr, ~] = size(P);

phi = eye(nr) - P * P';
%y = x - P * (P' * x);

end


function [Sx,tau,fs] = stft_fw(t, x, opt)
% STFT  Compute the short-time Fourier transform
%
% Very closely based on Steven Schimmel's stft.m and istft.m from
% his SPHSC 503: Speech Signal Processing course at Univ. Washington.

if nargin<3, opt = struct(); end

if ~isfield(opt, 'window'), opt.window = round(length(x)/16); end
if length(opt.window) == 1, opt.window = hamming(opt.window); end
%if ~isfield(opt, 'overlap'), opt.overlap = length(opt.window)-1; end
opt.overlap = length(opt.window)-1;

window = opt.window / norm(opt.window, 2); % Unit norm

% Pre-pad signal
n = length(x);

% extract short-time segments from the input
Nwin = length(window);
xbuf = buffer(x, Nwin, opt.overlap, 'nodelay');

% apply the specified window
xbuf = diag(sparse(window)) * xbuf;

% take the FFT, and keep only 0 to pi
Sx = fft(xbuf,[],1);
Sx = Sx(1:floor(Nwin/2)+1, :);

% define time and frequency sampling vectors
tau = [0:size(Sx,2)-1]*(Nwin-opt.overlap) + (Nwin-1)/2;
fs = linspace(0,1,Nwin+1); fs = fs(1:floor(Nwin/2)+1);

% correct for sampling frequency
tdiff = median(diff(t));
tau = tau * tdiff;
fs = fs / tdiff;


function out = idwMvInterp(imgw, map, maxhw, p )
% Description:
% Fill holes using Inverse Distance Weighting interpolation
%
% Inputs:
% imgw - input image
% map - Map of the canvas with 0 indicating holes and 1 indicating pixel
% maxhw - Radius of inverse weighted interpolation
% p - power for inverse weighted interpolation
%
% Output:
% out - interpolated image
%
% Author: Fitzgerald J Archibald
% Date: 23-Apr-09

outH  = size(imgw,1);
outW  = size(imgw,2);
out = imgw;

[yi_arr, xi_arr] = find(map==0); % Find locations needing fill
if isempty(yi_arr) == false
    color = size(imgw,3);
    for ix = 1:length(yi_arr),

        xi = xi_arr(ix);
        yi = yi_arr(ix);

        % Find min window which has non-hole neighbors
        yixL=max(yi-maxhw,1);
        yixU=min(yi+maxhw,outH);
        xixL=max(xi-maxhw,1);
        xixU=min(xi+maxhw,outW);

        % use inverse distance weighting filter for filling
        mapw = map(yixL:yixU,xixL:xixU);
        if isempty(find(mapw, 1)) == false,
            wk = compWk(mapw, xi-xixL+1, yi-yixL+1, p); % compute weights for interpolation
            for colIx = 1:color,
                out(yi,xi,colIx) = idw(imgw(yixL:yixU, xixL:xixU, colIx), mapw, wk); % interpolation
            end
        end
    end
end

return;

% compute wk
function wk = compWk(map, cx, cy, p)

[h,w] = size(map);
[x,y] = meshgrid(1:h,1:w);
y=(y-cx).^2;
x=(x-cy).^2;
d2=x+y; % square of distance
wk = 1./(d2'.^(p/2));

return;

% Inverse distance weighting
function out = idw(in, map, wk)

num=sum(double(in(find(map))).*wk(find(map))); % weight the available pixel values among the neighbors
den=sum(wk(find(map))); % sum of contributing weights within radius r

out = num / den;

return


function v = ml_get_isolines(xtest,xtrain,alpha,kernel,kpar)
%ML_GET_ISOLINES: Plot the isolines of a kernel method.
%
%   input ---------------------------------------------------------------
%
%       o xtest       : (N x P), dataset of N samples of dimension P
%
%       o xtrain      : (M x P), original training data points
%
%       o alpha       : (M x 1), eigenvector of dimension M
%
%       o kernel      : string, the type of kernel
%                               [gauss,diag-gauss,poly,linear].
%
%       o kpar        : sig = kpar(1);  % variance for the gauss kernels.
%                       p   = kpar(1);	% polynome order
%                       c   = kpar(2);	% additive constant
%
%   output ----------------------------------------------------------------
%
%       o v           : value when projected onto the eigenvetors alpha
%
%   comment ---------------------------------------------------------------
%
%       Each data point of xtest is projected onto every alpha eigenvector.
%       Usually xtest will be (N x 2) since we are using this function for
%       plotting. The dim input is for the dimensions of the xtrain we want
%       to use in conjunction with xtest points. Basically both xtest
%       and xtrain should be of the same dimension.
%

N = size(xtest,1);
v = zeros(N,1);

if(strcmp(kernel,'poly') == true)
   kpar_ = [kpar(2) kpar(1)];
   kpar  = kpar_;
end

for i=1:N
    % Compute gramm matrix between test point i and all train points
    K_i  = km_kernel(xtest(i,:),xtrain,kernel,kpar);
    v(i) = alpha(:,1)' * K_i(:);

end

end


% Riemannian Dictionary Learning and Sparse Coding
%  if you plan to use this code, please cite
% Cherian, Anoop, and Suvrit Sra. "Riemannian Dictionary Learning and Sparse Coding for Positive Definite Matrices." arXiv preprint arXiv:1507.02772 (2015).
% This code is released on the BSD3 license.
% This code should not be used for non-commercial purposes.
% The authors are not liable to any loss or damage caused by running this code.
% For any issues/bugs, please contact anoop.cherian@gmail.com.

% Copyright (c) 2016, Anoop Cherian
% All rights reserved.
% Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
% 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
% 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
% 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
% PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
% INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
% OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
% OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

addpath ./spalgos/;
addpath(genpath('./manopt/'));
addpath ./tools;
key = 1234;
randn('state', key); rand('twister', key);

n=100; num_atoms=20; active_size=5; d=5;
[data, B, A] = generate_data(n, d, num_atoms, active_size); data=data';
data = data(randperm(n));

% sparsity for varying lambda:
lambda = 0.1;
[BB, alpha, obj] = Fast_Riem_DL(data, num_atoms, active_size, 'sim', lambda, 1234);


function X=JumpDiffusionMerton(m,s,l,a,D,ts,J)

T=ts(end);
% simulate number of jumps;
N=poissrnd(l*T,J,1);

Jumps=[];
L=length(ts);
for j=1:J
    % simulate jump arrival time
    t=T*rand(N(j),1);
    t=sort(t);

    % simulate jump size
    S=a+D*randn(N(j),1);

    % put things together
    CumS=cumsum(S);
    for n=1:L
        Events=sum(t<=ts(n));
        Jumps_ts(n)=0;
        if Events
            Jumps_ts(n)=CumS(Events);
        end
    end

    Jumps=[Jumps
        Jumps_ts];
end
for l=1:L
    Dt=ts(l);
    if l>1
        Dt=ts(l)-ts(l-1);
    end
    D_Diff(:,l)=m*Dt + s*sqrt(Dt)*randn(J,1);
end

X=[zeros(J,1) cumsum(D_Diff,2)+Jumps];


% Purpose: Matlab script to compute the advection terms for incompressible Navier-Stokes

% evaluate flux vectors
fxUx = Ux.^2;  fyUx = Ux.*Uy;  fxUy = Ux.*Uy; fyUy = Uy.^2;

% save old nonlinear terms
NUxold = NUx; NUyold = NUy;

% evaluate inner-product of test function gradient and flux functions
NUx = Div2D(fxUx, fyUx); NUy = Div2D(fxUy, fyUy);

% interpolate velocity to face nodes on element faces
UxM = zeros(Nfp*Nfaces, K); UxP = zeros(Nfp*Nfaces, K);
UyM = zeros(Nfp*Nfaces, K); UyP = zeros(Nfp*Nfaces, K);
UxM(:) = Ux(vmapM); UyM(:) = Uy(vmapM); UxP(:) = Ux(vmapP); UyP(:) = Uy(vmapP);

% set '+' trace of velocity at boundary face nodes
UxP(mapI) = bcUx(mapI); UxP(mapW) = bcUx(mapW); UxP(mapC) = bcUx(mapC);
UyP(mapI) = bcUy(mapI); UyP(mapW) = bcUy(mapW); UyP(mapC) = bcUy(mapC);

% evaluate flux vectors at '-' and '+' traces at face nodes
fxUxM = UxM.^2; fyUxM = UxM.*UyM; fxUyM = UxM.*UyM; fyUyM = UyM.^2;
fxUxP = UxP.^2; fyUxP = UxP.*UyP; fxUyP = UxP.*UyP; fyUyP = UyP.^2;

% evaluate dot product of normal and velocity at face nodes
UDotNM = UxM.*nx + UyM.*ny; UDotNP = UxP.*nx + UyP.*ny;
maxvel = max(abs(UDotNM), abs(UDotNP)) ;

% evaluate maximum normal velocity at face face nodes
maxvel = reshape(maxvel, Nfp, Nfaces*K);
maxvel = ones(Nfp, 1)*max(maxvel, [], 1);
maxvel = reshape(maxvel, Nfp*Nfaces, K);

% form local Lax-Friedrichs/Rusonov fluxes
fluxUx = 0.5*( -nx.*(fxUxM-fxUxP) - ny.*(fyUxM-fyUxP)  - maxvel.*(UxP - UxM));
fluxUy = 0.5*( -nx.*(fxUyM-fxUyP) - ny.*(fyUyM-fyUyP)  - maxvel.*(UyP - UyM));

% put volume and surface terms together
NUx = NUx + LIFT*(Fscale.*fluxUx); NUy = NUy + LIFT*(Fscale.*fluxUy);

% compute (U~,V~)
UxT = ((a0*Ux+a1*Uxold) - dt*(b0*NUx + b1*NUxold))/g0;
UyT = ((a0*Uy+a1*Uyold) - dt*(b0*NUy + b1*NUyold))/g0;


function [p,y,a] = qint(ym1,y0,yp1)
%QINT   Quadratic interpolation of 3 uniformly spaced samples
%
%               [p,y,a] = qint(ym1,y0,yp1)
%
%       returns extremum-location p, height y, and curvature a
%       of a parabolic fit through three points.
%       The parabola is given by y(x) = a*(x-p)^2+b,
%       where y(-1)=ym1, y(0)=y0, y(1)=yp1.

   p = (yp1 - ym1)/(2*(2*y0 - yp1 - ym1));
   if nargout>1
     y = y0 - 0.25*(ym1-yp1)*p;
   end;
   if nargout>2
     a = 0.5*(ym1 - 2*y0 + yp1);
   end;


function polynomial_compress_test ( )

%*****************************************************************************80
%
%% POLYNOMIAL_COMPRESS_TEST tests POLYNOMIAL_COMPRESS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2014
%
%  Author:
%
%    John Burkardt
%
  m = 3;
  o = 10;

  c = [ 7.0, - 5.0, 5.0, 9.0, 11.0, 3.0, 6.0, 0.0, - 13.0, 1.0E-20 ];
  e = [ 1, 2, 2, 4, 5, 5, 5, 12, 33, 35 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLYNOMIAL_PRINT_TEST\n' );
  fprintf ( 1, '  POLYNOMIAL_PRINT prints a polynomial.\n' );

  fprintf ( 1, '\n' );
  title = '  Uncompressed P(X) =';
  polynomial_print ( m, o, c, e, title );

  [ o2, c2, e2 ] = polynomial_compress ( o, c, e );

  fprintf ( 1, '\n' );
  title = '  Compressed P(X) =';
  polynomial_print ( m, o2, c2, e2, title );

  return
end


function gauss_seidel_poisson_1d ( k )

%*****************************************************************************80
%
%% GAUSS_SEIDEL_POISSON_1D uses Gauss-Seidel iteration for 1D Poisson equation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Commandline input, integer K, the grid index.
%    K specifies the number of nodes, by the formula NK = 2^K + 1.
%    K must be at least 0.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAUSS_SEIDEL_POISSON_1D:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Use Gauss-Seidel iteration for the 1D Poisson equation.\n' );

  if ( nargin < 1 )
    k = input ( '  Enter K >= 0, the grid index, so that NK = 2^K + 1:  ' );
  elseif ( ischar ( k ) )
    k = str2num ( k );
  end
%
%  Set boundaries.
%
  a = 0.0;
  b = 1.0;
%
%  Set boundary conditions.
%
  ua = 0.0;
  ub = 0.0;
%
%  Get NK.
%
  nk = 2^k + 1;
%
%  Set XK.
%
  xk = ( linspace ( a, b, nk ) )';
%
%  Get HK.
%
  hk = ( b - a ) / ( nk - 1 );
%
%  Set FK.
%
  fk = force ( xk );
  fk(1) = ua;
  fk(nk) = ub;
%
%  Set the -1/2/-1 entries of Ak.
%
%  In order that the operator Ak approximate the Poisson operator,
%  and in order that we can compare linear systems for successive grids,
%  we should NOT multiply through by hk^2.
%
  Uk = sparse ( 2:nk-1, 3:nk,   -1.0, nk, nk ) / hk^2;
  Dk = sparse ( 2:nk-1, 2:nk-1,  2.0, nk, nk ) / hk^2;
  Dk(1,1) = 1.0;
  Dk(nk,nk) = 1.0;
  Lk = sparse ( 2:nk-1, 1:nk-2, -1.0, nk, nk ) / hk^2;
  Ak = Uk + Dk + Lk;
%
%  Just because we can, ask MATLAB to get the exact solution of the linear system
%  directly.
%
  udk = Ak \ fk;
%
%  Sample the solution to the continuous problem.
%
  uek = exact ( xk );
%
%  Use Gauss-Seidel iteration to solve the linear system to the given tolerance.
%
  ujk = zeros ( nk, 1 );
  it_max = 10000000;
  res_max = 0.000001;

  [ ujk, it_num, res_norm ] = gauss_seidel ( nk, Lk, Dk, Uk, fk, ujk, it_max, ...
    res_max );
%
%  Examine errors:
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Using grid index K = %d\n', k );
  fprintf ( 1, '  System size NK was %d\n', nk );
  fprintf ( 1, '  Tolerance for linear residual %g\n', res_max );
  fprintf ( 1, '  RMS of linear residual %g\n', res_norm );
  fprintf ( 1, '  Number of GS iterations taken was %d\n', it_num );
  fprintf ( 1, '  RMS GS error in solution of linear system = %g\n', ...
    norm ( udk - ujk ) / sqrt ( nk ) );
  fprintf ( 1, '  RMS discretization error in Poisson solution = %g\n', ...
    norm ( uek - ujk ) / sqrt ( nk ) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     I        X        U_Exact    U_Direct    U_GS\n' );
  fprintf ( 1, '\n' );
  for i = 1 : nk
    fprintf ( 1, '  %4d  %10.4f  %10.4g  %10.4g  %10.4g\n', ...
      i, xk(i), uek(i), udk(i), ujk(i) );
  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAUSS_SEIDEL_POISSON_1D:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function uex = exact ( x )

%*****************************************************************************80
%
%% UEX evaluates the solution of the continuous problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(*), the evaluation points.
%
%    Output, real UEX(*), the solution of the continuous problem
%    at the evaluation points.
%
  uex = x .* ( x - 1 ) .* exp ( x );

  return
end
function f = force ( x )

%*****************************************************************************80
%
%% FORCE evaluates the forcing term.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(*), the evaluation points.
%
%    Output, real F(*), the forcing term at the evaluation points.
%
  f = - x .* ( x + 3 ) .* exp ( x );

  return
end
function [ u, it_num, res_norm ] = gauss_seidel ( n, Lk, Dk, Uk, f, u, ...
  it_max, res_max )

%*****************************************************************************80
%
%% GAUSS_SEIDEL carries out the Gauss-Seidel iteration.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real sparse Lk(*,*), Dk(*,*), Uk(*,*), the strict lower triangle,
%    the diagonal, and the strict upper triangle of A.
%
%    Input, real F(N), the right hand side.
%
%    Input, real U(N), the estimated solution.
%
%    Input, integer IT_MAX, the maximum number of iterations.
%
%    Input, real RES_MAX, a convergence tolerance on the residual.
%
%    Output, real U(N), the improved estimate of the solution.
%
%    Output, integer IT_NUM, the number of iterations taken.
%
%    Output, real RES_NORM, the RMS norm of the residual.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Step    Residual      Change\n' );
  fprintf ( 1, '\n' );

  it_num = it_max;

  for it = 1 : it_max

    u_old = u;
    u = ( Lk + Dk ) \ ( f - Uk * u_old );
    r = ( Lk + Dk + Uk ) * u - f;
    res_norm = norm ( r ) / sqrt ( n );

    fprintf ( 1, '  %6d  %10.4g  %10.4g\n', ...
      it, res_norm, norm ( u - u_old ) / sqrt ( n ) );

    if ( res_norm <= res_max )
      it_num = it;
      break;
    end

  end

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


% HNORMALISE - Normalises array of homogeneous coordinates to a scale of 1
%
% Usage:  nx = hnormalise(x)
%
% Argument:
%         x  - an Nxnpts array of homogeneous coordinates.
%
% Returns:
%         nx - an Nxnpts array of homogeneous coordinates rescaled so
%              that the scale values nx(N,:) are all 1.
%
% Note that any homogeneous coordinates at infinity (having a scale value of
% 0) are left unchanged.

% Peter Kovesi
% School of Computer Science & Software Engineering
% The University of Western Australia
% http://www.csse.uwa.edu.au/~pk
%
% February 2004

function nx = hnormalise(x)

    [rows,npts] = size(x);
    nx = x;

    % Find the indices of the points that are not at infinity
    finiteind = find(abs(x(rows,:)) > eps);

    %if length(finiteind) ~= npts
    %    warning('Some points are at infinity');
    %end

    % Normalise points not at infinity
    for r = 1:rows-1
	nx(r,finiteind) = x(r,finiteind)./x(rows,finiteind);
    end
    nx(rows,finiteind) = 1;


% VL_CUMMAX  Cumulative maximum
%   If X is a vector, VL_CUMMAX(X) is the vector with the cumulative
%   maximum of X. If X is a matrix, VL_CUMMAX(X) is the matrix of the
%   cumulative maximum of the columns of X. If X is a multi-dimensional
%   array, VL_CUMMAX(X) operates along the first non-singleton
%   dimension.
%
%   The function is analogous to CUMSUM() and CUMPROD(), but it
%   computes the cumulative maximum rather than the cumulative sum or
%   product.
%
%   The array X can be of any numeric class.
%
%   Example::
%     The cumulative maximum VL_CUMMAX([1 2 3 2 1]) is the vector
%     [1 2 3 3 3].
%
%   See also: CUMSUM(), CUMPROD(), VL_HELP().

% Author: Andrea Vedaldi

% Copyright


function prob_test077 ( )

%*****************************************************************************80
%
%% TEST077 tests GAMMA_CDF, GAMMA_PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  a = 1.0;
  b = 1.5;
  c = 3.0;

  check = gamma_check ( a, b, c );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST077 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST077\n' );
  fprintf ( 1, '  For the Gamma PDF:\n' );
  fprintf ( 1, '  GAMMA_CDF evaluates the CDF.\n' );
  fprintf ( 1, '  GAMMA_PDF evaluates the PDF.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A =             %14f\n', a );
  fprintf ( 1, '  PDF parameter B =             %14f\n', b );
  fprintf ( 1, '  PDF parameter C =             %14f\n', c );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  X  PDF   CDF\n' );
  fprintf ( 1, '\n' );

  for i = 0 : 10

    x = i / 5.0;

    cdf = gamma_cdf ( x, a, b, c );

    pdf = gamma_pdf ( x, a, b, c );

    fprintf ( 1, '  %12f  %12f  %12f\n', x, pdf, cdf );

  end

  return
end


% REALPROBA - compute the effective probability of the value
%               in the sample.
%
% Usage:
%   >> [probaMap, probaDist ] = realproba( data, discret);
%
% Inputs:
%   data       - the data onto which compute the probability
%   discret    - discretisation factor (default: (size of data)/5)
%                if 0 base the computation on a Gaussian
%                approximation of the data
%
% Outputs:
%   probaMap   - the probabilities associated with the values
%   probaDist  - the probabilities distribution
%
% Author: Arnaud Delorme, CNL / Salk Institute, 2001

% Copyright (C) 2001 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [ probaMap, sortbox ] = realproba( data, bins );

if nargin < 1
	help realproba;
	return;
end
if nargin < 2
	bins = round(size(data,1)*size(data,2)/5);
end;

if bins > 0
	% COMPUTE THE DENSITY FUNCTION
	% ----------------------------
	SIZE = size(data,1)*size(data,2);
	sortbox = zeros(1,bins);
	minimum =  min(data(:));
	maximum =  max(data(:));
	data = floor((data - minimum )/(maximum - minimum)*(bins-1))+1;
    if any(any(isnan(data))), warning('Binning failed - could be due to zeroed out channel'); end
	for index=1:SIZE
		sortbox(data(index)) = sortbox(data(index))+1;
	end
	probaMap = sortbox(data) / SIZE;
	sortbox  = sortbox / SIZE;
else
	% BASE OVER ERROR FUNCTION
	% ------------------------
	data     = (data-mean(data(:)))./std(data(:));
	probaMap = exp(-0.5*( data.*data ))/(2*pi);
	probaMap = probaMap/sum(probaMap); % because the total surface under a normalized Gaussian is 2
	sortbox  = probaMap/sum(probaMap);
end
return;


function [fds, ics, averFD, averIC] = fdvolfft(im)
% FDVOLFFT Compute fractal dimensions FDS and intercepts ICS of a 3D image
%          IM using FFT and draw rose plots of FDS and ICS
% IM: input 3D array of a 3D image (grayvalue)
% FDS: a 2D array of size 24x12 which stores the FDs in 24x12 directions
% ICS: a 2D array of size 24x12 which stores the average intercepts in
%      24x12 directions
% AVERFD: average fractal dimension for all directions
% AVERIC: average intercept for all directions
%
%
% This script is free to use except for commercial purpose
% Written by  Mr. Jianbo ZHANG, J.Zhang@ewi.utwente.nl
% 19 Jan, 2005
%
%
% for example, the following script can compute the fractal dimension of
%    an MRI image volume
%
% load mri
% D = squeeze(D) ;
% image_num = 8;
% image(D(:,:,image_num))
% axis image
% colormap(map)
% title('Image of MRI slice No. 8 ')
% [fds ics averFD averIC]= fdvolfft(D)
%
% Note: Since an MRI image is a self-affine, not self-similiar volume, its
%   fractal dimensions do not necessarily lie between 3 and 4.

tic
NUM_AZI = 24; % number of azimuth directions
              % that the frequency space is evenly divided
NUM_ZEN = 12; % number of zenith directions
              % that the frequency space is evenly divided
NUM_RAD = 30; % number of points that the radial line is evenly divided

if nargin < 1,
    error('Missed input argument which must be a 3D array!')
end

[M N P] = size(im);
xctr = 1 + bitshift(N, -1); % x coordinate of center point
yctr = 1 + bitshift(M, -1); % y coordinate of center point
zctr = 1 + bitshift(P, -1); % z coordinate of center point
imMean = mean(im(:));
fim = fftshift(fftn(double(im) - imMean));
psd = log(fim .* conj(fim) + 10^(-6));
sumBrite = zeros(NUM_AZI, NUM_ZEN, NUM_RAD); %accumulation PSD
                                          %for along each direction and
                                          %radius
nCount = zeros(NUM_AZI, NUM_ZEN, NUM_RAD); % PSD count
radius = zeros(2 * NUM_RAD,1); %accumulation PSD for all directions
radCount = zeros(2 * NUM_RAD,1); % number of PSD for all directions

%Compute phase distribution
phase = zeros(180);
for k = 1:P
    for j = 1:M
        for i = 1:N
            realv = real(fim(j,i,k));
            imagv = imag(fim(j,i,k));
            if realv == 0
                value = pi/2;
            else
                value = atan((imagv / realv));
                ang = floor(180 * (pi / 2 + value) / pi);
            end
            if ang < 0
                ang = 0;
            end
            if ang > 179
                ang = 179;
            end
            phase(ang + 1) = phase(ang + 1) + 1;
        end
    end
end
maxphase = max(phase);
figure, plot(phase / maxphase);
title('Phase histogram (0...2 \pi)');%
axis off

%accumulation of PSD for each direction and radius
rmax = log(min(min(M,N),P)/2);% maximum radius
for k = 1:P
    if k ~= zctr
        zval = zctr - k;
        z2 = zval * zval;
        for j = 1:M
            if j ~= yctr
                yval = yctr - j;
                y2 = yval * yval;
                for i = 1:N
                    if i ~= xctr
                        xval = i - xctr;
                        r = sqrt(z2 + y2 + xval * xval);
                        rho = log(r);
                        if rho > 0 & rho <= rmax
                            mval = psd(j,i,k);
                            temp1 = yval / xval;
                            temp2 = zval / r;
                            theta = atan(temp1);
                            phi = acos(temp2);
                            if xval < 0
                                theta = theta + pi;
                            end
                            if theta < 0
                                theta = theta + 2 * pi;
                            end

                            aziSN = floor(NUM_AZI * theta /(2 * pi));
                            if aziSN > NUM_AZI - 1 | aziSN < 0
                                aziSN = NUM_AZI - 1;
                            end
                            zenSN = floor(NUM_ZEN * phi / pi);
                            if zenSN > NUM_ZEN - 1 | zenSN < 0
                                zenSN = NUM_ZEN - 1;
                            end
                            radSN = floor(2 * NUM_RAD * rho / rmax);
                            h = floor(radSN/2);
                            if radSN > 2 * NUM_RAD - 1
                                h = NUM_RAD - 1;
                                radSN = 2 * NUM_RAD - 1;
                            end

                            if h >= 5
                                if zenSN == 0 | zenSN == NUM_ZEN-1
                                    sumBrite(:, zenSN + 1, h + 1) = sumBrite(:, zenSN + 1, h + 1) + mval;
                                    nCount(:, zenSN + 1, h + 1) = nCount(:,zenSN + 1, h + 1) + 1;
                                else
                                    sumBrite(aziSN + 1,zenSN + 1, h + 1) = sumBrite(aziSN + 1, zenSN + 1, h + 1) + mval;
                                    nCount(aziSN + 1, zenSN + 1, h + 1) = nCount(aziSN + 1,zenSN + 1, h + 1) + 1;
                                end
                            end
                            if radSN >= 5
                                radius(radSN + 1) = radius(radSN + 1) + mval;
                                radCount(radSN + 1) = radCount(radSN + 1) + 1;
                            end
                        end
                    end
                end
            end
        end
    end
end

%linear regression
for aziSN = 1:NUM_AZI
    for zenSN = 1: NUM_ZEN
        sumx = 0;
        sumy = 0;
        sumx2 = 0;
        sumxy = 0;
        sumn = 0;
        for range = 6:NUM_RAD
            if nCount(aziSN, zenSN, range) > 0
                yval = sumBrite(aziSN, zenSN, range)/nCount(aziSN, zenSN, range);
                xval = (range -1) * rmax / NUM_RAD;
                sumx = sumx + xval;
                sumy = sumy + yval;
                sumx2 = sumx2 + xval * xval;
                sumxy = sumxy + xval * yval;
                sumn = sumn + 1;
            end
        end
        slope(aziSN, zenSN) = (sumn * sumxy - sumx * sumy) / (sumn * sumx2 - sumx * sumx);
        ics(aziSN, zenSN) = (sumy - slope(aziSN, zenSN) * sumx) / sumn;
        fds(aziSN, zenSN) = (11 + slope(aziSN, zenSN))/2;
    end
end

%compute average slope over all directions and radius
sumn = 0;
for radSN = 6:(2 * NUM_RAD)
    if radCount(radSN) > 0
        sumn = sumn + 1;
        yval(sumn) = radius(radSN) / radCount(radSN);
        tempr(sumn) = (radSN -1) * rmax / (2 * NUM_RAD);
    end
end
p = polyfit(tempr,yval,1);
averFD =(11+ p(1))/2; % Overall average fractal dimension
averIC = p(2); % Overall average intercept
fitln = polyval(p,tempr);
figure; plot(tempr,yval,tempr,fitln,'r-');
title('Log Log plot of PSD. vs Freq.');
ylabel('Log PSD');
xlabel('Log Frequency');
legend('data','LSE fit line');

% draw hedgehog plot of fractal dimension and intercept
% I personally prefer to call these 3D visulization plots of fractal
% dimension and intercept as 'hedgehog' plot
fd = [fds; fds(1,:)];
fd = [fd, fd(:,1)];
aziSN = (1:NUM_AZI + 1)';
zenSN = 1:(NUM_ZEN + 1);
[ph th] = meshgrid(pi/2 - ((zenSN - 1) * pi / NUM_ZEN) , 2 * pi / NUM_AZI / 2 + (aziSN - 1) * 2 * pi / NUM_AZI);
[x,y,z] = sph2cart(th,ph, fd);
figure, surf(x,y,z,fd); colorbar
title('Hedgehog plot of fractal dimension');

ic = [ics; ics(1,:)];
ic = abs([ic, ic(:,1)]); % ic stores the absolue value of ics
aziSN = (1:NUM_AZI + 1)';
zenSN = 1:(NUM_ZEN + 1);
[ph th] = meshgrid(pi/2 - ((zenSN - 1) * pi / NUM_ZEN) , 2 * pi / NUM_AZI / 2 + (aziSN - 1) * 2 * pi / NUM_AZI);
[x,y,z] = sph2cart(th,ph, ic);
figure, surf(x,y,z, ic); colorbar
title('Hedgehog plot of intercept');

disp(['Elapsed time: ' num2str(toc)]);


% Calculate the eigenvalues of massive 3x3 real symmetric matrices.
% Computation is based on matrix operation and GPU computation is
% supported.
% Syntax:
% [eigenvalue1,eigenvalue2,eigenvalue3] = eigenvaluefield33( a11, a12, a13, a22, a23, a33)
% a11, a12, a13, a22, a23 and a33 specify the symmetric 3x3 real symmetric
% matrices as:
%   a11 a12 a13
% [ a12 a22 a13 ]
%   a13 a23 a33
% These six inputs must have the same size. They can be 2D, 3D or any
% dimension. The outputs eigenvalue1, eigenvalue2 and eigenvalue3 will
% follow the size and dimension of these inputs. Owing to the use of
% trigonometric functions, the inputs must be double to maintain the
% accuracy.
%
% eigenvalue1, eigenvalue2 and eigenvalue3 are the unordered resultant
% eigenvalues. They are solved using the cubic equation solver, see
% http://en.wikipedia.org/wiki/Eigenvalue_algorithm
%
% The peak memory consumption of the method is about 1.5 times of the total
% of all inputs, in addition to the original inputs. GPU computation is
% used automatically if all inputs are matlab GPU arrays.
%
% Author: Max W.K. Law
% Email:  max.w.k.law@gmail.com
% Page:   http://www.cse.ust.hk/~maxlawwk/
%
function [b,j,d] = eigenvaluefield33( a11, a12, a13, a22, a23, a33)
%    multiprocess;
%    b=a11*0;
%    j=a11*0;
%    d=a11*0;
%    parfor i=1:numel(a11)
%        [~,l]=eig([a11(i) a12(i) a13(i); a12(i) a22(i) a23(i); a13(i) a23(i) a33(i)]);
%        b(i)=l(1,1);
%        j(i)=l(2,2);
%        d(i)=l(3,3);
%    end
%    return;

    ep=1e-50;
    b=double(a11)+ep;
    d=double(a22)+ep;
    j=double(a33)+ep;

    c=-(double(a12).^2 + double(a13).^2 + double(a23).^2 - b.*d - d.*j - j.*b);
    d=-(b.*d.*j - double(a23).^2.*b - double(a12).^2.*j - double(a13).^2.*d + 2*double(a13).*double(a12).*double(a23));

    b=-double(a11) - double(a22) - double(a33) - ep - ep -ep;

    d = d + ((2*b.^3) - (9*b.*c))/27;

%     c=c-(b.^2)/3;
%     c=c.^3;
%     c=c/27;
%     c=(d.^2/4-(d.^2/4 + c));

%%%%    c=(d.^2/4-(d.^2/4 + ((3 * c - (b.^2))/3).^3/27));
    c=(b.^2)/3 - c;
    c=c.^3;
    c=c/27;
    c=max(c,0);
    c=realsqrt(c);

    j=c.^(1/3);
    c=c+(c==0);
    d=-d/2./c;
    d=min(d, 1);
    d=max(d, -1);

    d=real(acos(d)/3);
%    d=real(acos(-(d/2./c))/3);
    c=j.*cos(d);
    d=j.*sqrt(3).*sin(d);
    b=-b/3;

    j = single(-c-d+b);
    d = single(-c+d+b);

    b = single(2*c+b);

end

% Old version
%     ep=1e-20;
%     b=double(a11)+ep;
%     d=double(a22)+ep;
%     j=double(a33)+ep;
%
%     c=-double(double(a12).^2 + double(a13).^2 + double(a23).^2 - b.*d - d.*j - j.*b);
%     d=-(b.*d.*j - double(a23).^2.*b - double(a12).^2.*j - double(a13).^2.*d + 2*double(a13).*double(a12).*double(a23));
%
%     b=-double(a11) - double(a22) - double(a33) - ep - ep -ep;
%
%
%     d = ((2*b.^3) - (9*b.*c) + (27*d))/27;
%     %clear d;
%     %x = (((3*c) - (b.^2))/3);
%
%     %c = (d^2/4 + (((3*c) - (b.^2))/3).^3/27);
%
%     %clear x;
%     c=(d.^2/4-(d.^2/4 + (((3*c) - (b.^2))/3).^3/27));
%     c(c<0)=0;
%     c=realsqrt(c);
%     %clear c;
%
%
%     j=c.^(1/3);
%     d=real(acos(-(d/2./c))/3);
%     c=j.*cos(d);
%     d=j.*sqrt(3).*sin(d);
%     b=-b/3;
%
%
%     j = single(-c-d+b);
%     d = single(-c+d+b);
%
%     b = single(2*c+b);


function d=v_distchar(ar1,ar2,mode)
%V_DISTCHAR calculates the cosh spectral distance between AR coefficients D=(AR1,AR2,MODE)
%
% Inputs: AR1,AR2     AR coefficient sets to be compared. Each row contains a set of coefficients.
%                     AR1 and AR2 must have the same number of columns.
%
%         MODE        Character string selecting the following options:
%                         'x'  Calculate the full distance matrix from every row of AR1 to every row of AR2
%                         'd'  Calculate only the distance between corresponding rows of AR1 and AR2
%                              The default is 'd' if AR1 and AR2 have the same number of rows otherwise 'x'.
%
% Output: D           If MODE='d' then D is a column vector with the same number of rows as the shorter of AR1 and AR2.
%                     If MODE='x' then D is a matrix with the same number of rows as AR1 and the same number of columns as AR2'.
%
% The COSH spectral distance is the average over +ve and -ve frequency of
%
%                     cosh(log(p1/p2))-1   =   (p1-p2)^2/(2p1*p2)   =   (p1/p2 + p2/p1)/2 - 1
%
% Where p1 and p2 are the power spectra corresponding to the AR coefficient sets AR1 and AR2.
% The COSH distance is a symmetrical version of the Itakura-Saito distance: v_distchar(x,y)=(v_distisar(x,y)+v_distisar(y,x))/2

% Since the power spectrum is the fourier transform of the autocorrelation, we can calculate
% the average value of p1/p2 by taking the 0'th order term of the convolution of the autocorrelation
% functions associated with p1 and 1/p2. Since 1/p2 corresponds to an FIR filter, this convolution is
% a finite sum even though the autocorrelation function of p1 is infinite in extent.

% The Cosh distance can also be calculated directly from the power spectra; providing np is large
% enough, the values of d0 and d1 in the following will be very similar:
%
%         np=255; d0=v_distchar(ar1,ar2); d1=v_distchpf(v_lpcar2pf(ar1,np),v_lpcar2pf(ar2,np))
%

% Ref: A.H.Gray Jr and J.D.Markel, "Distance measures for speech processing", IEEE ASSP-24(5): 380-391, Oct 1976
%      L. Rabiner abd B-H Juang, "Fundamentals of Speech Recognition", Section 4.5, Prentice-Hall 1993, ISBN 0-13-015157-2

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: v_distchar.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf1,p1]=size(ar1);
nf2=size(ar2,1);
p2=p1+1;
m1=zeros(nf1,2*p1);
m2=zeros(nf2,2*p1);
m1(:,1:p1)=v_lpcar2rr(ar1);
m1(:,p2:end)=v_lpcar2ra(ar1);
m1(:,1)=m1(:,1)*0.5;
m1(:,p2)=m1(:,p1+1)*0.5;
m2(:,p2:end)=v_lpcar2rr(ar2);
m2(:,1:p1)=v_lpcar2ra(ar2);

if nargin<3 | isempty(mode) mode='0'; end
if any(mode=='d') | (mode~='x' & nf1==nf2)
   nx=min(nf1,nf2);
   d=sum(m1(1:nx,:).*m2(1:nx,:),2)-1;
else
   d=m1*m2'-1;
end