function lebesgue_test07 ( )

%*****************************************************************************80
%
%% LEBESGUE_TEST07 looks at Equidistant3 points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEBESGUE_TEST07:\n' );
  fprintf ( 1, '  Analyze Equidistant3 points.\n' );

  n_max = 11;
  l = zeros ( n_max, 1 );
  xfun = linspace ( -1.0, +1.0, 501 );
  xfun = xfun';

  for n = 1 : n_max
    x = equidistant3 ( n );
    l(n) = lebesgue_constant ( n, x, xfun );
  end

  r8vec_print ( n_max, l, '  Equidistant3 Lebesgue constants for N = 1 to 11:' )
%
%  Examine one case more closely.
%
  n = 11;
  x = equidistant3 ( n );
  r8vec_print ( n, x, '  Equidistant3 points for N = 11' );

  label = 'Equidistant3 points for N = 11';
  filename = 'equidistant3.png';
  lebesgue_plot ( n, x, xfun, label, filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot file saved as "%s"\n', filename );

  return
end


function c = correlation_damped_cosine ( n, rho, rho0 )

%*****************************************************************************80
%
%% CORRELATION_DAMPED_COSINE evaluates the damped cosine correlation function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Petter Abrahamsen,
%    A Review of Gaussian Random Fields and Correlation Functions,
%    Norwegian Computing Center, 1997.
%
%  Parameters:
%
%    Input, integer N, the number of arguments.
%
%    Input, real RHO(N,1), the arguments.
%
%    Input, real RHO0, the correlation length.
%
%    Output, real C(N,1), the correlations.
%
  rho = rho ( : );

  rhohat = abs ( rho ) / rho0;

  c = exp ( - rhohat ) .* cos ( rhohat );

  return
end


[dummy,T] = lu(rand(5));
b = rand(5,1);
%fprintf('The columns should be equal:\n');
r = [solve_triu(T,b) T\b];
assert(all(abs(r(:,1) - r(:,2)) < 1e-10))
r = [solve_tril(T',b) T'\b];
assert(all(abs(r(:,1) - r(:,2)) < 1e-10))
fprintf('Verified that solve_triu and solve_tril results match backslash.\n');

d = 100;
niter = (20000/d)^2;
A = rand(d);
[dummy,T] = lu(A);
b = rand(d,1);
tic; for i = 1:niter T\b; end; t1=toc/niter;
tic; for i = 1:niter solve_triu(T,b); end; t2=toc/niter;
fprintf('backslash: \t%g\nsolve_triu: \t%g (%g times faster)\n',t1,t2,t1/t2);
% backslash is detecting triangularity as a preprocessing step, which doubles
% the time.
%fprintf('by flops, should be %g times faster\n',...
%    flops_solve(T,b)/flops_solve_tri(T,b));

niter = ceil(niter/d);
tic; for i = 1:niter inv(T); end; t1=toc/niter;
%I = eye(size(T));
%tic; for i = 1:niter solve_triu(T,I); end; t2=toc;
tic; for i = 1:niter inv_triu(T); end; t2=toc/niter;
fprintf('inv: \t%g\ninv_triu: \t%g (%g times faster)\n',t1,t2,t1/t2);
fprintf('by flops, should be %g times faster\n',...
    flops_inv(rows(T))/flops_solve_tri(T,eye(size(T))));


J = 5;  % J: number of stages

% get filters
[Faf, Fsf] = FSfarras;
[af, sf] = dualfilt1;

x = zeros(1,256);  % zero signal

% Compute dual-tree complex DWT of zero signal
w = dualtree(x, J, Faf, af);
% Set a single (real) coefficient to 1
w{5}{1}(4) = 1;
% Compute the inverse transform
y1 = idualtree(w, J, Fsf, sf);

% Compute dual-tree complex DWT of zero signal
w = dualtree(x, J, Faf, af);
% Set a single (imaginary) coefficient to 1
w{5}{2}(4) = 1;
% Compute the inverse transform
y2 = idualtree(w, J, Fsf, sf);

% Display real and imaginary parts and magnitude
n = [1:256]/256;
plot(n,y1,n,y2,n,sqrt(y1.^2+y2.^2))
title('COMPLEX 1D WAVELET')
xlabel('t');
ylabel('\psi(t)');


function f = NLP_objFunction(x, Prob)
% f = NLP_objFunction(x, Prob)
% From tomlab quickguide

if ~isfield(Prob, 'uP')
    alpha = 100;
else
    if isempty(Prob.uP)
        alpha = 100;
    else
        alpha = Prob.uP(1);
    end
end
f = alpha * (x(2) - x(1) ^ 2) ^ 2 + (1 - x(1)) ^ 2;


function z = piecewise_eval(x,breakpoints,funs)
% PIECEWISE_EVAL: evaluates a piecewise function of x
% usage: y = PIECEWISE_EVAL(x,breakpoints,funs)
%
% arguments (input)
%  x    - vector or array of points to evaluate though the function
%
%  breakpoints - list of n breakpoints, -inf and +inf are implicitly
%         the first and last breakpoints. A function with only two
%         pieces has only one explicit breakpoint. In the event that
%         you want to define a function with breakpoints [a,b,c],
%         and only two functions, but you do not care what happens
%         for x < a or x > b, then you should specify only the
%         breakpoint b. Alternatively, one could specify all 3
%         breaks, and force the function to return NaN above and
%         below those limits.
%
%         x(i) will be identified as falling in interval (j) if
%         break(j) <= x(i) < break(j+1)
%
%  funs - cell array containing n+1 functions as scalar constants,
%         strings, anonymous functions, inline functions, or any
%         simple matlab function name.
%
%         Note: use .*, ./, .^ where appropriate in the function
%
%         These functions need not be differentiable or even
%         continuous across the breaks.
%
% arguments (output)
%  z    - evaluated function, result is same shape as x
%
% Example usage:
%  For       x < -5, y = 2
%  For -5 <= x < 0,  y = sin(x)
%  For  0 <= x < 2,  y = x.^2
%  For  2 <= x < 3,  y = 6
%  For  3 <= x,      y = inf
%
%  y = piecewise_eval(-10:10,[-5 0 2 3],{2,'sin(x)','x.^2',6,inf})

n=length(breakpoints);
% there must be n+1 funs for n breaks
if length(funs)~=(n+1)
  error 'funs and breakpoints are incompatible in size'
end

if any(diff(breakpoints)<=0)
  error 'Breakpoints must be both distinct and increasing'
end

% ensure the functions are feval-able
for i=1:(n+1)
  if ischar(funs{i})
    % A string. Make it a function
    f=inline(funs{i});
    funs{i} = f;
  elseif isa(funs{i},'function_handle') || isa(funs{i},'inline')
    % a function handle or an inline. do nothing.
  elseif isnumeric(funs{i}) | isnan(funs{i}) | isinf(funs{i})
    % A scalar value was supplied, may be NaN or inf.
    % Make it a function.
    funs{i}=@(x) funs{i};
  else
    % It must be something that feval can handle
    % directly, so leave funs{i} alone.
  end
end

% initialize as nans
z=nan(size(x));

% below the first break
k=(x<breakpoints(1));
z(k)=feval(funs{1},x(k));

left = k;
for i=2:n
  k=(~left) & (x<breakpoints(i));
  if any(k)
    z(k)=feval(funs{i},x(k));
    left = k | left;
  end
end

% over the top
k=(x>=breakpoints(end));
z(k)=feval(funs{end},x(k));


function y=synth(freq,dur,amp,Fs,type)
% y=synth(freq,dur,amp,Fs,type)
%
% Synthesize a single note
%
% Inputs:
%  freq - frequency in Hz
%  dur - duration in seconds
%  amp - Amplitude in range [0,1]
%  Fs -  sampling frequency in Hz
%  type - string to select synthesis type
%         current options: 'fm', 'sine', or 'saw'

% Copyright (c) 2009 Ken Schutte
% more info at: http://www.kenschutte.com/midi

if nargin<5
  error('Five arguments required for synth()');
end

N = floor(dur*Fs);

if N == 0
  warning('Note with zero duration.');
  y = [];
  return;

elseif N < 0
  warning('Note with negative duration. Skipping.');
  y = [];
  return;
end

n=0:N-1;
if (strcmp(type,'sine'))
  y = amp.*sin(2*pi*n*freq/Fs);

elseif (strcmp(type,'saw'))

  T = (1/freq)*Fs;     % period in fractional samples
  ramp = (0:(N-1))/T;
  y = ramp-fix(ramp);
  y = amp.*y;
  y = y - mean(y);

elseif (strcmp(type,'fm'))

  t = 0:(1/Fs):dur;
  envel = interp1([0 dur/6 dur/3 dur/5 dur], [0 1 .75 .6 0], 0:(1/Fs):dur);
  I_env = 5.*envel;
  y = envel.*sin(2.*pi.*freq.*t + I_env.*sin(2.*pi.*freq.*t));

else
  error('Unknown synthesis type');
end

% smooth edges w/ 10ms ramp
if (dur > .02)
  L = 2*fix(.01*Fs)+1;  % L odd
  ramp = bartlett(L)';  % odd length
  L = ceil(L/2);
  y(1:L) = y(1:L) .* ramp(1:L);
  y(end-L+1:end) = y(end-L+1:end) .* ramp(end-L+1:end);
end


function x = spgridm(levelseq)
% SPGRIDM  Compute grid points, maximum-norm-based grid
%    X = SPGRIDM(LEVELSEQ,D)  Computes the sparse grid points for
%    the given sequence of index LEVELSEQ and problem dimension D.
%    The coordinate value of dimension i is stored in column i of
%    the matrix X. One row of matrix X represents one grid point.
%    (Internal function)

% Author : Andreas Klimke, Universitaet Stuttgart
% Version: 1.2
% Date   : January 24, 2006

% Change log:
% V1.0   : Sep 24, 2003
% V1.1   : April 21, 2004
%          Altered function header to enable dimension-adaptive
%          grids. Simplified code.
% V1.2   : January 24, 2006
%          Changed data types to operate on uint arrays

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

% Get the number of levels
nlevels = uint32(size(levelseq,1));

% Get the dimension
d = uint16(size(levelseq,2));

npoints = zeros(nlevels,1,'uint32');

% Initialize sp with the total number of grid points of the
% level.
% Compute number of points
totalpoints = uint32(0);
for k = 1:nlevels;
	ntemp = uint32(1);
	for l = 1:d
		lev = levelseq(k,l);
		if lev == 0
			ntemp = ntemp * 3;
		else
			ntemp = ntemp * 2^uint32(lev);
		end
	end
	npoints(k) = ntemp;
	totalpoints = totalpoints + ntemp;
end

% index contains the index of the resulting array containing all
% subdomains of the level.
index = uint32(1);

x = zeros(totalpoints,d);

for kl = 1:nlevels
	level = double(levelseq(kl,:));
	for i = 1:d
		% compute the points, scaled to [0,1]
		if level(i) == 0
			c = [0; 0.5; 1];
		else
			c = (((1:2:(2^level(i).*2))).*2^(-1-level(i)))';
		end

		% Compute the number of grid points per dimension, store it
		% in repvec. The funny (level == 0) statement makes sure that
		% each level 0 dimension is counted as three.
		repvec = [2.^level+(level == 0).*2]';
		npoints = prod(repvec);
		repvec(i) = 1;
		c = repmat(shiftdim(c, 1-double(i)), repvec);
		x(index:index+npoints-1,i) = c(:);
	end
	index = index + npoints;
end


function [val,retData]=unbiasedMomentCumulant(x,r,isMoment,algorithm,retData)
%%UNBIASEDMOMENTCUMULANT Given a set of samples of a scalar distribution,
%           return an unbiased estimate of the rth order central moment (h
%           statistic), which is E{(x-mu)^r}, where E is the expected value
%           operation, when given only samples of x and not knowing the
%           mean mu. Alternatively, one could compute an unbiased estimate
%           of the rth order cumulant (k statistic). Note that for r=0, if
%           a moment is desired, then the mean is returned, rather than
%           returning 0. If r is a vector, one can estimate generalized h
%           statistics, which are an estimate of
%           E{(x-mu)^r(1)}*E{(x-mu)^r(2)}*...*E{(x-mu)^r(v)} However,
%           algorithm 2 is used for generalized h statistics and its
%           complexity increases rapidly with the number of samples.
%
%INPUTS: x A 1XN or NX1 array of scalar samples of the distribution.
%        r The scalar order of the cumulant or of the central moment.
%          Alternatively, if this is a vector, then one will get a
%          generalized h statistic, and in that instance, algorithm 2 is
%          required.
% isMoment A parameter selecting whether a moment or a cumulant is desired.
%          The default if omitted or an empty matrix is passed is true,
%          indicating that a central moment is desired.
% algorithm An optional parameter selecting which algorithm to use.
%          possible values are:
%          0 Use an explicit solution with precomputed coefficients. These
%            are only available for scalar r=1,...,5. The explicit
%            solutions use the parameters in Table I of [1]. This is the
%            default if r<=5 is scalar.
%          1 Use Dwyer's method in [1] to determine the coefficients of the
%            expression of the moment in terms of the sample power sums
%            sum(x^k) for k=1 to r.
%          2 Use the symmetric function based algorithm of [2]. This
%            algorithm can only estimate central moments, not cumulants.
%            The complexity of this algorithm increases rapidly with N.
%            This can be used with a vector r and is thus the default when
%            r is a vector.
% retData If algorithm=1 is used, then the retData value is returned. If
%         one calls this function passing back retData and keeps all other
%         inputs the same except replacing x with a different set of
%         samples, then the function will execute faster. One should not
%         pass back retData if any inputs other than x have changed.
%
%OUTPUTS: val The scalar value of the moment.
%     retData If algorithm=1, then this is a structure that can be passed
%     back so that if the function is called a second time with the same
%     inputs, changing only x to different samples, then the algorithm will
%     be faster.
%
%When mu is not available and must be found from the samples, as is the
%case here, then the average mean((x-mean(x)).^p) to approximate
%E{(x-mu)^p} is a biased estimator. Hence, h statistics are more
%complicated.
%
%EXAMPLE 1:
%Here, we estimate the sixth central moment of the scalar Laplace
%distribution from 1000 samples. Since the distribution is symmetric and
%unbounded, we can assume that if the estimator is unbiased, the averaging
%a large number of these estimates will approach the true moment value.
%That is done here and compared to the true moment value. One can also see
%how retData is used to speed up the algorithm.
% n=1000;
% lambda=0.1;
% mu=2;
% Gamma=1;
% momentNum=6;
% numRuns=1e4;
%
% hAvg=0;
% retData=[];
% for curRun=1:numRuns
%     x=LaplaceD.rand(n,lambda,mu,Gamma);
%     [val,retData]=unbiasedMomentCumulant(x,momentNum,true,[],retData);
%     hAvg=hAvg+val;
% end
% avgMomentEst=hAvg/numRuns
% muList=zeros(momentNum,1);
% for k=1:momentNum
%     muList(k)=LaplaceD.momentGenFun(lambda,mu,Gamma,k);
% end
% centralMoment=raw2CentralMoments(muList);
% trueMoment=centralMoment(end)
%One should see that the average moment and the true moment should agree to
%a few decimal places.
%
%EXAMPLE 2:
%Now, we estimate the product of the second central moment and the fourth
%central moment. This necessitates the use of algorith 2, which can be
%rather slow. This example will typically take a few seconds to run.
% rng(0);%To make the results reproducible
% n=10;
% lambda=0.25;
% mu=2;
% Gamma=1;
% p=[2,4];
% numRuns=1000;
%
% hAvg=0;
% for curRun=1:numRuns
%     x=LaplaceD.rand(n,lambda,mu,Gamma);
%     hAvg=hAvg+unbiasedMomentCumulant(x,p);
% end
% avgMomentEst=hAvg/numRuns
% muList=zeros(max(p),1);
% for k=1:max(p)
%     muList(k)=LaplaceD.momentGenFun(lambda,mu,Gamma,k);
% end
% centralMoment=raw2CentralMoments(muList);
% trueMoment=prod(centralMoment(p))
%The average moment estimate will be about 0.0998 and the true moment will
%be about 0.0938.
%
%REFERENCES:
%[1] P. S. Dwyer, "Moments of any rational integral isobaric sample moment
%    function," The Annals of Mathematical Statistics, vol. 8, no. 1, pp.
%    21-65, Mar. 1937.
%[2] D. S. Tracy and B. C. Gupta, "Generalized h-statistics and other
%    symmetric functions," The Annals of Statistics, vol. 2, no. 4, pp.
%    837-844, 1974.
%
%November 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release

if(nargin<3||isempty(isMoment))
    isMoment=true;
end

if(nargin<4||isempty(algorithm))
    if(isscalar(r))
        if(r<=5)
            algorithm=0;%Use the explicit solution.
        else
            algorithm=1;%Use the power sum solution of Dwyer.
        end
    else
        algorithm=2;

        if(isMoment==false)
            error('Vector values of r are not allowed for algorithm~=2.')
        end
    end
end

if(nargin<5)
    retData=[];
end

if(isMoment)%h-statistics
    switch(algorithm)
        case 0
            val=hStatExplicit(x,r);
            retData=[];
        case 1
            [val,retData]=momCumDwyer(x,r,false,retData);
        case 2
            val=genHStat(x,r);

            retData=[];
        otherwise
            error('Unknown Algorithm specified.')
    end
else%k-statistics
    switch(algorithm)
        case 0
            val=kStatExplicit(x,r);
            retData=[];
        case 1
            [val,retData]=momCumDwyer(x,r,true,retData);
        otherwise
            error('Unknown Algorithm specified.')
    end
end
end

function [momentVal,retData]=momCumDwyer(x,r1,isCumulant,retData)
%%MOMCUMDWYER Compute unbiased central moments (h statistics) or cumulants
%             (k statistics) using Dwyer's algorithm, described in [1]. The
%             definition of the symmetric powers in the algorithm can be
%             bettwer understood when considering [2].
%
%The algorithm uses a weird definition of a product, which involves
%appending the indices of various "b" variables on each other. Thus, an
%initial phase of the algorithm involves just determinign the indices of
%the necessary coefficients. Sections 17 and 18 of [1] give the b variables
%for k statistics and h statistics. Section 16 discusses how to turn the b
%variables into a variables. The expression for turning the a variables
%into moments, F_r, is in Section 1.
%
%REFERENCES:
%[1] P. S. Dwyer, "Moments of any rational integral isobaric sample moment
%    function," The Annals of Mathematical Statistics, vol. 8, no. 1, pp.
%    21-65, Mar. 1937.
%[2] P. S. Dwyer, "Combined expansions of products of symmetric power sums
%    and of sums of symmetric power products with application to sampling,"
%    The Annals of Mathematical Statistics, vol. 9, no. 1, pp. 1-47, Mar.
%    1938.
%
%November 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

n=length(x);

numParts=numberOfPartitions(r1,true);

if(nargin<4||isempty(retData))
    %If we have to compute the coefficients.
    %AParts holds the indices of the b that go into the sum to form the a as
    %well as their coefficients.
    aParts=cell(r1,2);
    for r=1:r1
        %Allocate space for the indices of the b values needed and their
        %coefficients.
        bIdx=cell(numParts(r+1),1);
        bCoeff=zeros(numParts(r+1),1);
        [thePartition,theData]=getNextPartition(r);
        curB=1;
        while(~isempty(thePartition))
            s=theData.d;
            p=theData.r(1:s);
            piVals=theData.m(1:s);
            rho=sum(piVals);

            [p,idx]=sort(p,'descend');
            piVals=piVals(idx);

            bCoeff(curB)=partComboNum(r,p,piVals)*(-1)^(rho-1)*factorial(rho-1);
            idx=zeros(rho,1);
            startIdx=1;
            for k=1:s
                numRep=piVals(k);
                sel=startIdx:(startIdx+numRep-1);
                idx(sel)=p(k);
                startIdx=startIdx+numRep;
            end
            bIdx{curB}=idx;

            [thePartition,theData]=getNextPartition(r,theData);
            curB=curB+1;
        end
        aParts{r,1}=bIdx;
        aParts{r,2}=bCoeff;
    end

    %Now, determine the b indices needed to construct the sum in terms of the
    %as. (The Fr equation).
    aPartNeeded=cell(r1,3);
    %This stores the a indices, the coefficient of the a term and the powers of
    %the P sums for each a term.
    [thePartition,theData]=getNextPartition(r1);
    curA=1;
    while(~isempty(thePartition))
        s=theData.d;
        p=theData.r(1:s);
        piVals=theData.m(1:s);
        rho=sum(piVals);

        [p,idx]=sort(p,'descend');
        piVals=piVals(idx);
        coeffVal=partComboNum(r1,p,piVals);

        idx=zeros(rho,1);
        startIdx=1;
        for k=1:s
            numRep=piVals(k);
            sel=startIdx:(startIdx+numRep-1);
            idx(sel)=p(k);
            startIdx=startIdx+numRep;
        end

        aPartNeeded{curA,1}=idx;
        aPartNeeded{curA,2}=coeffVal;
        aPartNeeded{curA,3}=piVals(:);
        aPartNeeded{curA,4}=p(:);

        [thePartition,theData]=getNextPartition(r1,theData);
        curA=curA+1;
    end

    %To build each of the required "a" terms with the parts needed from the
    %single index a terms.
    numCoeffs=curA-1;
    %Allocate space for the a coefficients. These must be built from the suffix
    %multiplication rule of Section 16.
    aCoeffs=zeros(numCoeffs,1);
    for curPart=1:numCoeffs
        idx=aPartNeeded{curPart,1};
        r=length(idx);

        bIdxList=aParts{idx(1),1};
        bCoeffList=aParts{idx(1),2};
        for k=2:r
            bIdxCur=aParts{idx(k),1};
            bCoeffCur=aParts{idx(k),2};
            numB=length(bIdxList);
            numCur=length(bIdxCur);

            bIdxListNew=cell(numB*numCur,1);
            bCoeffListNew=zeros(numB*numCur,1);
            newCur=1;
            for i1=1:numB
                for i2=1:numCur
                    bIdxListNew{newCur}=[bIdxList{i1};bIdxCur{i2}];
                    bCoeffListNew(newCur)=bCoeffList(i1)*bCoeffCur(i2);

                    newCur=newCur+1;
                end
            end
            bIdxList=bIdxListNew;
            bCoeffList=bCoeffListNew;
        end

        %We now have the b indices and coefficients to construct the a
        %coefficients once we compute the b values.
        numB=length(bIdxList);
        aSum=0;
        for curB=1:numB
            bIdx=bIdxList{curB};
            bCoeff=bCoeffList(curB);

            numBIdx=length(bIdx);
            s=numBIdx;

            bIdx=sort(bIdx,'descend');
            p=zeros(s,1);
            piVals=zeros(s,1);

            p(1)=bIdx(1);
            numUnique=1;
            piVals(1)=1;
            for curIdx=2:numBIdx
                if(bIdx(curIdx)==bIdx(curIdx-1))
                    s=s-1;
                    piVals(numUnique)=piVals(numUnique)+1;
                else
                    numUnique=numUnique+1;
                    p(numUnique)=bIdx(curIdx);
                    piVals(numUnique)=1;
                end
            end
            %Note that s==numUnique.
            piVals=piVals(1:s);
            p=p(1:s);
            rho=sum(piVals);

            if(isCumulant)%For Fisher k statistics
                b=(-1)^(rho-1)*factorial(rho-1)/fallingFactorial(n,rho);
            else%For h statistics
                if(s==1&&piVals(1)==1&&p(1)==r1)
                    AVal=1;
                elseif(p(1)>1&&piVals(1)==1&&s==2&&p(2)==1)
                    AVal=(-1)^piVals(2);
                elseif(p(1)==1&&s==1&&piVals(1)==r1)
                    AVal=(-1)^(r1-1)*(r1-1);
                else
                    AVal=0;
                end
                b=AVal/fallingFactorial(n,rho);
            end

            aSum=aSum+bCoeff*b;
        end

        aCoeffs(curPart)=aSum;
    end

    retData.aPartNeeded=aPartNeeded;
    retData.aCoeffs=aCoeffs;
else
    aPartNeeded=retData.aPartNeeded;
    aCoeffs=retData.aCoeffs;
    numCoeffs=length(aCoeffs);
end

%We have all of the coefficients. To get this moment with a generic length
%n set of data, we just need the power sums up to degree r.
P=zeros(r1,1);
for k=1:r1
    P(k)=sum(x.^k);
end

momentVal=0;
for curA=1:numCoeffs
    aCur=aCoeffs(curA);
    coeff=aPartNeeded{curA,2};
    piVals=aPartNeeded{curA,3};
    idx=aPartNeeded{curA,4};

    momentVal=momentVal+coeff*aCur*prod(P(idx).^piVals);
end

end

function val=partComboNum(r,p,piVals)
%%PARTCOMBONUM The expression for this multinomial value is given on page
%              23 of [1].
%
%REFERENCES:
%[1] P. S. Dwyer, "Moments of any rational integral isobaric sample moment
%    function," The Annals of Mathematical Statistics, vol. 8, no. 1, pp.
%    21-65, Mar. 1937.
%
%November 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

    %For large r, use this form to help reduce overflow.
    val=exp(gammaln(r+1)-sum(piVals.*gammaln(p+1))-sum(gammaln(piVals+1)));
end

function val=hStatExplicit(x,r)
%%HSTATEXPLICIT This implements the explicit h statistics of orders 1 to 5
%               that are given in the table I of [1] using the formula for
%               Fr given in Section 1 of [1]. h statistics are unbiased
%               estimates of the moment about the mean.
%
%INPUTS: x A 1XN or NX1 array of scalar samples of the distribution.
%        r The order of the moment. 1<=r<=5.
%
%OUTPUT: val The rth order h statistic. This is a scalar value.
%
%REFERENCES:
%[1] P. S. Dwyer, "Moments of any rational integral isobaric sample moment
%    function," The Annals of Mathematical Statistics, vol. 8, no. 1, pp.
%    21-65, Mar. 1937.
%
%November 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

n=length(x);
switch(r)
    case 1
        a1=1/n;

        P1=sum(x);
        val=a1*P1;
    case 2
        a2=1/(n-1);
        a11=-1/(n*(n-1));

        P1=sum(x);
        P2=sum(x.^2);

        val=a2*P2+a11*P1^2;
    case 3
        a3=n/((n-1)*(n-2));
        a21=-1/((n-1)*(n-2));
        c21=3;
        a111=2/(n*(n-1)*(n-2));

        P1=sum(x);
        P2=sum(x.^2);
        P3=sum(x.^3);

        val=a3*P3+c21*a21*P1*P2+a111*P1^3;
    case 4
        n13=(n-1)*(n-2)*(n-3);
        n4=n*n13;

        a4=(n^2-2*n+3)/n13;
        a31=-(n^2-2*n+3)/n4;
        c31=4;
        a22=-(2*n-3)/n4;
        c22=3;
        a211=1/n13;
        c211=6;
        a1111=-3/n4;

        P1=sum(x);
        P2=sum(x.^2);
        P3=sum(x.^3);
        P4=sum(x.^4);

        val=a4*P4+c31*a31*P3*P1+c22*a22*P2^2+c211*a211*P2*P1^2+a1111*P1^4;
    case 5
        n14=(n-1)*(n-2)*(n-3)*(n-4);
        n5=n*n14;

        a5=n*(n^2-5*n+10)/n14;
        a41=-(n^2-5*n+10)/n14;
        c41=5;
        a32=-(n-2)/n14;
        c32=10;
        a311=(n^2-4*n+8)/n5;
        c311=10;
        a221=(2*n-4)/n5;
        c221=15;
        a2111=-1/n14;
        c2111=10;
        a11111=4/n5;

        P1=sum(x);
        P2=sum(x.^2);
        P3=sum(x.^3);
        P4=sum(x.^4);
        P5=sum(x.^5);
        val=P5*a5+c41*a41*P4*P1+c32*a32*P3*P2+c311*a311*P3*P1^2+c221*a221*P2^2*P1+c2111*a2111*P2*P1^3+a11111*P1^5;
    otherwise
        error('The explicit solutions are only available up to order 5.');
end
end

function val=kStatExplicit(x,r)
%%KSTATEXPLICIT This implements the explicit Fisher k statistics of orders
%               1 to 5 that are given in the table I of [1] using the
%               formula for Fr given in Section 1 of [1]. h statistics are
%               unbiased estimates of the cumulants of the distribution. Up
%               to order 3, they are the same as the h statistics.
%
%INPUTS: x A 1XN or NX1 array of scalar samples of the distribution.
%        r The order of the moment. 1<=r<=5.
%
%OUTPUT: val The rth order h statistic. This is a scalar value.
%
%REFERENCES:
%[1] P. S. Dwyer, "Moments of any rational integral isobaric sample moment
%    function," The Annals of Mathematical Statistics, vol. 8, no. 1, pp.
%    21-65, Mar. 1937.
%
%November 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

n=length(x);
switch(r)
    case 1
        a1=1/n;

        P1=sum(x);
        val=a1*P1;
    case 2
        a2=1/(n-1);
        a11=-1/(n*(n-1));

        P1=sum(x);
        P2=sum(x.^2);

        val=a2*P2+a11*P1^2;
    case 3
        a3=n/((n-1)*(n-2));
        a21=-1/((n-1)*(n-2));
        c21=3;
        a111=2/(n*(n-1)*(n-2));

        P1=sum(x);
        P2=sum(x.^2);
        P3=sum(x.^3);

        val=a3*P3+c21*a21*P1*P2+a111*P1^3;
    case 4
        n22=(n-2)*(n-3);
        n13=(n-1)*n22;
        n4=n*n13;

        a4=n*(n+1)/n13;
        a31=-(n+1)/n13;
        c31=4;
        a22=-1/n22;
        c22=3;
        a211=2/n13;
        c211=6;
        a1111=-6/n4;

        P1=sum(x);
        P2=sum(x.^2);
        P3=sum(x.^3);
        P4=sum(x.^4);

        val=a4*P4+c31*a31*P3*P1+c22*a22*P2^2+c211*a211*P2*P1^2+a1111*P1^4;
    case 5
        n14=(n-1)*(n-2)*(n-3)*(n-4);
        n5=n*n14;

        a5=n^2*(n+5)/n14;
        a41=-n*(n+5)/n14;
        c41=5;
        a32=-n*(n-1)/n14;
        c32=10;
        a311=2*(n+2)/n14;
        c311=10;
        a221=2*(n-1)/n14;
        c221=15;
        a2111=-6/n14;
        c2111=10;
        a11111=24/n5;

        P1=sum(x);
        P2=sum(x.^2);
        P3=sum(x.^3);
        P4=sum(x.^4);
        P5=sum(x.^5);
        val=P5*a5+c41*a41*P4*P1+c32*a32*P3*P2+c311*a311*P3*P1^2+c221*a221*P2^2*P1+c2111*a2111*P2*P1^3+a11111*P1^5;
    otherwise
        error('The explicit solutions are only available up to order 5.');
end
end

function h=genHStat(x,p)
%%GENHSTAT Given a set of scalar samples, obtain an unbiased estimate of
%          the pth central sample moment. That is, estimate E{(x-mu)^p},
%          where E is the expected value operation and mu is the mean of
%          the distribution. Alternatively, p can be a length-v vector, and
%          an unbiased estimate of
%          E{(x-mu)^p(1)}*E{(x-mu)^p(2)}*...*E{(x-mu)^p(v)} is obtained.
%          When mu is not available and must be found from the samples, as
%          is the case here, then the average  mean((x-mean(x)).^p) is a
%          biased estimator. Additionally multiplying unbiased estimators
%          can lead to a biased estimator. The unbiased estimator for this
%          type of problem when p is a scalar is called an h-statistic and
%          when p is a vector is a generalized statistic.
%
%INPUTS: x An nX1 or 1Xn vector of real samples. Note that this function
%          can be slow for values of p>4 when a large number of samples is
%          used.
%        p The integer moment desired. p>=1. p can be a vector of all
%          unique elements if the product of central moments should be
%          estimated.
%
%OUTPUTS: h The unbiased estimate of the pth central moment given the
%           samples.
%
%The estimator of [1], which is an h-statistic, is rather complicated. An
%explicit expression for h-statistics in terms of symmetric means is given
%in [2], and an expression for generalized h statististics in terms of
%symmetric means is also provided. These are how this function is
%implemented. Equation 2.2 is used when p is a scalar (a central moment)
%and Equation 3.3 is used when p is a vector (a product of central
%moments). Note that the evaluation of symmetric means involves the
%evaluation of matrix permanents and thus the complexity scales
%exponentialy with the number of samples in x. Only small numbers of
%samples can be used, because the computational complexity increases too
%rapidly.
%
%REFERENCES:
%[1] P. R. Halmos, "The theory of unbiased estimation," The Annals of
%    Mathematical Statistics, vol. 17, no. 1, pp. 34-43, Mar. 1946.
%[2] D. S. Tracy and B. C. Gupta, "Generalized h-statistics and other
%    symmetric functions," The Annals of Statistics, vol. 2, no. 4, pp.
%    837-844, 1974.
%
%September 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=length(x);

if(any(p>=n))
    error('p must be >=n for an unbiased estimator to exist.')
end

if(isscalar(p))
    if(p==1)%Return the mean instead of 0.
        h=mean(x);
        return
    end

    h=0;
    for r=0:p
        h=h+(-1)^r*binomial(p,r)*symmetricMean(x,[p-r,ones(1,r)]);
    end
else
    v=length(p);

    p=p(:)';
    h=0;

    r=zeros(1,v);
    while(~isempty(r))
        sumR=sum(r);

        prodVal=1;
        for k=1:v
            prodVal=prodVal*binomial(p(k),r(k));
        end

        %We want p-r without the zero elements.
        pV=p-r;
        pV(pV==0)=[];

        h=h+(-1)^sumR*prodVal*symmetricMean(x,[pV,ones(1,sumR)]);

        r=getNextTuple(r,p);
    end
end
end

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


function [nfft,tableout]=floor23(n)
%FLOOR23  Previous number with only 2,3 factors
%   Usage: nceil=floor23(n);
%
%   `floor23(n)` returns the first number less than or equal to *n*,
%   which can be written as a product of powers of *2* and *3*.
%
%   The algorithm will look up the best size in a table, which is computed
%   the first time the function is run. If the input size is larger than the
%   largest value in the table, the input size will be reduced by factors of
%   *2*, until it is in range.
%
%   `[nceil,table]=floor23(n)` additionally returns the table used for lookup.
%
%   Examples:
%   ---------
%
%   Return the first number smaller or equal to *26* that can be written
%   solely as products of powers of *2* and *3*:::
%
%     floor23(26)
%
%   This plot shows the behaviour of |floor23| and |ceil23| for numbers
%   up to 100:::
%
%     x=1:100;
%     plot(x,floor23(x),x,ceil23(x));
%     legend('floor23','ceil23','Location','Northwest');
%
%   See also: ceil23, floor235, nextfastfft

%   AUTHOR: Peter L. Søndergaard

persistent table;

maxval=2^20;

if isempty(table)
    % Compute the table for the first time, it is empty.
    l2=log(2);
    l3=log(3);
    l5=log(5);
    lmaxval=log(maxval);
    table=zeros(143,1);
    ii=1;
    prod2=1;
    for i2=0:floor(lmaxval/l2)
        prod3=prod2;
        for i3=0:floor((lmaxval-i2*l2)/l3)
            table(ii)=prod3;
            prod3=prod3*3;
            ii=ii+1;
        end;
        prod2=prod2*2;
    end;
    table=sort(table);
end;

% Copy input to output. This allows us to efficiently work in-place.
nfft=n;

% Handle input of any shape by Fortran indexing.
for ii=1:numel(n)
  n2reduce=0;

  if n(ii)>maxval
    % Reduce by factors of 2 to get below maxval
    n2reduce=ceil(log2(nfft(ii)/maxval));
    nfft(ii)=nfft(ii)/2^n2reduce;
  end;

  % Use a simple bisection method to find the answer in the table.
  from=1;
  to=numel(table);
  while from<=to
    mid = round((from + to)/2);
    diff = table(mid)-nfft(ii);
    if diff<0
      from=mid+1;
    else
      to=mid-1;
    end
  end
  if nfft(ii)~=table(from)
      nfft(ii)=table(from-1);
  end;

  % Add back the missing factors of 2 (if any)
  nfft(ii)=nfft(ii)*2^n2reduce;

end;

tableout=table;


function pn = line_par_point_near_2d ( f, g, x0, y0, p )

%*****************************************************************************80
%
%% LINE_PAR_POINT_NEAR_2D: nearest point on parametric line to given point, 2D.
%
%  Discussion:
%
%    The parametric form of a line in 2D is:
%
%      X = X0 + F * T
%      Y = Y0 + G * T
%
%    We may normalize by choosing F*F + G*G = 1, and F nonnegative.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Adrian Bowyer, John Woodwark,
%    A Programmer's Geometry,
%    Butterworths, 1983,
%    ISBN: 0408012420.
%
%  Parameters:
%
%    Input, real F, G, X0, Y0, the parametric line parameters.
%
%    Input, real P(2,1), the point whose distance from the line is
%    to be measured.
%
%    Output, real PN(2,1), the point on the parametric line which
%    is nearest to P.
%
  t = ( f * ( p(1) - x0 ) + g * ( p(2) - y0 ) ) / ( f * f + g * g );

  pn(1,1) = x0 + t * f;
  pn(2,1) = y0 + t * g;

  return
end


function h=window(N,type, varargin)

% window
%
% h=window(N,type, varargin)
%
% Generate window of length N (in samples)
%
%
% Possible types are :
% 'Hamming', 'Hanning', 'Nuttall',  'Papoulis', 'Harris',
% 'Rect',    'Triang',  'Bartlett', 'BartHann', 'Blackman'
% 'Gauss',   'Parzen',  'Kaiser',   'Dolph',    'Hanna'.
% 'Nutbess', 'spline'
%
% For the gaussian window, an optionnal parameter K
% sets the value at both extremities. The default value is 0.005
%
% For the Kaiser-Bessel window, an optionnal parameter
% sets the scale. The default value is 3*pi.
%
% For the Spline windows, h=window(N,'spline',nfreq,p)
% yields a spline weighting function of order p and frequency
% bandwidth proportional to nfreq.
%
% Example : w=window(256,'Gauss',0.005);

if nargin < 1
	help(mfilename);
	return
end

if  N<=0
	error('N should be strictly positive.');
end

if nargin < 2
	type= 'Hamming';
end

type=upper(type);

switch(type)
	case {'RECTANG','RECT'}
	 	h=ones(N,1);
	case 'HAMMING'
		h=0.54 - 0.46*cos(2.0*pi*(1:N)'/(N+1));
	case {'HANNING', 'HANN'}
		h=0.50 - 0.50*cos(2.0*pi*(1:N)'/(N+1));
	case 'KAISER'
		if (nargin==3), beta=varargin{1}; else beta=3.0*pi; end;
		ind=(-(N-1)/2:(N-1)/2)' *2/N; beta=3.0*pi;
		h=bessel(0,j*beta*sqrt(1.0-ind.^2))/real(bessel(0,j*beta));
	case 'NUTTALL',
		ind=(-(N-1)/2:(N-1)/2)' *2.0*pi/N;
		h=+0.3635819 ...
		+0.4891775*cos(    ind) ...
		+0.1363995*cos(2.0*ind) ...
		+0.0106411*cos(3.0*ind) ;
	case 'BLACKMAN'
		ind=(-(N-1)/2:(N-1)/2)' *2.0*pi/N;
		h= +0.42 + 0.50*cos(ind) + 0.08*cos(2.0*ind) ;
	case 'HARRIS'
		ind=(1:N)' *2.0*pi/(N+1);
		h=+0.35875 ...
		-0.48829 *cos(    ind) ...
		+0.14128 *cos(2.0*ind) ...
		-0.01168 *cos(3.0*ind);
	case {'BARTLETT','TRIANG'}
		h=2.0*min((1:N),(N:-1:1))'/(N+1);
	case 'BARTHANN'
		h=  0.38 * (1.0-cos(2.0*pi*(1:N)/(N+1))') ...
		+ 0.48 * min((1:N),(N:-1:1))'/(N+1);
	case 'PAPOULIS'
		ind=(1:N)'*pi/(N+1); h=sin(ind);
	case 'GAUSS'
		if (nargin==3), K=varargin{1}; else K=0.005; end;
		h= exp(log(K) * linspace(-1,1,N)'.^2 );
	case 'PARZEN'
		ind=abs(-(N-1)/2:(N-1)/2)'*2/N; temp=2*(1.0-ind).^3;
		h= min(temp-(1-2.0*ind).^3,temp);
	case 'HANNA'
		if (nargin==3), L=varargin{1}; else L=1; end;
		ind=(0:N-1)';h=sin((2*ind+1)*pi/(2*N)).^(2*L);
	case {'DOLPH','DOLF'}
		if (rem(N,2)==0), oddN=1; N=2*N+1; end;
		if (nargin==3), A=10^(param/20); else A=1.0e-3; end;
		K=N-1; Z0=cosh(acosh(1.0/A)/K); x0=acos(1/Z0)/pi; x=(0:K)/N;
		indices1=find((x<x0)|(x>1-x0));
		indices2=find((x>=x0)&(x<=1-x0));
		h(indices1)= cosh(K*acosh(Z0*cos(pi*x(indices1))));
		h(indices2)= cos(K*acos(Z0*cos(pi*x(indices2))));
		h=fftshift(real(ifft(A*real(h))));h=h'/h(K/2+1);
 		if oddN, h=h(2:2:K); end;
	case 'NUTBESS',
		if (nargin==3), beta=varargin{1}; nu=0.5;
		elseif (nargin==4), beta=varargin{1}; nu=varargin{2};
		else beta=3*pi; nu=0.5;
		end;
		ind=(-(N-1)/2:(N-1)/2)' *2/N;
		h=sqrt(1-ind.^2).^nu .* ...
		real(bessel(nu,j*beta*sqrt(1.0-ind.^2)))/real(bessel(nu,j*beta));
	case 'SPLINE'
		if (nargin < 3),
			error('Three or four parameters required for spline windows');
		elseif (nargin==3)
			nfreq=param; p=pi*N*nfreq/10.0;
			else nfreq=varargin{1}; p=varargin{2};
		end
		ind=(-(N-1)/2:(N-1)/2)';
		h=sinc((0.5*nfreq/p)*ind) .^ p;
	otherwise
		error('unknown window type');
end


function [alpha, beta, stability] = evaluateAlphaBetaParam(process, noisy, dt)
% evaluateAlphaBetaParam - evaluates alpha and beta parameters for alpha-beta filter
% With this parameters, alpha-beta filter becomes a steady-state Kalman filter
%
% Syntax:          [xkp,vkp] = alphaBetaFilter(xm, dt, xk, vk, alpha, beta)
%   [alpha, beta, stability] = evaluateAlphaBetaParam(process, noisy, dt)
%     [alpha,beta,stability] = evalABGParam([alpha,beta])
%
% Inputs:
%   process - real system state
%     noisy - measured system state
%
% Outputs:
%   alpha - alpha parameter
%    beta - beta parameter
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: alphaBetaFilter;

% Author: Marco Borges, Ph.D. Student, Computer/Biomedical Engineer
% UFMG, PPGEE, Neurodinamica Lab, Brazil
% email address: marcoafborges@gmail.com
% Website: http://www.cpdee.ufmg.br/
% June 2013; Version: v2; Last revision: 2013-09-18
% Changelog:
%  v2 - add Stability test
%
%------------------------------- BEGIN CODE -------------------------------

if nargin == 3
    varProcess = var(process);
    varNoise = var(noisy);
    l = varProcess * dt / varNoise; % lambda
    r = (4+l-sqrt(8*l+l^2))/4;
    alpha = 1 - r^2;
    beta = 2*(2-alpha)-4*sqrt(1-alpha);
elseif nargin == 1 && length(process) == 2
    alpha = process(1);
    beta = process(2);
else
    error('evaluateAlphaBetaParam : Incorrect Parameters!');
end

if ( alpha > 0 && alpha < 2 && beta > 0 && beta < (4-2*alpha) )
    stability = 'Stable';
else
    stability = 'Unstable';
end

end
%-------------------------------- END CODE --------------------------------


close all;
clear all;
clc;
rng('default');
png_export = true;
pdf_export = false;
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');

mf = spx.graphics.Figures();

% Signal space
N = 256;
% Number of measurements
M = 64;
% Sparsity level
K = 8;
% Number of signals
S = 4;
% Construct the signal generator.
gen  = spx.data.synthetic.SparseSignalGenerator(N, K, S);
% Generate bi-uniform signals
X = gen.biUniform(1, 2);
% Sensing matrix
Phi = spx.dict.simple.gaussian_dict(M, N);
% Measurement vectors
Y = Phi * X;

solver = spx.pursuit.joint.BasisPursuit(Phi);
result = solver.solve_l2_l1(Y);
% Solution vectors
Z = result.Z;
% Comparison
cs = spx.commons.SparseSignalsComparison(X, Z, K);
cs.summarize();
for s=1:S
    mf.new_figure(sprintf('MMV signal: %d', s));
    subplot(411);
    stem(X(:, s), '.');
    title('Sparse vector');
    subplot(412);
    stem(Z(:, s), '.');
    title('Recovered sparse vector');
    subplot(413);
    stem(abs(X(:, s) - Z(:, s)), '.');
    title('Recovery error');
    subplot(414);
    stem(Y(:, s), '.');
    title('Measurement vector');
end


function val=logOnePlusXMinusX(x)
%%LOGONEPLUSXMINUSX Evaluate log(1+x)-x reducing the loss of precision that
%            occurs for values of x that are very close to 0 for real
%            values of x.
%
%INPUTS: x A matrix of real values>=-1.
%
%OUTPUTS: val The values log(1+x)-x.
%
%The function first computes z=log(1+x) using log1p(x) to reduce finite
%precision errors. This value is put into the expSlope2 function, which
%computes 2*(exp(x)-1-x)/x^2 without a low loss of finite precision.
%Multiplying the result by (-1/2)*z^2, one gets the expression log(1+x)-x.
%This approach ovoids a direct subtraction of x, which is where the finite
%precision problems occur.
%
%EXAMPLE:
%Here, we show the improvement that this algorithm offers to a direct
%evaluation.
% x=1e-150;
% valAccurate=logOnePlusXMinusX(x)
% valInaccurate=log1p(x)-x
%One will see that valAccurate is about -5e-301, whereas valInaccurate is
%just zero. The value -5e-301 can be verified using greatly extended
%precision arithmetic.
%
%October 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(any(x(:)<-1)||~isreal(x))
       error('This function only supports real values of x >=-1.');
    end

    val=zeros(size(x));

    selNeg=x<-0.99;
    xNeg=x(selNeg);
    xPos=x(~selNeg);
    if(~isempty(xNeg))
        valNeg=log1p(x)-x;
    else
        valNeg=[];
    end

    if(~isempty(xPos))
        z=log1p(x);
        e2Val=expSlope2(z);
        valPos=-(1/2)*e2Val.*z.^2;
    else
        valPos=[];
    end

    val(selNeg)=valNeg;
    val(~selNeg)=valPos;
end

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


function pts = rotate(pts, phi, theta, psi)
% ROTATE - Apply rotation to a vector of points

% Define rotation matrix (right handed)
R_roll = [...
    1, 0, 0;...
    0, cos(phi), sin(phi);...
    0, -sin(phi), cos(phi)];
R_pitch = [...
    cos(theta), 0, -sin(theta);...
    0, 1, 0;...
    sin(theta), 0, cos(theta)];
R_yaw = [...
    cos(psi), sin(psi), 0;...
    -sin(psi), cos(psi), 0;...
    0, 0, 1];
R = R_roll * R_pitch * R_yaw;   % inertial to body
R = R';  % body to inertial

% Rotate vertices
pts = R*pts;

end


function m = rnsubset(k,n)
%RNSUBSET choose k distinct random integers from 1:n M=(K,N)
%
%  Inputs:
%
%    K is number of disinct integers required from the range 1:N
%    N specifies the range - we must have K<=N
%
%  Outputs:
%
%    M(1,K) contains the output numbers

%      Copyright (C) Mike Brookes 2006
%      Version: $Id: rnsubset.m,v 1.2 2007/05/04 07:01:39 dmb Exp $
%
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if k>n
    error('rnsubset: k must be <= n');
end
% We use two algorithms according to the values of k and n
[f,e]=log2(n);
if k>0.03*n*(e-1)
[v,m]=sort(rand(1,n)); % for large k, just do a random permutation
else
    v=ceil(rand(1,k).*(n:-1:n-k+1));
    m=1:n;
    for i=1:k
        j=v(i)+i-1;
        x=m(i);
        m(i)=m(j);
        m(j)=x;
    end
end
m=m(1:k);


%% Main function to generate tests
function tests = givens_vs_svd()
tests = functiontests(localfunctions);
end

%% Test Functions
function testQRvsSVD(nullspace)
% compare 3 methods to compute null space basis vectors: svd,
% matlab null which is said to use svd, and qr decomposition
% qr decomposition may be done with givens or householder rotations

trials=1e1;
timers=zeros(3,1);
for shot=1:trials
    Hf=rand(10,3);

    tic
    [u,d,v]= svd(Hf');
    n1= v(:, 4:end);
    timers(1) = timers(1) + toc;
    assert(length(find(d>0))==3);
    %svd to get nullspace basis, https://inst.eecs.berkeley.edu/~ee127a/book/login/l_svd_mat_prop.html

    tic
    n2=null(Hf');
    timers(2) = timers(2) + toc;
    assert(max(abs(n1 - n2), [], 'all') < 1e-6)

    tic
    % compute nullspace with qr decomposition
    % http://stackoverflow.com/questions/2181418/computing-the-null-space-of-a-matrix-as-fast-as-possible
    [q, r]= qr(Hf);
    n3=q(:,4:end);
    timers(3) = timers(3) + toc;

    assert(max(abs(n1 - n3), [], 'all') < 1e-6)
end
disp('times consumed for computing nullspace by svd, matlab null, and qr');
disp(timers)
end

function testQRvsSVD2(projectToNullspace)
% To project a matrix or vector onto the nullspace of another matrix is
% useful in eliminating unnecessary variables. e.g., r= H_x*X+H_f*p +n
% where H_f is of size 2M*3. p is of size 3*1. To remove p, we need to
% project r and H_x onto the nullspace of H_f', Q_2.
% Note H_f =[Q_1, Q_2][R; 0]= Q[R; 0]
%
% To accomplish this, we can successively apply Givens rotations on
% [r, H_x, H_f] until it becomes [Q'*r, Q'*H_x, [R; 0]]. And remove its first
% 3 rows, we get [Q_2'*r, Q_2'*H_x, 0].
%
% This trick is used in Mourikis, Anastasios, and Stergios Roumeliotis.
% "A multi-state constraint Kalman filter for vision-aided inertial navigation."
% Robotics and Automation, 2007 IEEE International Conference on. IEEE, 2007.
% How to apply Givens rotations can be found in
% G. Golub and C. van Loan, Matrix computations. The Johns Hopkins
% University Press, London, 1996.

trials=1e2;
timers=zeros(2,1);
% For small matrices, svd outperforms qr +svd; for large thin tall
% matrices, svd is a little less efficient than qr +svd
m =10; %3000;
n =5; %1200;
for shot=1:trials
    assert(m>n);
    A= rand(m, n);
    tic
    [~, ~, v] = svd(A);
    x1= v(:, end);
    timers(1) = timers(1) + toc;

    tic
    [~, r] = qr(A);
    Th= r(1:n, 1:n);
    [~, ~, v2]= svd(Th);
    x2= v2(:, end);
    timers(2) = timers(2) + toc;

    assert(max(abs(x1 - x2), [], 'all') < 1e-6)
end

disp('times consumed for solving least squares by svd, and qr+svd');
disp(timers)
end


% EX_MAXWELL_EIG_MIXED2_SQUARE: solve Maxwell eigenproblem in the unit square, with the second mixed formulation.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_square.txt';

% Type of boundary conditions
problem_data.nmnn_sides   = [];
problem_data.drchlt_sides = [1 2 3 4];

% Physical parameters
problem_data.c_elec_perm = @(x, y) ones(size(x));
problem_data.c_magn_perm = @(x, y) ones(size(x));

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.degree     = [3 3];     % Degree of the bsplines
method_data.regularity = [2 2];     % Regularity of the splines
method_data.nsub       = [10 10];     % Number of subdivisions
method_data.nquad      = [4 4];     % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER
[geometry, msh, space, sp_mul, eigv, eigf] = ...
                       solve_maxwell_eig_mixed2_2d (problem_data, method_data);

% 4) POSTPROCESSING
[eigv, perm] = sort (eigv);

fprintf ('First computed eigenvalues: \n')
disp (eigv(1:6))

figure
sp_plot_solution (eigf(1:space.ndof,perm(9)), space, geometry, [30 30])
title ('8^{th} eigenfunction')

%!demo
%! ex_maxwell_eig_mixed2_square

%!test
%! problem_data.geo_name = 'geo_square.txt';
%! problem_data.nmnn_sides   = [];
%! problem_data.drchlt_sides = [1 2 3 4];
%! problem_data.c_elec_perm = @(x, y) ones(size(x));
%! problem_data.c_magn_perm = @(x, y) ones(size(x));
%! method_data.degree     = [3 3];     % Degree of the bsplines
%! method_data.regularity = [2 2];     % Regularity of the splines
%! method_data.nsub       = [10 10];     % Number of subdivisions
%! method_data.nquad      = [4 4];     % Points for the Gaussian quadrature rule
%! [geometry, msh, space, sp_mul, eigv, eigf] = solve_maxwell_eig_mixed2_2d (problem_data, method_data);
%! [eigv, perm] = sort (eigv);
%! nzeros = numel (find (eigv < 1e-4));
%! assert (msh.nel, 100)
%! assert (space.ndof, 312)
%! assert (sp_mul.ndof, 144)
%! assert (eigv(nzeros+(1:5))/pi^2, [1.00000003326399; 1.00000003326400; 2.00000006652799; 4.00000968384168; 4.00000968384168], 2e-14)


function value = gamma_inc ( p, x )

%*****************************************************************************80
%
%% GAMMA_INC computes the incomplete Gamma function.
%
%  Discussion:
%
%    GAMMA_INC(P,X) = Integral ( 0 <= T <= X ) T**(P-1) EXP(-T) DT / GAMMA(P).
%
%    GAMMA_INC(P,       0) = 0,
%    GAMMA_INC(P,Infinity) = 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 September 2004
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    B L Shea,
%    Chi-squared and Incomplete Gamma Integral,
%    Algorithm AS239,
%    Applied Statistics,
%    Volume 37, Number 3, 1988, pages 466-473.
%
%  Parameters:
%
%    Input, real P, the exponent parameter.
%    0.0 < P.
%
%    Input, real X, the integral limit parameter.
%    If X is less than or equal to 0, GAMMA_INC is returned as 0.
%
%    Output, real VALUE, the value of the function.
%
  exp_arg_min = -88.0;
  overflow = 1.0E+37;
  plimit = 1000.0;
  tol = 1.0E-07;
  xbig = 1.0E+08;

  value = 0.0;

  if ( p <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'GAMMA_INC - Fatal error!\n' );
    fprintf ( 1, '  Parameter P <= 0.\n' );
    error ( 'GAMMA_INC - Fatal error!' );
  end

  if ( x <= 0.0 )
    value = 0.0;
    return
  end
%
%  Use a normal approximation if PLIMIT < P.
%
  if ( plimit < p )
    pn1 = 3.0 * sqrt ( p ) * ( ( x / p )^( 1.0 / 3.0 ) + 1.0 / ( 9.0 * p ) - 1.0 );
    cdf = normal_01_cdf ( pn1 );
    value = cdf;
    return
  end
%
%  Is X extremely large compared to P?
%
  if ( xbig < x )
    value = 1.0;
    return
  end
%
%  Use Pearson's series expansion.
%  (P is not large enough to force overflow in the log of Gamma.
%
  if ( x <= 1.0 | x < p )

    arg = p * log ( x ) - x - gammaln ( p + 1.0 );
    c = 1.0;
    value = 1.0;
    a = p;

    while ( 1 )

      a = a + 1.0;
      c = c * x / a;
      value = value + c;

      if ( c <= tol )
        break
      end

    end

    arg = arg + log ( value );

    if ( exp_arg_min <= arg )
      value = exp ( arg );
    else
      value = 0.0;
    end

  else
%
%  Use a continued fraction expansion.
%
    arg = p * log ( x ) - x - gammaln ( p );
    a = 1.0 - p;
    b = a + x + 1.0;
    c = 0.0;
    pn1 = 1.0;
    pn2 = x;
    pn3 = x + 1.0;
    pn4 = x * b;
    value = pn3 / pn4;

    while ( 1 )

      a = a + 1.0;
      b = b + 2.0;
      c = c + 1.0;
      pn5 = b * pn3 - a * c * pn1;
      pn6 = b * pn4 - a * c * pn2;

      if ( 0.0 < abs ( pn6 ) )

        rn = pn5 / pn6;

        if ( abs ( value - rn ) <= min ( tol, tol * rn ) )

          arg = arg + log ( value );

          if ( exp_arg_min <= arg )
            value = 1.0 - exp ( arg );
          else
            value = 1.0;
          end

          return

        end

        value = rn;

      end

      pn1 = pn3;
      pn2 = pn4;
      pn3 = pn5;
      pn4 = pn6;
%
%  Rescale terms in continued fraction if terms are large.
%
      if ( overflow <= abs ( pn5 ) )
        pn1 = pn1 / overflow;
        pn2 = pn2 / overflow;
        pn3 = pn3 / overflow;
        pn4 = pn4 / overflow;
      end

    end

  end

  return
end


function [V, Q, policy] = mdp_bellman_operator_with_Q(P, PR, discount, Vprev)

% mdp_bellman_operator Applies the Bellman operator on the value function Vprev
%                      Returns a new value function and a Vprev-improving policy
% Arguments ---------------------------------------------------------------
% Let S = number of states, A = number of actions
%   P(SxSxA) = transition matrix
%              P could be an array with 3 dimensions or
%              a cell array (1xA), each cell containing a matrix (SxS) possibly sparse
%   PR(SxA) = reward matrix
%              PR could be an array with 2 dimensions or
%              a sparse matrix
%   discount = discount rate, in ]0, 1]
%   Vprev(S) = value function
% Evaluation --------------------------------------------------------------
%   V(S)   = new value function
%   policy(S) = Vprev-improving policy

% MDPtoolbox: Markov Decision Processes Toolbox
% Copyright (C) 2009  INRA
% Redistribution and use in source and binary forms, with or without modification,
% are permitted provided that the following conditions are met:
%    * Redistributions of source code must retain the above copyright notice,
%      this list of conditions and the following disclaimer.
%    * 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.
%    * Neither the name of the <ORGANIZATION> 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.

if iscell(P); S = size(P{1},1); else S = size(P,1); end;
if discount <= 0 || discount > 1
     disp('--------------------------------------------------------')
     disp('MDP Toolbox ERROR: Discount rate must be in ]0; 1]')
     disp('--------------------------------------------------------')
elseif size(Vprev,1) ~= S
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: Vprev must have the same dimension as P')
    disp('--------------------------------------------------------')
else

    if iscell(P)
        A = length(P);
        for a=1:A
            Q(:,a) = PR(:,a) + discount*P{a}*Vprev;
        end
    else
        A = size(PR,2);
        for a=1:A
            Q(:,a) = PR(:,a) + discount*P(:,:,a)*Vprev;
        end
    end
    [V, policy] = max(Q,[],2);

end;


% -----------------------------------------------------------------  %
% Matlab Programs included the Appendix B in the book:               %
%  Xin-She Yang, Engineering Optimization: An Introduction           %
%                with Metaheuristic Applications                     %
%  Published by John Wiley & Sons, USA, July 2010                    %
%  ISBN: 978-0-470-58246-6,   Hardcover, 347 pages                   %
% -----------------------------------------------------------------  %
% Citation detail:                                                   %
% X.-S. Yang, Engineering Optimization: An Introduction with         %
% Metaheuristic Application, Wiley, USA, (2010).                     %
%                                                                    %
% http://www.wiley.com/WileyCDA/WileyTitle/productCd-0470582464.html %
% http://eu.wiley.com/WileyCDA/WileyTitle/productCd-0470582464.html  %
% -----------------------------------------------------------------  %
% ===== ftp://  ===== ftp://   ===== ftp:// =======================  %
% Matlab files ftp site at Wiley                                     %
% ftp://ftp.wiley.com/public/sci_tech_med/engineering_optimization   %
% ----------------------------------------------------------------   %

% Simulated Annealing (by X-S Yang, Cambridge University)            %
% Usage: B2_sa                                                       %
% For the constrained optimization, please see the file: sa_mincon.m %
% ------------------------------------------------------------------ %

disp('Simulating ... it will take a minute or so!');
% Rosenbrock's function with f*=0 at (1,1)
fstr='(1-x)^2+100*(y-x^2)^2';
% Convert into an inline function
f=vectorize(inline(fstr));
% Show the topography of the objective function
range=[-2 2 -2 2];
xgrid=range(1):0.1:range(2); ygrid=range(3):0.1:range(4);
[x,y]=meshgrid(xgrid,ygrid);
surfc(x,y,f(x,y));
% Initializing parameters and settings
T_init = 1.0;       % Initial temperature
T_min =  1e-10;     % Finial stopping temperature
F_min = -1e+100;    % Min value of the function
max_rej=5000;       % Maximum number of rejections
max_run=500;        % Maximum number of runs
max_accept = 250;   % Maximum number of accept
k = 1;              % Boltzmann constant
alpha=0.95;         % Cooling factor
Enorm=1e-8;         % Energy norm (eg, Enorm=1e-8)
guess=[2 2];        % Initial guess
% Initializing the counters i,j etc
i= 0; j = 0; accept = 0; totaleval = 0;
% Initializing various values
T = T_init;
E_init = f(guess(1),guess(2));
E_old = E_init; E_new=E_old;
best=guess;  % initially guessed values
% Starting the simulated annealling
while ((T > T_min) & (j <= max_rej) & E_new>F_min)
    i = i+1;
    % Check if max numbers of run/accept are met
    if (i >= max_run) | (accept >= max_accept)
    % Cooling according to a cooling schedule
        T = alpha*T;
        totaleval = totaleval + i;
        % reset the counters
        i = 1;  accept = 1;
    end
    % Function evaluations at new locations
      ns=guess+rand(1,2)*randn;
      E_new = f(ns(1),ns(2));
    % Decide to accept the new solution
    DeltaE=E_new-E_old;
    % Accept if improved
    if (-DeltaE > Enorm)
        best = ns; E_old = E_new;
        accept=accept+1;   j = 0;
    end
    % Accept with a small probability if not improved
    if (DeltaE<=Enorm & exp(-DeltaE/(k*T))>rand );
        best = ns; E_old = E_new;
        accept=accept+1;
    else
        j=j+1;
    end
    % Update the estimated optimal solution
    f_opt=E_old;
end
% Display the final results
disp(strcat('Obj function  :',fstr));
disp(strcat('Evaluations   :', num2str(totaleval)));
disp(strcat('Best  solution:', num2str(best)));
disp(strcat('Best objective:', num2str(f_opt)));


function p = predict(Theta1, Theta2, X)
%PREDICT Predict the label of an input given a trained neural network
%   p = PREDICT(Theta1, Theta2, X) outputs the predicted label of X given the
%   trained weights of a neural network (Theta1, Theta2)

% Useful values
m = size(X, 1);
num_labels = size(Theta2, 1);

% You need to return the following variables correctly
p = zeros(size(X, 1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned neural network. You should set p to a
%               vector containing labels between 1 to num_labels.
%
% Hint: The max function might come in useful. In particular, the max
%       function can also return the index of the max element, for more
%       information see 'help max'. If your examples are in rows, then, you
%       can use max(A, [], 2) to obtain the max for each row.
%

Z = zeros(size(X, 1), 1);

X = [ones(m, 1) X];

A2 = sigmoid(X * Theta1');

A2 = [ones(m, 1), A2];

A3 = sigmoid(A2 * Theta2');

[Z, p] = max(A3, [], 2);

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

end


function [Pro,Res] = interpolationAMGsa(A,node2agg,omega,smoothingstep)
%% INTERPOLATIONAMGSA construct prolongation using smoothed aggregation
%
% [Pro,Res] = INTERPOLATIONAMGSA(A,node2agg) construct prolongation and
% restriction matrices using smoothed aggregation.
%
% In the input, A is a SPD matrix and node2agg assigns an aggregate for
% each node in A. In the output Pro and Res are prolongation and
% restriction matrices satisfying Res = Pro'.
%
% The prolongation operator is first set to be piecewise constant and then
% using matrix A to smooth out this simple prolongation using weighted
% Jacobi iteration for several steps. The default choice of the weight is
% 0.35 and the smoothingstep is 2. Larger smoothing steps will result a
% denser prolongation operator.
%
% Example
%   load lakemesh
%   A = assemblematrix(node,elem);
%   [node2agg,As] = coarsenAMGa(A);
%   [Pro,Res] = interpolationAMGsa(As,node2agg);
%
% See also: coarsenAMGc, amg
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('omega','var'), omega = 0.35; end
if ~exist('smoothingstep','var'), smoothingstep = 2; end

%% A simple prolongation
N = size(A,1);
Nc = max(node2agg);
idx = find(node2agg~=0);
Pro = sparse(idx,node2agg(idx),1,N,Nc);

%% Smooth the piecewise constant prolongation
for k = 1:smoothingstep
    Pro = Pro - omega*(A*Pro);
end

%% Normalize the prolongation such that the row sum is one
rowsum = sum(Pro,2);
D = spdiags(1./rowsum,0,N,N);
Pro = D*Pro;
Res = Pro';


function [c ceq gradc gradceq] = hs71C(x)

c = -(prod(x)-25);
ceq = sum(x.^2)-40;

if(nargout > 2)
    gradc = -(prod(x)./x')';
    gradceq = 2*x;
end


function line_num = sphere_llt_grid_line_num ( lat_num, long_num )

%*****************************************************************************80
%
%% SPHERE_LLT_GRID_LINE_NUM counts lines for an LLT grid.
%
%  Discussion:
%
%    An LLT grid is a grid of triangles bounded by latitude and longitude
%    lines over the surface of a sphere in 3D.
%
%    The number returned is the number of pairs of points to be connected.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LAT_NUM, LONG_NUM, the number of latitude and
%    longitude lines to draw.  The latitudes do not include the North and South
%    poles, which will be included automatically, so LAT_NUM = 5, for instance,
%    will result in points along 7 lines of latitude.
%
%    Output, integer LINE_NUM, the number of grid lines.
%
  line_num = long_num * ( lat_num + 1 ) ...
           + long_num *   lat_num ...
           + long_num * ( lat_num - 1 );

  return
end


function points = intersectLineCircle(line, circle)
%INTERSECTLINECIRCLE Intersection point(s) of a line and a circle.
%
%   INTERS = intersectLineCircle(LINE, CIRCLE);
%   Returns a 2-by-2-by-N array, containing on each row the coordinates of
%   an intersection point for each line-circle pair, i.e. INTERS(:,:,k)
%   contains the intersections between LINE(k,:) and CIRCLE(k,:).
%
%   If a line-circle pair does not intersect, the corresponding results are
%   set to NaN.
%
%   Example
%     % base point
%     center = [10 0];
%     % create vertical line
%     l1 = [center 0 1];
%     % circle
%     c1 = [center 5];
%     pts = intersectLineCircle(l1, c1)
%     pts =
%     10   -5
%     10    5
%     % draw the result
%     figure; clf; hold on;
%     axis([0 20 -10 10]);
%     drawLine(l1);
%     drawCircle(c1);
%     drawPoint(pts, 'rx');
%     axis equal;
%
%   See also
%   lines2d, circles2d, intersectLines, intersectCircles
%
%   References
%   http://local.wasp.uwa.edu.au/~pbourke/geometry/sphereline/
%   http://mathworld.wolfram.com/Circle-LineIntersection.html
%

% ------
% Authors: David Legland, JuanPi Carbajal
% E-mail: david.legland@inrae.fr, ajuanpi+dev@gmail.com
% Created: 2011-01-14, using Matlab 7.9.0.529 (R2009b)
% Copyright 2011-2022 INRA - Cepia Software Platform

% check size of inputs
nLines = size(line, 1);
nCircles = size(circle, 1);
if nLines ~= nCircles
  error ('matGeom:geom3d:invalidArguments', ...
      'Requires same number of lines and circles');
end

% center parameters
center = circle(:, 1:2);
radius = circle(:, 3);

% line parameters
dp = line(:, 1:2) - center;
vl = line(:, 3:4);

% coefficients of second order equation
a = sum(line(:, 3:4).^2, 2);
b = 2 * sum(dp .* vl, 2);
c = sum(dp.^2, 2) - radius.^2;

% discriminant
delta = b .^ 2 - 4 * a .* c;

points = nan(2, 2, nCircles);

valid = delta >= 0;

if any(valid)
    % compute roots (as a N-by-N-by-2 array)
    u = bsxfun(@plus, -b(valid), bsxfun(@times, [-1 1], sqrt(delta(valid))));
    u = bsxfun(@rdivide, u, a(valid)) / 2;

    if sum(valid) == 1
        points = [...
            line(1:2) + u(:,1) .* line(3:4); ...
            line(1:2) + u(:,2) .* line(3:4)];
    else
        tmp = [...
            line(valid, 1:2) + u(:,1) .* line(valid, 3:4) ...
            line(valid, 1:2) + u(:,2) .* line(valid, 3:4)].';
	    points(:, :, valid) = permute(reshape(tmp, [2, 2, nCircles]), [2 1 3]);
    end
end


function [X_poly] = polyFeatures(X, p)
%POLYFEATURES Maps X (1D vector) into the p-th power
%   [X_poly] = POLYFEATURES(X, p) takes a data matrix X (size m x 1) and
%   maps each example into its polynomial features where
%   X_poly(i, :) = [X(i) X(i).^2 X(i).^3 ...  X(i).^p];
%

% You need to return the following variables correctly.
X_poly = zeros(numel(X), p);

% ====================== YOUR CODE HERE ======================
% Instructions: Given a vector X, return a matrix X_poly where the p-th
%               column of X contains the values of X to the p-th power.
%

for i=1:p

   X_poly(:,i) = X.^i;
end

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

end


function [y,r,vr]=ssa(x1,L)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% -----------------------------------------------------------------
%    Author: Francisco Javier Alonso Sanchez    e-mail:fjas@unex.es
%    Departament of Electronics and Electromecanical Engineering
%    Industrial Engineering School
%    University of Extremadura
%    Badajoz
%    Spain
% -----------------------------------------------------------------
%
% SSA generates a trayectory matrix X from the original series x1
% by sliding a window of length L. The trayectory matrix is aproximated
% using Singular Value Decomposition. The last step reconstructs
% the series from the aproximated trayectory matrix. The SSA applications
% include smoothing, filtering, and trend extraction.
% The algorithm used is described in detail in: Golyandina, N., Nekrutkin,
% V., Zhigljavsky, A., 2001. Analisys of Time Series Structure - SSA and
% Related Techniques. Chapman & Hall/CR.

% x1 Original time series (column vector form)
% L  Window length
% y  Reconstructed time series
% r  Residual time series r=x1-y
% vr Relative value of the norm of the approximated trajectory matrix with respect
%	  to the original trajectory matrix

% The program output is the Singular Spectrum of x1 (must be a column vector),
% using a window length L. You must choose the components be used to reconstruct
%the series in the form [i1,i2:ik,...,iL], based on the Singular Spectrum appearance.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Step1 : Build trayectory matrix

   N=length(x1);
   if L>N/2;L=N-L;end
	K=N-L+1;
   X=zeros(L,K);
	for i=1:K
	  X(1:L,i)=x1(i:L+i-1);
	end

% Step 2: SVD

   S=X*X';
	[U,autoval]=eig(S);
	[d,i]=sort(-diag(autoval));
   d=-d;
   U=U(:,i);sev=sum(d);
	plot((d./sev)*100),hold on,plot((d./sev)*100,'rx');
	title('Singular Spectrum');xlabel('Eigenvalue Number');ylabel('Eigenvalue (% Norm of trajectory matrix retained)')
   V=(X')*U;
   rc=U*V';

% Step 3: Grouping

   I=input('Choose the agrupation of components to reconstruct the series in the form I=[i1,i2:ik,...,iL]  ')
   Vt=V';
   rca=U(:,I)*Vt(I,:);

% Step 4: Reconstruction

   y=zeros(N,1);
   Lp=min(L,K);
   Kp=max(L,K);

   for k=0:Lp-2
     for m=1:k+1;
      y(k+1)=y(k+1)+(1/(k+1))*rca(m,k-m+2);
     end
   end

   for k=Lp-1:Kp-1
     for m=1:Lp;
      y(k+1)=y(k+1)+(1/(Lp))*rca(m,k-m+2);
     end
   end

   for k=Kp:N
      for m=k-Kp+2:N-Kp+1;
       y(k+1)=y(k+1)+(1/(N-k))*rca(m,k-m+2);
     end
   end

   figure;subplot(2,1,1);hold on;xlabel('Data poit');ylabel('Original and reconstructed series')
   plot(x1);grid on;plot(y,'r')

   r=x1-y;
   subplot(2,1,2);plot(r,'g');xlabel('Data poit');ylabel('Residual series');grid on
   vr=(sum(d(I))/sev)*100;


function [sigma_points, w_m, w_c] = compute_sigma_points(mu, sigma, lambda, alpha, beta)
% This function samples 2n+1 sigma points from the distribution given by mu and sigma
% according to the unscented transform, where n is the dimensionality of mu.
% Each column of sigma_points should represent one sigma point
% i.e. sigma_points has a dimensionality of nx2n+1.
% The corresponding weights w_m and w_c of the points are computed using lambda, alpha, and beta:
% w_m = [w_m_0, ..., w_m_2n], w_c = [w_c_0, ..., w_c_2n] (i.e. each of size 1x2n+1)
% They are later used to recover the mean and covariance respectively.

n = length(mu);
sigma_points = zeros(n,2*n+1);
w_m = zeros(1,2*n+1);
w_c = zeros(1,2*n+1);

% TODO: compute all sigma points
sigma_points(:,1) = mu;
addTerm = sqrtm((n+lambda)*sigma);
for i=1:n
    sigma_points(:,i+1) = mu + addTerm(:,i);
endfor
for i=n+1:2*n
    sigma_points(:,i+1) = mu - addTerm(:,i-n);
endfor

% TODO compute weight vectors w_m and w_c
w_m(1,1) = lambda/(n+lambda);
w_c(1,1) = w_m(1) + (1 - alpha*alpha + beta);
w_m(1,2:end) = 1/(2*(n+lambda));
w_c(1,2:end) = 1/(2*(n+lambda));
end


%%% Projection onto Feasible motion space

% Course: Robotic Manipulation and Mobility
% Advisor: Dr. V. Krovi
%
% Homework Number: MIDTERM
%
% Names: Sourish Chakravarty
% 	Hrishi Lalit Shah

function [dX]= ROBO_midterm_f1(t,X)

global l1 lc1 m1 j1 tau1 l2 lc2 m2 j2 tau2 g

th1=X(1);
x2= X(2);
y2= X(3);
th2=X(4);
th1d= X(5);
th2d= X(6);
%%%%%%%%%%%%%%%%%%%%%%% CREATING IMPORTANT MATRICES IN THE GOVERNING EQUATION
s1=sin(th1);
s2=sin(th2);
c1=cos(th1);
c2=cos(th2);

%%%% Element - 1
M1= [j1+m1*(lc1^2)];
E1= [1,-1];
G1= [m1*lc1*c1*g];
% B11 = [-l1*s1, l1*c1];
% B12 = [-m1*lc1*c1];

%%%% Element - 2
M2= [m2, 0, 0;
     0, m2, 0;
     0, 0, j2];
E2= [ 0; 0; 1];
G2= [0; m2*g; 0];
% B21= [-1, 0;
%      0, -1;
%      -lc2*s2, lc2*c2];
% B22= [0, -m2, 0]';

%%%% Constraints
C= [x2-l1*c1-lc2*c2;
    y2-l1*s1-lc2*s2];
A= [l1*s1, 1, 0, lc2*s2;
    -l1*c1, 0, 1, -lc2*c2];% Jacobian of constraint matrix
S = [1, 0;
    -l1*s1, -lc2*s2;
    l1*c1, lc2*c2;
    0, 1]; % Null space of A or Feasible Motion
Sd = [0, 0;
    -l1*c1*th1d, -lc2*c2*th2d;
    -l1*s1*th1d, -lc2*s2*th2d;
    0, 0]; % Derivative of S matrix

%%%%%%%%%%%%%%%%%%%%%%% PROJECTION ONTO FEASIBLE MOTION SPACE
M=[M1, zeros(1,3);
    zeros(3,1), M2];
E=[E1;
    zeros(3,1),E2];
G= [G1;G2];
T =[tau1; tau2];
V=zeros(4,1);

t1= inv(S'*M*S);
t2= S'*(M*Sd*[th1d,th2d]'+V+G-E*T);

dX(1:4,1)=S*[th1d,th2d]';
dX(5:6,1)= -t1*t2;
% Cerr1=([Cerr1;abs(C')]);
% Tstore1=[Tstore1,t];
return


function [ rexp, sexp ] = poly_q9 ( )

%*****************************************************************************80
%
%% POLY_Q9 returns the monomials associated with a 9 node quadrilateral.
%
%  Reference Element Q9:
%
%    |
%    1  4--7--3
%    |  |     |
%    |  |     |
%    S  8  9  6
%    |  |     |
%    |  |     |
%    0  1--5--2
%    |
%    +--0--R--1-->
%
%  Formula:
%
%    Given coefficients A(I), the polynomial interpolant at (R,S) is
%
%      P(R,S) = sum ( 1 <= I <= N ) A(I) * R**REXP(I) * S**SEXP(I)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer REXP(9), SEXP(9), the powers of R and S associated
%    with each monomial.
%
  rexp(1:9) = [ 0, 0, 1, 0, 1, 2, 1, 2, 2 ];
  sexp(1:9) = [ 0, 1, 0, 2, 1, 0, 2, 1, 2 ];

  return
end


function varargout = rosenbruck(X)
% Extended Rosenbruck's Banana-function, for N-dimensional input
%
%   ROSENBRUCK([x1, x2, .., xn]) returns the value of the Rosenbruck
%   function at the specified points. All [xi] may be vectors. The search
%   domain is
%
%               -100 < x_i < 100
%
%   The global minimum is
%
%               f(x1, x2, ..., xn) = f(1, 1, ..., 1) = 0

% Author: Rody P.S. Oldenhuis
% Delft University of Technology
% E-mail: oldenhuis@dds.nl
% Last edited 28/Feb/2009

    % if no input is given, return dimensions, bounds and minimum
    if (nargin == 0)
        varargout{1} = inf;  % # dims
        varargout{2} = -100; % LB
        varargout{3} = +100; % UB
        varargout{4} = 1; % solution
        varargout{5} = 0; % function value at solution

    % otherwise, output function value
    else

        % keep all values within the domain
        X(X < -100) = inf;  X(X > 100) = inf;

        % split input vector X into X1, X2
        % NOTE: proper orientation can not be determined automatically
        % the sum is taken by default over the rows:
        X1 = X(1:2:end-1, :);        X2 = X(2:2:end, :);

        % output rowsum
        varargout{1} = sum(  100*(X2 - X1.^2).^2 + (1 - X1).^2, 1);
    end

end


% Separating ellipsoids in 2D
% Joelle Skaf - 11/06/05
% (a figure is generated)
%
% Finds a separating hyperplane between 2 ellipsoids {x| ||Ax+b||^2<=1} and
% {y | ||Cy + d||^2 <=1} by solving the following problem and using its
% dual variables:
%               minimize    ||w||
%                   s.t.    ||Ax + b||^2 <= 1       : lambda
%                           ||Cy + d||^2 <= 1       : mu
%                           x - y == w              : z
% the vector z will define a separating hyperplane because z'*(x-y)>0

% input data
n = 2;
A = eye(n);
b = zeros(n,1);
C = [2 1; -.5 1];
d = [-3; -3];

% solving for the minimum distance between the 2 ellipsoids and finding
% the dual variables
cvx_begin
    variables x(n) y(n) w(n)
    dual variables lam muu z
    minimize ( norm(w,2) )
    subject to
    lam:    square_pos( norm (A*x + b) ) <= 1;
    muu:    square_pos( norm (C*y + d) ) <= 1;
    z:      x - y == w;
cvx_end

t = (x + y)/2;
p=z;
p(1) = z(2); p(2) = -z(1);
c = linspace(-2,2,100);
q = repmat(t,1,length(c)) +p*c;

% figure
nopts = 1000;
angles = linspace(0,2*pi,nopts);
[u,v] = meshgrid([-2:0.01:4]);
z1 = (A(1,1)*u + A(1,2)*v + b(1)).^2 + (A(2,1)*u + A(2,2)*v + b(2)).^2;
z2 = (C(1,1)*u + C(1,2)*v + d(1)).^2 + (C(2,1)*u + C(2,2)*v + d(2)).^2;
contour(u,v,z1,[1 1]);
hold on;
contour(u,v,z2,[1 1]);
axis square
plot(x(1),x(2),'r+');
plot(y(1),y(2),'b+');
line([x(1) y(1)],[x(2) y(2)]);
plot(q(1,:),q(2,:),'k');


function points = intersectLineCircle(line, circle)
%INTERSECTLINECIRCLE Intersection point(s) of a line and a circle
%
%   INTERS = intersectLineCircle(LINE, CIRCLE);
%   Returns a 2-by-2-by-N array, containing on each row the coordinates of
%   an intersection point for each line-circle pair, i.e. INTERS(:,:,k)
%   contains the intersections between LINE(k,:) and CIRCLE(k,:).
%
%   If a line-circle pair does not intersect, the corresponding results are
%   set to NaN.
%
%   Example
%     % base point
%     center = [10 0];
%     % create vertical line
%     l1 = [center 0 1];
%     % circle
%     c1 = [center 5];
%     pts = intersectLineCircle(l1, c1)
%     pts =
%     10   -5
%     10    5
%     % draw the result
%     figure; clf; hold on;
%     axis([0 20 -10 10]);
%     drawLine(l1);
%     drawCircle(c1);
%     drawPoint(pts, 'rx');
%     axis equal;
%
%   See also
%   lines2d, circles2d, intersectLines, intersectCircles
%
%   References
%   http://local.wasp.uwa.edu.au/~pbourke/geometry/sphereline/
%   http://mathworld.wolfram.com/Circle-LineIntersection.html
%

% ------
% Author: David Legland, david.legland@inra.fr
% Author: JuanPi Carbajal <ajuanpi+dev@gmail.com>
% Created: 2011-01-14,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

% HISTORY
% 2011-06-06 fix bug in delta test
% 2017-05-05 included some suggestions from code by JuanPi Carbajal <ajuanpi+dev@gmail.com>
% 2017-08-08 update doc

% check size of inputs
nLines = size(line, 1);
nCircles = size(circle, 1);
if nLines ~= nCircles
  error ('matGeom:geom3d:invalidArguments', ...
      'Requires same number of lines and circles');
end

% center parameters
center = circle(:, 1:2);
radius = circle(:, 3);

% line parameters
dp = line(:, 1:2) - center;
vl = line(:, 3:4);

% coefficient of second order equation
a = sum(line(:, 3:4).^2, 2);
b = 2*sum(dp .* vl, 2);
c = sum(dp.^2, 2) - radius.^2;

% discriminant
delta = b .^ 2 - 4 * a .* c;

points = nan(2, 2, nCircles);

valid = delta >= 0;

if any(valid)
    % compute roots
    u = bsxfun(@plus, -b(valid), bsxfun(@times, [-1 1], sqrt(delta(valid))));
    u = bsxfun(@rdivide, u, a(valid)) / 2;

    if nCircles == 1
        points = [...
            line(1:2) + u(:,1) .* line(3:4); ...
            line(1:2) + u(:,2) .* line(3:4)];
    else
        tmp = [...
            line(valid, 1:2) + u(:,1) .* line(valid, 3:4) ...
            line(valid, 1:2) + u(:,2) .* line(valid, 3:4)].';
	    points(:, :, valid) = permute(reshape(tmp, [2, 2, nCircles]), [2 1 3]);
    end
end


function [ pr, pq ] = plane_normal_basis_3d ( pp, normal )

%*****************************************************************************80
%
%% PLANE_NORMAL_BASIS_3D finds two perpendicular vectors in a plane in 3D.
%
%  Discussion:
%
%    The normal form of a plane in 3D is:
%
%      PP is a point on the plane,
%      N is a normal vector to the plane.
%
%    The two vectors to be computed, PQ and PR, can be regarded as
%    the basis of a Cartesian coordinate system for points in the plane.
%    Any point in the plane can be described in terms of the "origin"
%    point PP plus a weighted sum of the two vectors PQ and PR:
%
%      P = PP + a * PQ + b * PR.
%
%    The vectors PQ and PR have unit length, and are perpendicular to N
%    and to each other.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real PP(3,1), a point on the plane.  (Actually,
%    we never need to know these values to do the calculation!)
%
%    Input, real NORMAL(3,1), a normal vector N to the plane.  The
%    vector must not have zero length, but it is not necessary for N
%    to have unit length.
%
%    Output, real PQ(3,1), a vector of unit length,
%    perpendicular to the vector N and the vector PR.
%
%    Output, real PR(3,1), a vector of unit length,
%    perpendicular to the vector N and the vector PQ.
%
  dim_num = 3;
%
%  Compute the length of NORMAL.
%
  normal_norm = norm ( normal );

  if ( normal_norm == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PLANE_NORMAL_BASIS_3D - Fatal error!\n' );
    fprintf ( 1, '  The normal vector is 0.\n' );
    error ( 'PLANE_NORMAL_BASIS_3D - Fatal error!' );
  end
%
%  Find a vector PQ that is normal to NORMAL and has unit length.
%
  pq = r8vec_any_normal ( 3, normal );
%
%  Now just take the cross product NORMAL x PQ to get the PR vector.
%
  pr = r8vec_cross_product_3d ( normal, pq );

  pr_norm = norm ( pr );

  pr(1:dim_num,1) = pr(1:dim_num,1) / pr_norm;

  return
end


function [J, grad] = cofiCostFunc(params, Y, R, num_users, num_movies, ...
                                  num_features, lambda)
%COFICOSTFUNC Collaborative filtering cost function
%   [J, grad] = COFICOSTFUNC(params, Y, R, num_users, num_movies, ...
%   num_features, lambda) returns the cost and gradient for the
%   collaborative filtering problem.
%

% Unfold the U and W matrices from params
X = reshape(params(1:num_movies*num_features), num_movies, num_features);
Theta = reshape(params(num_movies*num_features+1:end), ...
                num_users, num_features);

% You need to return the following values correctly
J = 0;
X_grad = zeros(size(X));
Theta_grad = zeros(size(Theta));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost function and gradient for collaborative
%               filtering. Concretely, you should first implement the cost
%               function (without regularization) and make sure it is
%               matches our costs. After that, you should implement the
%               gradient and use the checkCostFunction routine to check
%               that the gradient is correct. Finally, you should implement
%               regularization.
%
% Notes: X - num_movies  x num_features matrix of movie features
%        Theta - num_users  x num_features matrix of user features
%        Y - num_movies x num_users matrix of user ratings of movies
%        R - num_movies x num_users matrix, where R(i, j) = 1 if the
%            i-th movie was rated by the j-th user
%
% You should set the following variables correctly:
%
%        X_grad - num_movies x num_features matrix, containing the
%                 partial derivatives w.r.t. to each element of X
%        Theta_grad - num_users x num_features matrix, containing the
%                     partial derivatives w.r.t. to each element of Theta
%

J = sum(((X * Theta' - Y) .^ 2)(R == 1)) / 2;
J = J + lambda / 2 * (sum(sum(Theta .^ 2)) + sum(sum(X .^ 2)));

X_grad = ((X * Theta' - Y) .* R) * Theta;
Theta_grad = ((X * Theta' - Y) .* R)' * X;

X_grad = X_grad + (lambda * X);
Theta_grad = Theta_grad + (lambda * Theta);

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

grad = [X_grad(:); Theta_grad(:)];

end


% fit_circle.m

%fit circle to set of positions

% Eq1: (X-x0)^2 + (Y-y0)^2 = r0^2
% Eq2: (-2*x0)*X + (-2*y0)*Y + (x0^2 + y0^2 - r0^2) = -(X^2 + Y^2)
% Eq3: [X Y 1] * [-2*x0; -2*y0; x0^2+y0^2-r0^2] = -(X^2+Y^2)
% Eq4: A*x = b

% Copyright 2003-2010 The MathWorks, Inc.

% substitute:
A = [X(:) Y(:) ones(size(X(:)))];
b = -(X(:).^2+Y(:).^2);

% solve:
x = A\b;

% back calculate parameters
x0 = -x(1)/2;
y0 = -x(2)/2;
r0 = sqrt(x0^2 + y0^2 - x(3));

%draw fitted circular arc through data points
[x_lo,i1]=min(X); th1=atan2(Y(i1)-y0,X(i1)-x0);
[x_hi,i2]=max(X); th2=atan2(Y(i2)-y0,X(i2)-x0);
th=linspace(th1,th2,100);
[xx,yy]=pol2cart(th,r0*ones(size(th)));
figure(myFig)
line(xx+x0+x1,yy+y0+y1,'color','m')
line([X(i1) x0 X(i2)]+x1,[Y(i1) y0 Y(i2)]+y1,'color','g')
axis tight
title(sprintf('Center = (%.1f,%.1f), Radius = %.1f pixels',x0,y0,r0))


close all;
clc;
clearvars;
rng('default');

try_spx = true;
try_ehsan = true;
try_cvx = false;

% dimension of ambient space
n = 100;
% number of subspaces = number of clusters
ns = 2;
% dimensions of individual subspaces 1 and 2
d1  = 2;
d2 = 2;
% number of signals in individual subspaces
s1 = 100;
s2 = 100;
% total number of signals
s = s1 + s2;
% A random basis for first subspace;
basis1 = randn(n,d1);
% An origin for first subspace
origin1 = randn(n, 1);
% coefficients for s1 vectors chosen randomly in subspace 1
coeffs1 = randn(d1,s1);
% Random signals from first subspace
Y1 = basis1 * coeffs1 + origin1;
% A random basis for second subspace
basis2 = randn(n,d2);
% An origin for second subspace
origin2 = randn(n, 1);
% coefficients for s2 vectors chosen randomly in subspace 2
coeffs2 = randn(d2,s2);
% Random signals from first subspace
Y2 = basis2 * coeffs2 + origin2;
% Prepare the overall set of signals
Y = [Y1 Y2];
% ground through clustering data
true_labels = [1*ones(s1,1) ; 2*ones(s2,1)];
% the largest dimension amongst all subspaces
K = max(d1, d2);
% All signals are expected to  have a K-sparse representation

% This is an affine problem
affine = true;

if try_spx
    fprintf('Attempting our implementation of SPR-ADMM-Affine\n');
    tstart = tic;
    options.affine = true;
    [C1, details] = spx.cluster.ssc.spr_admm(Y, options);
    elapsed_time = toc(tstart);
    fprintf('Maximum difference: %.4f\n', max(max(abs(Y - Y*C1))));
    fprintf('Number of iterations: %d\n', details.iterations);
    fprintf('Elapsed time: %.4f seconds\n', elapsed_time);
    disp(details);
    figure;
    imshow(C1);
end

if try_ehsan
    fprintf('Attempting admmLasso_mat_func\n');
    tstart = tic;
    [C2, details] = admmLasso_mat_func(Y, affine);
    elapsed_time = toc(tstart);
    fprintf('Maximum difference: %.4f\n', max(max(abs(Y - Y*C2))));
    fprintf('Number of iterations: %d\n', details.iterations);
    fprintf('Elapsed time: %.4f seconds\n', elapsed_time);
    disp(details);
    figure;
    imshow(C2);
end

if try_cvx
    fprintf('Attempting CVX\n');
    tstart = tic;
    options.verbose = 1;
    options.affine = affine;
    [C3, details] = spx.cluster.ssc.spr_cvx(Y, options);
    elapsed_time = toc(tstart);
    fprintf('Maximum difference: %.4f\n', max(max(abs(Y - Y*C3))));
    fprintf('Elapsed time: %.4f seconds\n', elapsed_time);
    disp(details);
    figure;
    imshow(C3);
end


function y = digamma(x)
%DIGAMMA   Digamma function.
% DIGAMMA(X) returns digamma(x) = d log(gamma(x)) / dx
% If X is a matrix, returns the digamma function evaluated at each element.

% Reference:
%
%    J Bernardo,
%    Psi ( Digamma ) Function,
%    Algorithm AS 103,
%    Applied Statistics,
%    Volume 25, Number 3, pages 315-317, 1976.
%
% From http://www.psc.edu/~burkardt/src/dirichlet/dirichlet.f

large = 9.5;
d1 = -0.5772156649015328606065121;  % digamma(1)
d2 = pi^2/6;
small = 1e-6;
s3 = 1/12;
s4 = 1/120;
s5 = 1/252;
s6 = 1/240;
s7 = 1/132;
s8 = 691/32760;
s9 = 1/12;
s10 = 3617/8160;

% Initialize
y = zeros(size(x));

% illegal arguments
i = find(x == -Inf | isnan(x));
if ~isempty(i)
  x(i) = NaN;
  y(i) = NaN;
end

% Negative values
i = find(x < 0);
if ~isempty(i)
  % Use the reflection formula (Jeffrey 11.1.6):
  % digamma(-x) = digamma(x+1) + pi*cot(pi*x)
  y(i) = digamma(-x(i)+1) + pi*cot(-pi*x(i));
  % This is related to the identity
  % digamma(-x) = digamma(x+1) - digamma(z) + digamma(1-z)
  % where z is the fractional part of x
  % For example:
  % digamma(-3.1) = 1/3.1 + 1/2.1 + 1/1.1 + 1/0.1 + digamma(1-0.1)
  %               = digamma(4.1) - digamma(0.1) + digamma(1-0.1)
  % Then we use
  % digamma(1-z) - digamma(z) = pi*cot(pi*z)
end

i = find(x == 0);
if ~isempty(i)
  y(i) = -Inf;
end

%  Use approximation if argument <= small.
i = find(x > 0 & x <= small);
if ~isempty(i)
  y(i) = y(i) + d1 - 1 ./ x(i) + d2*x(i);
end

%  Reduce to digamma(X + N) where (X + N) >= large.
while(1)
  i = find(x > small & x < large);
  if isempty(i)
    break
  end
  y(i) = y(i) - 1 ./ x(i);
  x(i) = x(i) + 1;
end

%  Use de Moivre's expansion if argument >= large.
% In maple: asympt(Psi(x), x);
i = find(x >= large);
if ~isempty(i)
  r = 1 ./ x(i);
  y(i) = y(i) + log(x(i)) - 0.5 * r;
  r = r .* r;
  y(i) = y(i) - r .* ( s3 - r .* ( s4 - r .* (s5 - r .* (s6 - r .* s7))));
end


function value = legendre_3d_monomial_integral ( a, b, p )

%*****************************************************************************80
%
%% LEGENDRE_3D_MONOMIAL_INTEGRAL the Legendre integral of a monomial.
%
%  Discussion:
%
%    The Legendre integral to be evaluated has the form
%
%      I(f) = integral ( z1 <= z <= z2 )
%             integral ( y1 <= y <= y2 )
%             integral ( x1 <= x <= x2 ) x^i y^j z^k dx dy dz
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 August 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A(3), the lower limits of integration.
%
%    Input, real B(3), the upper limits of integration.
%
%    Input, integer P(3), the exponents of X and Y.
%
%    Output, real VALUE, the value of the exact integral.
%
  value = ( b(1) ^ ( p(1) + 1 ) - a(1) ^ ( p(1) + 1 ) ) / ( p(1) + 1 ) ...
        * ( b(2) ^ ( p(2) + 1 ) - a(2) ^ ( p(2) + 1 ) ) / ( p(2) + 1 ) ...
        * ( b(3) ^ ( p(3) + 1 ) - a(3) ^ ( p(3) + 1 ) ) / ( p(3) + 1 );

  return
end


% Copyright (C) 2016, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function jacobian_symbolic_spherical_v
% link lengths
L3 = 0.38;
L6 = 0.065;

syms q1 q2 q3
% matrices DH
A01 = dh_sym(q1, L3, 0, pi/2);
A12 = dh_sym(q2, 0, 0, -pi/2);
A23 = dh_sym(q3, L6, 0, 0);

A02 = A01*A12;
A03 = A01*A12*A23;

z0 = [0 0 1]';
z1 = A01(1:3,3);
z2 = A02(1:3,3);

p03=A03(1:3,4);
p13=A03(1:3,4)-A01(1:3,4);
p23=A03(1:3,4)-A02(1:3,4);

Jv = [cross(z0, p03) cross(z1, p13) cross(z2, p23)];

singularities = det(Jv)

function A = dh_sym(theta, d, a, alpha)
syms q1 q2 q3
% avoid almost zero elements in cos(alpha) and sin(alpha)
ca = cos(alpha);
sa = sin(alpha);
if abs(ca) < 1e-6
    ca = 0;
end
if abs(sa) < 1e-6
    sa = 0;
end

A=[cos(theta)  -ca*sin(theta)   sa*sin(theta)   a*cos(theta);
   sin(theta)   ca*cos(theta)  -sa*cos(theta)   a*sin(theta);
            0              sa             ca             d;
            0         0                     0              1];


% PAD_SIZE Compute the optimal size for padding
%
% Usage
%   sz_padded = PAD_SIZE(sz, min_margin, max_ds)
%
% Input
%   sz (int): The size of the original signal.
%   min_margin (int): The minimum margin for padding.
%   max_ds (int): The maximum downsampling factor.
%
% Output
%   sz_padded (int): The minimum size of the padded signal.
%
% Description
%   Calculates the smallest multiple of 2^max_ds larger than sz by at least
%   2*min_margin. This ensures that there is enough margin on both sides of
%   the signal to avoid border effects, assuming that min_margin is equal
%   to at least half of the size of the largest filter used, while ensuring
%   that downsampling by powers of 2 up to 2^max_ds are possible through
%   periodization of the Fourier transform.
%   sz_added is also enforced to be at least 1
%
% See Also
%   PAD_SIGNAL, UNPAD_SIGNAL

function sz_padded = pad_size(sz, min_margin, max_ds)
    sz_padded = 2^max_ds * ceil( (sz + 2*min_margin)/2^max_ds );
    sz_padded = max(1, sz_padded);
end


function exactness_test09 ( )

%*****************************************************************************80
%
%% EXACTNESS_TEST09 tests Gauss-Chebyshev2 rules for the Chebyshev2 integral.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 May 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'EXACTNESS_TEST09\n' );
  fprintf ( 1, '  Test Gauss-Chebyshev2 rules for the Chebyshev2 integral.\n' );
  fprintf ( 1, '  Density function rho(x) = sqrt(1-x^2).\n' );
  fprintf ( 1, '  Region: -1 <= x <= +1.\n' );
  fprintf ( 1, '  Exactness: 2*N-1.\n' );

  for n = 1 : 5

    [ x, w ] = chebyshev2_set ( n );
    p_max = 2 * n;
    chebyshev2_exactness ( n, x, w, p_max );

  end

  return
end


function [ x, w ] = hermite_ek_compute ( n )

%*****************************************************************************80
%
%% HERMITE_EK_COMPUTE computes a Gauss-Hermite quadrature rule.
%
%  Discussion:
%
%    The code uses an algorithm by Elhay and Kautsky.
%
%    The abscissas are the zeros of the N-th order Hermite polynomial.
%
%    The integral:
%
%      integral ( -oo < x < +oo ) exp ( - x * x ) * f(x) dx
%
%    The quadrature rule:
%
%      sum ( 1 <= i <= n ) w(i) * f ( x(i) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 May 2012
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer N, the number of abscissas.
%
%    Output, real X(N), the abscissas.
%
%    Output, real W(N), the weights.
%

%
%  Define the zero-th moment.
%
  zemu = gamma ( 0.5 );
%
%  Define the Jacobi matrix.
%
  bj = zeros ( n, 1 );
  for i = 1 : n
    bj(i) = i / 2.0;
  end
  bj(1:n) = sqrt ( bj(1:n) );

  x = zeros ( n, 1 );

  w = zeros ( n, 1 );
  w(1) = sqrt ( zemu );
%
%  Diagonalize the Jacobi matrix.
%
  [ x, w ] = imtqlx ( n, x, bj, w );
%
%  If N is odd, force the center X to be 0.
%
  if ( mod ( n, 2 ) == 1 )
    x((n+1)/2) = 0.0;
  end

  w(1:n) = w(1:n).^2;

  return
end


function [ pols, dersx, dersy ] = klegeypols3 ( x, y, n )

%*****************************************************************************80
%
%% KLEGEYPOLS3 evaluate scaled Legendre polynomials and derivatives.
%
%  Discussion:
%
%    This routine evaluates a sequence of scaled Legendre polynomials
%    P_n(x/y) y^n, with the parameter y in [0,1], together with their
%    derivatives with respect to the parameters x and y.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    27 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Input, real Y, the parameter value.
%
%    Input, integer N, the highest degree to be evaluated.
%
%    Output, real POLS(N+1), the polynomial values.
%
%    Output, real DERSX(N+1), the derivatives with respect to X.
%
%    Output, real DERSY(N+1), the derivatives with respect to Y.
%
  pkp1 = 1.0;
  pols(1) = pkp1;
  dkp1 = 0.0;
  dersx(1) = dkp1;
  ykp1 = 0.0;
  dersy(1) = ykp1;

  if ( n == 0 )
    return
  end

  pk = pkp1;
  pkp1 = x;
  pols(2) = pkp1;
  dk = dkp1;
  dkp1 = 1.0;
  dersx(2) = dkp1;
  yk = ykp1;
  ykp1 = 0.0;
  dersy(2) = ykp1;

  if ( n == 1 )
    return
  end

  for k = 1 : n - 1
    pkm1 = pk;
    pk = pkp1;
    dkm1 = dk;
    dk = dkp1;
    ykm1 = yk;
    yk = ykp1;
    pkp1 = ( ( 2.0 * k + 1.0 ) * x * pk - k * pkm1 * y * y ) / ( k + 1.0 );
    dkp1 = ( ( 2.0 * k + 1.0 ) * ( x * dk + pk ) ...
      - k * dkm1 * y * y ) / ( k + 1.0 );
    ykp1 = ( ( 2.0 * k + 1.0 ) * ( x * yk ) ...
      - k * ( pkm1 * 2.0 * y + ykm1 * y * y ) ) / ( k + 1.0 );
    pols(k+2) = pkp1;
    dersx(k+2) = dkp1;
    dersy(k+2) = ykp1;
  end

  return
end


function exact = p42_exact ( )

%*****************************************************************************80
%
%% P42_EXACT returns the exact integral for problem 42.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the value of the integral.
%
  alpha = p42_param_get ( );

  exact = 2.0^( alpha - 2.0 ) * ( gamma ( alpha / 2.0 ) )^2 / gamma ( alpha );

  return
end