function constant_test ( )

%*****************************************************************************80
%
%% CONSTANT_TEST examines the constant correlation.
%
%  Discussion:
%
%    This code is based substantially on a document by Toby Driscoll.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Toby Driscoll,
%    Mercer's theorem and the Karhunen-Loeve expansion,
%    Oxford University Mathematical Institute,
%    http://www2.maths.ox.ac.uk/chebfun/examples/stats/pdf/MercerKarhunenLoeve.pdf
%
  rmpath ( '/usr/local/matlab/toolbox/datafeed/datafeed' )
  addpath ( '../chebfun' )

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CONSTANT_TEST:\n' );
  fprintf ( 1, '  Demonstrate Mercer''s theorem and the KL expansion\n' );
  fprintf ( 1, '  for the constant kernel.\n' );
%
%  Set the interval.
%
  a = 0.0;
  b = +10.0;
  fprintf ( 1, '  Using interval [%g,%g]\n', a, b );
  s_num = 21;
  s_vec = linspace ( a, b, s_num );
%
%  FRED is a function from the CHEBFUN library.
%
%  It constructs a "chebop" representing the Fredholm integral operator
%  with kernel K for functions in domain [A,B].
%
  F = fred ( @constant_correlation, domain ( [ a, b ] ) );
%
%  EIGS has been extended to be able to compute the eigenvalues Lambda
%  and eigenfunctions Psi of the Fredholm integral operator represented by F.
%
%  The "LM" switch requests that we return the eigenvalues of largest magnitude.
%
%  Each Psi is a CHEBFUN, that is, it takes a real number argument.
%
  eigen_num = 20;
  [ Psi, Lambda ] = eigs ( F, eigen_num, 'lm' );

  eigen_found = length ( Lambda );
  fprintf ( 1, '  Requested %d eigenmodes, computed %d\n', eigen_num, eigen_found );
  eigen_num = min ( eigen_num, eigen_found );
%
%  Print the eigenvalues.
%
  lambda_vec = diag ( Lambda );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   I       Lambda(I)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : eigen_num
    fprintf ( 1, '  %2d  %10g\n', i, lambda_vec(i) );
  end
%
%  Plot the eigenvalues.
%
  figure ( 1 )
  clf
  plot ( lambda_vec, 'Linewidth', 2 );
  hold on
  plot ( lambda_vec, 'b.', 'Markersize', 20 );
  title ( 'constant: Mercer eigenvalues' );
  xlabel ( '<--- N --->')
  grid on
  print -dpng 'constant_figure1.png'
%
%  Plot selected eigenfunctions.
%
  figure ( 2 )
  subplot ( 4, 1, 1 )
  plot ( Psi(:,1), 'Linewidth', 2 );
  title ( 'constant: Mercer eigenfunction PSI(1)' )
  grid on
  if ( 2 <= eigen_found )
    subplot ( 4, 1, 2 )
    plot ( Psi(:,2), 'Linewidth', 2 );
    title ( 'constant: Mercer eigenfunction PSI(2)' )
    grid on
  end
  if ( 3 <= eigen_found )
    subplot ( 4, 1, 3 )
    plot ( Psi(:,5), 'Linewidth', 2 );
    title ( 'constant: Mercer eigenfunction PSI(5)' )
    grid on
  end
  if ( 4 <= eigen_found )
    subplot ( 4, 1, 4 )
    plot ( Psi(:,10), 'Linewidth', 2 );
    title ( 'constant: Mercer eigenfunction PSI(10)' )
    grid on
  end
  print -dpng 'constant_figure2.png'
%
%  Orthonormality check.
%
  ptp = Psi' * Psi;
  error_frobenius = r8mat_is_identity ( eigen_num, ptp );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Frobenius norm of I - Psi'' * Psi = %g\n', error_frobenius );
%
%  K(S,S) should be exactly 1.
%  Because we are using a truncated representation of K, our estimate of K(S,S)
%  will be smaller than 1.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Truncated estimate of K(s,s) = 1 for S in the interval.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '        S        K(s,s) estimate\n' );
  fprintf ( 1, '\n' );
  for i = 1 : 21
    s = s_vec(i);
    ptlp = Psi(s,:) * Lambda * Psi(s,:)';
    fprintf ( 1, '  %10g  %14g\n', s, ptlp );
  end
%
%  Look at eigenvalue decay.
%
  x = diff ( log ( ( 1:eigen_num ) ) );
  y = diff ( log ( lambda_vec ) )';
  c = y ./ x;
  figure ( 3 );
  clf
  plot ( c, 'Linewidth', 2 );
  grid on
  hold on
  plot ( c, 'b.', 'Markersize', 20 );
  xlabel ( '<-- N -->' )
  title ( 'constant: Eigenvalue decay rate' );
  print -dpng 'constant_figure3.png'
%
%  Look at eigenvalue sum.
%
%  The trace of K(s,s) over [a,b] should be the integral of 1 over [a,b],
%  that is, b - a.
%
%  We compare the trace to the partial sums of the lambda's to see how much
%  of the variance of the process we have captured.
%
  trace_K = b - a;
  lambda_cum = cumsum ( lambda_vec ) / trace_K;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Index    Cumulative Eigenvalue sum\n' );
  fprintf ( 1, '\n' );
  for i = 1 : eigen_num
    fprintf ( 1, '  %5d  %10g\n', i, lambda_cum(i) );
  end
%
%  We decide to use the first 10 eigenfunctions.
%
  eigen_use = min ( 10, eigen_found );
%
%  Find 400 realizations of the process by selecting, for each realization,
%  10 random parameters Z in the truncated KL expansion.
%
  Z = randn ( eigen_use, 400 );
  X = Psi(:,1:eigen_use) * ( sqrt ( Lambda(1:eigen_use,1:eigen_use) ) * Z );
%
%  Plot 40 of the realizations;
%  Plot their mean, computed from all 400.
%
  figure ( 4 )
  clf
  plot ( X(:,1:40) )
  mu = sum ( X, 2 ) / 400;
  hold on;
  plot ( mu, 'k', 'linewidth', 3 );
  title ( 'constant: 40 Random Realizations X(t,omega), and their Mean.' )
  print -dpng 'constant_figure4.png'
%
%  Estimate the covariance from the data.
%
  figure ( 5 )
  clf
  [ S, T ] = meshgrid ( s_vec, s_vec );
  C = cov ( X(s_vec,:)' );
  mesh ( S, T, C );
  hold on;
  D = constant_correlation ( S, T );
  plot3 ( S, T, D, 'k.', 'markersize', 10 )
  title ( 'constant: Covariance K(S,T) (dots), and Estimate from Realizations (mesh)' )
  print -dpng 'constant_figure5.png'
%
%  Using just 10 functions in the expansion,
%  reduce the correlation length,
%  and examine the sum of the lambda's.
%
  if ( 0 )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use a fixed number of eigenfunctions = %d\n', eigen_use );
  fprintf ( 1, '  but vary the correlation length RHOBAR.\n' );
  fprintf ( 1, '  (We used RHOBAR = 1 above.)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The sum of the eigenvalues, divided by (B-A),\n' );
  fprintf ( 1, '  discloses the relative amount of the total variation\n' );
  fprintf ( 1, '  that is captured by the truncated expansion.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      RHOBAR      VARSUM\n' );
  fprintf ( 1, '\n' );
  rhobar = 4.0;
  for i = 1 : 10
    F = fred ( @constant_correlation, domain ( [ a, b ] ) );
    lambda_vec = eigs ( F, 20, 'lm' );
    varsum = sum ( lambda_vec(1:eigen_use) ) / ( b - a );
    fprintf ( 1, '  %10g  %10g\n', rhobar, varsum );
    rhobar = rhobar / 2.0;
  end

  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CONSTANT_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  rmpath ( '../chebfun' )

  return
end


function [CMerged,omega,xMerged]=covarianceIntersect(C1,C2,optCrit,x1,x2)
%%COVARIANCEINTERSECT Perform covariance intersection. This is a method of
%                     fusing the first two moments of estimates when the
%                     correlation between the estimates is unknown. Given
%                     the covariance matrices of the estimates, this
%                     function returns a covariance matrix and a scaling
%                     factor, which can be used to fuse estimates.
%                     Alternatively, if the estimates themselves are given,
%                     this function can also return the merged estimate.
%                     Compare this function to ellipsoidIntersect.
%
%INPUTS: C1 The nXn positive definite covariance matrix of the first
%           estimate.
%        C2 The nXn positive definite covariance matrix of the second
%           estimate.
%   optCrit An optional  parameter specifying the optimization criterion
%           for the fusion. Possible values are
%           'det' (The default if omitted or an empty matrix is passed) The
%                 fusion is performed to optimize the determinant of the
%                 matrix on the output.
%           'trace' The fusion is performed to minimize the trace of the
%                 output matrix.
%    x1,x2 The optional nX1 estimate vectors to be merged. These are only
%          used if xMerged is requested on the output.
%
%OUTPUTS: CMerged The nXn fused covariance matrix.
%           omega The weighting that played a role in the fusion of the
%                 covariance matrix/ state estimates. Specifically,
%                 CMerged=inv(omega*inv(C1)+(1-omega)*inv(C2))
%                 and the merged estimates are
%                 xMerged=CMerged*(omega*inv(C1)*x1+(1-omega)*inv(C2)*x2)
%         xMerged The merged estimate. This requires x1,x2 to be given on
%                 the input. The covariance matrix associated with the
%                 merged estimate is CMerged.
%
%The first (non-dissertation) publication of covariance intersection for
%fusing measurements having unknown correlations is [2]. Various optimality
%criteria are derived in [3]. However, the most practical approach is that
%of [1], which clearly expresses the solution in terms of the solution of
%polynomials, with some solutions given in closed form. Thus, this function
%uses the closed form solutions, when available, and the general polynomial
%solutions otherwise.
%
%Note that covariance intersection is overly convervative in its covariance
%estimates as mentioned in Chapter 9.3.7 of [5].
%
%EXAMPLE:
%Here we use the numerical values form the example for ellipsoidal
%intersection in [1]. The plots in the paper appear to be incorrect.
% xi=[1;-2];
% Pi=[3,0;0,0.4];
% xj=[-2;-1];
% Pj=[2,-0.8;-0.8,1];
%
% [P,~,x]=covarianceIntersect(Pi,Pj,'det',xi,xj);
% figure()
% clf
% hold on
% drawEllipse(xi,inv(Pi),[],'--r')
% drawEllipse(xj,inv(Pj),[],'--g')
% drawEllipse(x,inv(P),[],'-b')
%
%REFERENCES:
%[1] M. Reinhardt, B. Noack, and U. D. Hanebeck, "Closed-form optimization
%    of covariance intersection for low-dimensional matrices," in
%    Proceedings of the 15th International Conference on Information
%    Fusion, Singapore, 9-12 Jun. 2012, pp. 1891-1896.
%[2] S. J. Julier and J. K. Uhlmann, "A non-divergent estimation algorithm
%    in the presence of unknown correlations," in Proceedings of the
%    American Control Conference, vol. 4, Albuquerque, NM, 4-6 Jun. 1997,
%    pp. 2369-2373.
%[3] L. Chen, P. O. Arambel, and R. K. Mehra, "Estimation under unknown
%    correlation: Covariance intersection revisited," IEEE Transactions on
%    Automatic Control, vol. 47, no. 11, pp. 1879-1882, Nov. 2002.
%[4] J. Sijs, M. Lazar, and P. Bosch, "State fusion with unknown
%    correlation: Ellipsoidal intersection," in Proceedings of the 2010
%    American Control Conference, Baltimore, MD, 30 Jun. - 2 Jul. 2010.
%[5] Y. Bar-Shalom, P. K. Willett, and X. Tian, Tracking and Data Fusion.
%    Storrs, CT: YBS Publishing, 2011.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(optCrit))
    optCrit='det';
end

n=length(C1);

%The transformation matrix in Equation 2 of [1], but here we use an
%SVD-based algorithm instead of an eigenvalue-based algorithm, because it
%is numerically more stable.
A=twoMatDiag(C1,C2);

%The positive scalars d.
d=diag(A*C2*A');
dBar=1./d;
dTilde=dBar./(1-dBar);

invC1=inv(C1);
invC2=inv(C2);

%Determinant minimization
switch(optCrit)
    case 'det'
        if(n==1)
            if(C1<C2)
                omega=1;
            else
                omega=0;
            end
        elseif(n==2)
            %The solution is Equation 11 in [1].
            omega=-(1/2)*(dTilde(1)+dTilde(2));
        elseif(n==3)
            sqrtTerm=sqrt(sum(dTilde.^2)-dTilde(1)*dTilde(2)-dTilde(1)*dTilde(3)-dTilde(2)*dTilde(3));
            %Equation 12 in [1].
            omega(1,1)=-(1/3)*(sum(dTilde)+sqrtTerm);
            omega(2,1)=-(1/3)*(sum(dTilde)-sqrtTerm);
        elseif(n==4)
            b=(3/4)*sum(dTilde);

            c=0;
            for i=1:n
               for j=(i+1):n
                   c=c+dTilde(i)*dTilde(j);
               end
            end
            c=(1/2)*c;

            d=0;
            for i=1:n
                for j=(i+1):n
                    for k=(j+1):n
                        d=d+dTilde(i)*dTilde(j)*dTilde(k);
                    end
                end
            end
            d=(1/4)*d;

            Q=sqrt((2*b^3-9*b*c+27*d)^2-4*(b^2-3*c)^3);
            C=((1/2)*(Q+2*b^3-9*b*c+27*d))^(1/3);

            %Equation 20
            omega(1,1)=-(b/3)-C/3-(b^2-3*c)/(3*C);
            omega(2,1)=-(b/3)+C*(1+1j*sqrt(3))/6+(1-1j*sqrt(3))*(b^2-3*c)/(6*C);
            omega(3,1)=-(b/3)+C*(1-1j*sqrt(3))/6+(1+1j*sqrt(3))*(b^2-3*c)/(6*C);
        else%For n>4
            %The pi values are given in the unnumbered equation after
            %Equation 10 in [1].
            piVals=zeros(1,n-1);
            for curPi=1:(n-1)
                piIdx=1:curPi;
                while(1)
                    %Add the current  product
                    piVals(curPi)=piVals(curPi)+prod(dTilde(piIdx));

                    %Increment the value in the innermost sum.
                    curLevel=curPi;
                    allDone=false;
                    while(piIdx(curLevel)+1>(n-curPi+curLevel))
                        curLevel=curLevel-1;
                        if(curLevel<1)
                            allDone=true;
                            break;
                        end
                    end
                    if(allDone)
                        break;
                    end
                    %Increment the index
                    piIdx(curLevel)=piIdx(curLevel)+1;

                    %Fill in the minimum values in further nested sums.
                    for k=(curLevel+1):curPi
                        piIdx(k)=piIdx(k-1)+1;
                    end
                end
            end

            %Given the pi values, build the polynomial in omega.
            poly2Solve=(n:-1:1).*[1,piVals];

            %Omega hypotheses are the roots of an order n-1 polynomial in
            %Equation 10 of [1].
            omega=roots(poly2Solve);
        end

        if(n>1)
        %Given a set of omega values, we have to choose the best solution.
        %We also have to include omega=0 and omega=1 as possibilities as
        %per Algorithm 1 in [1]. Some of the solutions might be slightly
        %imaginary due to finite precision errors. We will just discard the
        %imaginary parts. This does not affect the choice of the optimal
        %solution.
            omega=[real(omega);0;1];
            numHyp=length(omega);

            minCost=Inf;
            minOmega=[];
            for curHyp=1:numHyp
                if(omega(curHyp)>=0&&omega(curHyp)<=1)
                    %The determinant of a matrix inverse is the inverse of
                    %the determinant.
                    cost=1/det(omega(curHyp)*invC1+(1-omega(curHyp))*invC2);

                    if(cost<minCost)
                       minCost=cost;
                       minOmega=omega(curHyp);
                    end
                end
            end

            omega=minOmega;
        end
    case 'trace'
        if(n==1)
            %The solution is the same as for the determinant minimization
            %criterion.
            if(C1<C2)
                omega=1;
            else
                omega=0;
            end
        elseif(n==2)
            invA=inv(A);
            a=diag(invA'*invA);

            denom=a(1)*(1+dTilde(1))+a(2)*(1+dTilde(2));
            p=(a(1)*dTilde(2)*(1+dTilde(1))+a(2)*dTilde(1)*(1+dTilde(2)))/denom;
            q=(a(1)*dTilde(2)^2*(1+dTilde(1))+a(2)*dTilde(1)^2*(1+dTilde(2)))/denom;

            %Equation 19
            omega(1,1)=-p+sqrt(p^2-q);
            omega(2,1)=-p-sqrt(p^2-q);
        else%for n>=3
            %Use convolutions to build up the polynomial in Equation 17 in
            %[1].
            invA=inv(A);
            a=diag(invA'*invA);

            %The polynomial is degree 2*(n-1).
            numPolyEls=2*(n-1)+1;
            poly2Solve=zeros(numPolyEls,1);
            for i=1:n
                coeff=a(i)*(1+dTilde(i));

                jPoly=zeros(numPolyEls,1);
                jPoly(end)=1;
                for j=1:n
                    if(j==i)
                        continue;
                    end

                    jPoly=conv(jPoly,[1;2*dTilde(j);dTilde(j)^2]);
                    %Get rid of zero-padding at the beginning.
                    jPoly=jPoly((end-numPolyEls+1):end);
                end
                poly2Solve=poly2Solve+coeff*jPoly;
            end

            %omega hypotheses are the roots of the equation.
            omega=roots(poly2Solve);
        end

        %Given a set of omega values, we have to choose the best solution.
        %We also have to include omega=0 and omega=1 as possibilities as
        %per Algorithm 1 in [1]. Some of the solutions might be slightly
        %imaginary due to finite precision errors. We will just discard the
        %imaginary parts. This does not affect the choice of the optimal
        %solution.
        if(n>1)
            omega=[real(omega);0;1];
            numHyp=length(omega);

            minCost=Inf;
            minOmega=[];
            for curHyp=1:numHyp
                if(omega(curHyp)>=0&&omega(curHyp)<=1)
                    cost=trace(inv(omega(curHyp)*invC1+(1-omega(curHyp))*invC2));

                    if(cost<minCost)
                       minCost=cost;
                       minOmega=omega(curHyp);
                    end
                end
            end

            omega=minOmega;
        end
    otherwise
        error('Invalid optimality criterion chosen.')
end

CMergedInv=omega*invC1+(1-omega)*invC2;
CMerged=inv(CMergedInv);

if(nargout>2)
    xMerged=CMergedInv\(omega*invC1*x1+(1-omega)*invC2*x2);
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 H = RS(sequence,isplot)
%
% 'RS' estimate the hurst parameter of a given sequence with R/S method.
%
% Inputs:
%     sequence: the input sequence for estimate
%     isplot: whether display the plot. without a plot if isplot equal to 0
% Outputs:
%     H: the estimated hurst coeffeient of the input sequence

%  Author: Chu Chen
%  Version 1.0,  03/10/2008
%  chen-chu@163.com
%

if nargin == 1
    isplot = 0;
end

N = length(sequence);
dlarge = floor(N/5);
dsmall = max(10,log10(N)^2);
D = floor(logspace(log10(dsmall),log10(dlarge),50));
D = unique(D);
n = length(D);
x = zeros(1,n);
y = zeros(1,n);

R = cell(1,n);
S = cell(1,n);
for i = 1:n
    d = D(i);
    m = floor(N/d);
    R{i} = zeros(1,m);
    S{i} = zeros(1,m);
    matrix_sequence = reshape(sequence(1:d*m),d,m);

    Z1 = cumsum(matrix_sequence);
    Z2 = cumsum(repmat(mean(matrix_sequence),d,1));
    R{i} = (max(Z1-Z2)-min(Z1-Z2));
    S{i} = std(matrix_sequence);

    if min(R{i})==0 || min(S{i}) ==0
        continue;
    end

    x(i) = log10(d);
    y(i) = mean(log10(R{i}./S{i}));
end

% fit a line with middle part of sequence
index = x~=0;
x = x(index);
y = y(index);
n2 = length(x);
cut_min = ceil(3*n2/10);
cut_max = floor(9*n2/10);

X = x(cut_min:cut_max);
Y = y(cut_min:cut_max);
p1 = polyfit(X,Y,1);
Yfit = polyval(p1,X);
H = (Yfit(end)-Yfit(1))/(X(end)-X(1));

if isplot ~= 0
    figure,hold on;
    bound = ceil(log10(N));
    axis([0 bound 0 0.75*bound]);

    temp = (1:n).*index;
    index = temp(index);
    for i = 1:n2
        plot(x(i),log10(R{index(i)}./S{index(i)}),'b.');
    end

    x = linspace(0,bound,10);
    y1 = 0.5*x;
    y2 = x;
    h1 = plot(x,y1,'b--','LineWidth',2);
    h2 = plot(x,y2,'b-.','LineWidth',2);
    plot(X,Yfit,'r-','LineWidth',3);
    legend([h1,h2],'slope 1/2','slope 1',4)
    xlabel('log10(blocks of size m)'),ylabel('log10(R/S)'),title('R/S Method');
end


function prob_test155 ( )

%*****************************************************************************80
%
%% TEST155 tests VON_MISES_MEAN, VON_MISES_SAMPLE, VON_MISES_CIRCULAR_VARIANCE;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST155\n' );
  fprintf ( 1, '  For the Von Mises PDF:\n' );
  fprintf ( 1, '  VON_MISES_MEAN computes the mean;\n' );
  fprintf ( 1, '  VON_MISES_SAMPLE samples.\n' );
  fprintf ( 1, ...
    '  VON_MISES_CIRCULAR_VARIANCE computes the circular variance;\n' );

  a = 1.0;
  b = 2.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A =             %14f\n', a );
  fprintf ( 1, '  PDF parameter B =             %14f\n', b );

  check = von_mises_check ( a, b );

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

  mean = von_mises_mean ( a, b );
  variance = von_mises_circular_variance ( a, b );

  fprintf ( 1, '  PDF mean =              %14f\n', mean );
  fprintf ( 1, '  PDF circular variance = %14f\n', variance );

  for i = 1 : nsample
    [ x(i), seed ] = von_mises_sample ( a, b, seed );
  end

  mean = r8vec_mean ( nsample, x );
  variance = r8vec_circular_variance ( nsample, x );
  xmax = max ( x(1:nsample) );
  xmin = min ( x(1:nsample) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =              %6d\n', nsample );
  fprintf ( 1, '  Sample mean =              %14f\n', mean );
  fprintf ( 1, '  Sample circular variance = %14f\n', variance );
  fprintf ( 1, '  Sample maximum =           %14f\n', xmax );
  fprintf ( 1, '  Sample minimum =           %14f\n', xmin );

  return
end


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

%% Apply defaults

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

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

%% Form state space model

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

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

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

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

  % Observation model
  H = [1, 0, 0];

%% Stationary covariance

  % Calculate Pinf only if requested
  if nargout > 4,

    % Derived constant
    kappa = 5/3*magnSigma2/lengthScale^2;

    % Stationary covariance
    Pinf = [magnSigma2, 0,      -kappa;
            0,          kappa,  0;
            -kappa,     0,      25*magnSigma2/lengthScale^4];

  end

%% Calculate derivatives

  % Calculate derivatives only if requested
  if nargout > 5

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

    % Derivative of F w.r.t parameter lengthScale
    dFlengthScale   =  [0,                          0,                  0;
                        0,                          0,                  0;
                        15*sqrt(5)/lengthScale^4,    30/lengthScale^3,   3*sqrt(5)/lengthScale^2];

    % Derivative of Qc w.r.t. parameter magnSigma2
    dQcmagnSigma2   =   400*sqrt(5)/3/lengthScale^5;

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

    % Derivative of Pinf w.r.t. parameter magnSigma2
    dPinfmagnSigma2 = Pinf/magnSigma2;

    % Derivative of Pinf w.r.t. parameter lengthScale
    kappa2 = -2*kappa/lengthScale;
    dPinflengthScale = [0,          0,       -kappa2;
                        0,          kappa2,  0;
                        -kappa2,    0,       -100*magnSigma2/lengthScale^5];

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

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

  end

%% Return parameter names

  % Only return if requested
  if nargout > 8

    % Stationarity
    p.stationary = true;

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

    % Return parameter setup
    params = p;

  end


function [ic,ik] = bspdegelev(d,c,k,t)
%
% Function Name:
%
%   bspdegevel - Degree elevate a univariate B-Spline.
%
% Calling Sequence:
%
%   [ic,ik] = bspdegelev(d,c,k,t)
%
% Parameters:
%
%   d	: Degree of the B-Spline.
%
%   c	: Control points, matrix of size (dim,nc).
%
%   k	: Knot sequence, row vector of size nk.
%
%   t	: Raise the B-Spline degree t times.
%
%   ic	: Control points of the new B-Spline.
%
%   ik	: Knot vector of the new B-Spline.
%
% Description:
%
%   Degree elevate a univariate B-Spline. This function provides an
%   interface to a toolbox 'C' routine.

[mc,nc] = size(c);
                                                          %
                                                          % int bspdegelev(int d, double *c, int mc, int nc, double *k, int nk,
                                                          %                int t, int *nh, double *ic, double *ik)
                                                          % {
                                                          %   int row,col;
                                                          %
                                                          %   int ierr = 0;
                                                          %   int i, j, q, s, m, ph, ph2, mpi, mh, r, a, b, cind, oldr, mul;
                                                          %   int n, lbz, rbz, save, tr, kj, first, kind, last, bet, ii;
                                                          %   double inv, ua, ub, numer, den, alf, gam;
                                                          %   double **bezalfs, **bpts, **ebpts, **Nextbpts, *alfs;
                                                          %
                                                          %   double **ctrl  = vec2mat(c, mc, nc);
% ic = zeros(mc,nc*(t));                                  %   double **ictrl = vec2mat(ic, mc, nc*(t+1));
                                                          %
n = nc - 1;                                               %   n = nc - 1;
                                                          %
bezalfs =  zeros(d+1,d+t+1);                              %   bezalfs = matrix(d+1,d+t+1);
bpts = zeros(mc,d+1);                                     %   bpts = matrix(mc,d+1);
ebpts = zeros(mc,d+t+1);                                  %   ebpts = matrix(mc,d+t+1);
Nextbpts = zeros(mc,d+1);                                 %   Nextbpts = matrix(mc,d+1);
alfs = zeros(d,1);                                        %   alfs = (double *) mxMalloc(d*sizeof(double));
                                                          %
m = n + d + 1;                                            %   m = n + d + 1;
ph = d + t;                                               %   ph = d + t;
ph2 = floor(ph / 2);                                      %   ph2 = ph / 2;
                                                          %
                                                          %   // compute bezier degree elevation coefficeients
bezalfs(1,1) = 1;                                         %   bezalfs[0][0] = bezalfs[ph][d] = 1.0;
bezalfs(d+1,ph+1) = 1;                                    %

for i=1:ph2                                               %   for (i = 1; i <= ph2; i++) {
   inv = 1/bincoeff(ph,i);                                %     inv = 1.0 / bincoeff(ph,i);
   mpi = min(d,i);                                        %     mpi = min(d,i);
                                                          %
   for j=max(0,i-t):mpi                                   %     for (j = max(0,i-t); j <= mpi; j++)
       bezalfs(j+1,i+1) = inv*bincoeff(d,j)*bincoeff(t,i-j);  %       bezalfs[i][j] = inv * bincoeff(d,j) * bincoeff(t,i-j);
   end
end                                                       %   }
                                                          %
for i=ph2+1:ph-1                                          %   for (i = ph2+1; i <= ph-1; i++) {
   mpi = min(d,i);                                        %     mpi = min(d, i);
   for j=max(0,i-t):mpi                                   %     for (j = max(0,i-t); j <= mpi; j++)
       bezalfs(j+1,i+1) = bezalfs(d-j+1,ph-i+1);          %       bezalfs[i][j] = bezalfs[ph-i][d-j];
   end
end                                                       %   }
                                                          %
mh = ph;                                                  %   mh = ph;
kind = ph+1;                                              %   kind = ph+1;
r = -1;                                                   %   r = -1;
a = d;                                                    %   a = d;
b = d+1;                                                  %   b = d+1;
cind = 1;                                                 %   cind = 1;
ua = k(1);                                                %   ua = k[0];
                                                          %
for ii=0:mc-1                                             %   for (ii = 0; ii < mc; ii++)
   ic(ii+1,1) = c(ii+1,1);                                %     ictrl[0][ii] = ctrl[0][ii];
end                                                       %
for i=0:ph                                                %   for (i = 0; i <= ph; i++)
   ik(i+1) = ua;                                          %     ik[i] = ua;
end                                                       %
                                                          %   // initialise first bezier seg
for i=0:d                                                 %   for (i = 0; i <= d; i++)
   for ii=0:mc-1                                          %     for (ii = 0; ii < mc; ii++)
      bpts(ii+1,i+1) = c(ii+1,i+1);                       %       bpts[i][ii] = ctrl[i][ii];
   end
end                                                       %
                                                          %   // big loop thru knot vector
while b < m                                               %   while (b < m)  {
   i = b;                                                 %     i = b;
   while b < m && k(b+1) == k(b+2)                        %     while (b < m && k[b] == k[b+1])
      b = b + 1;                                          %       b++;
   end                                                    %
   mul = b - i + 1;                                       %     mul = b - i + 1;
   mh = mh + mul + t;                                     %     mh += mul + t;
   ub = k(b+1);                                           %     ub = k[b];
   oldr = r;                                              %     oldr = r;
   r = d - mul;                                           %     r = d - mul;
                                                          %
                                                          %     // insert knot u(b) r times
   if oldr > 0                                            %     if (oldr > 0)
      lbz = floor((oldr+2)/2);                            %       lbz = (oldr+2) / 2;
   else                                                   %     else
      lbz = 1;                                            %       lbz = 1;
   end                                                    %

   if r > 0                                               %     if (r > 0)
      rbz = ph - floor((r+1)/2);                          %       rbz = ph - (r+1)/2;
   else                                                   %     else
      rbz = ph;                                           %       rbz = ph;
   end                                                    %

   if r > 0                                               %     if (r > 0) {
                                                          %       // insert knot to get bezier segment
      numer = ub - ua;                                    %       numer = ub - ua;
      for q=d:-1:mul+1                                    %       for (q = d; q > mul; q--)
         alfs(q-mul) = numer / (k(a+q+1)-ua);             %         alfs[q-mul-1] = numer / (k[a+q]-ua);
      end

      for j=1:r                                           %       for (j = 1; j <= r; j++)  {
         save = r - j;                                    %         save = r - j;
         s = mul + j;                                     %         s = mul + j;
                                                          %
         for q=d:-1:s                                     %         for (q = d; q >= s; q--)
            for ii=0:mc-1                                 %           for (ii = 0; ii < mc; ii++)
               tmp1 = alfs(q-s+1)*bpts(ii+1,q+1);
               tmp2 = (1-alfs(q-s+1))*bpts(ii+1,q);
               bpts(ii+1,q+1) = tmp1 + tmp2;              %             bpts[q][ii] = alfs[q-s]*bpts[q][ii]+(1.0-alfs[q-s])*bpts[q-1][ii];
            end
         end                                              %

         for ii=0:mc-1                                    %         for (ii = 0; ii < mc; ii++)
            Nextbpts(ii+1,save+1) = bpts(ii+1,d+1);       %           Nextbpts[save][ii] = bpts[d][ii];
         end
      end                                                 %       }
   end                                                    %     }
                                                          %     // end of insert knot
                                                          %
                                                          %     // degree elevate bezier
   for i=lbz:ph                                           %     for (i = lbz; i <= ph; i++)  {
      for ii=0:mc-1                                       %       for (ii = 0; ii < mc; ii++)
         ebpts(ii+1,i+1) = 0;                             %         ebpts[i][ii] = 0.0;
      end
      mpi = min(d, i);                                    %       mpi = min(d, i);
      for j=max(0,i-t):mpi                                %       for (j = max(0,i-t); j <= mpi; j++)
         for ii=0:mc-1                                    %         for (ii = 0; ii < mc; ii++)
            tmp1 = ebpts(ii+1,i+1);
            tmp2 = bezalfs(j+1,i+1)*bpts(ii+1,j+1);
            ebpts(ii+1,i+1) = tmp1 + tmp2;                %           ebpts[i][ii] = ebpts[i][ii] + bezalfs[i][j]*bpts[j][ii];
         end
      end
   end                                                    %     }
                                                          %     // end of degree elevating bezier
                                                          %
   if oldr > 1                                            %     if (oldr > 1)  {
                                                          %       // must remove knot u=k[a] oldr times
      first = kind - 2;                                                    %       first = kind - 2;
      last = kind;                                        %       last = kind;
      den = ub - ua;                                      %       den = ub - ua;
      bet = floor((ub-ik(kind)) / den);                   %       bet = (ub-ik[kind-1]) / den;
                                                          %
                                                          %       // knot removal loop
      for tr=1:oldr-1                                     %       for (tr = 1; tr < oldr; tr++)  {
         i = first;                                       %         i = first;
         j = last;                                        %         j = last;
         kj = j - kind + 1;                               %         kj = j - kind + 1;
         while j-i > tr                                   %         while (j - i > tr)  {
                                                          %           // loop and compute the new control points
                                                          %           // for one removal step
            if i < cind                                   %           if (i < cind)  {
               alf = (ub-ik(i+1))/(ua-ik(i+1));           %             alf = (ub-ik[i])/(ua-ik[i]);
               for ii=0:mc-1                              %             for (ii = 0; ii < mc; ii++)
                  tmp1 = alf*ic(ii+1,i+1);
                  tmp2 = (1-alf)*ic(ii+1,i);
                  ic(ii+1,i+1) = tmp1 + tmp2;             %               ictrl[i][ii] = alf * ictrl[i][ii] + (1.0-alf) * ictrl[i-1][ii];
               end
            end                                           %           }
            if j >= lbz                                   %           if (j >= lbz)  {
               if j-tr <= kind-ph+oldr                    %             if (j-tr <= kind-ph+oldr) {
                  gam = (ub-ik(j-tr+1)) / den;            %               gam = (ub-ik[j-tr]) / den;
                  for ii=0:mc-1                           %               for (ii = 0; ii < mc; ii++)
                     tmp1 = gam*ebpts(ii+1,kj+1);
                     tmp2 = (1-gam)*ebpts(ii+1,kj+2);
                     ebpts(ii+1,kj+1) = tmp1 + tmp2;      %                 ebpts[kj][ii] = gam*ebpts[kj][ii] + (1.0-gam)*ebpts[kj+1][ii];
                  end                                     %             }
               else                                       %             else  {
                  for ii=0:mc-1                           %               for (ii = 0; ii < mc; ii++)
                     tmp1 = bet*ebpts(ii+1,kj+1);
                     tmp2 = (1-bet)*ebpts(ii+1,kj+2);
                     ebpts(ii+1,kj+1) = tmp1 + tmp2;      %                 ebpts[kj][ii] = bet*ebpts[kj][ii] + (1.0-bet)*ebpts[kj+1][ii];
                  end
               end                                        %             }
            end                                           %           }
            i = i + 1;                                    %           i++;
            j = j - 1;                                    %           j--;
            kj = kj - 1;                                  %           kj--;
         end                                              %         }
                                                          %
         first = first - 1;                               %         first--;
         last = last + 1;                                 %         last++;
      end                                                 %       }
   end                                                    %     }
                                                          %     // end of removing knot n=k[a]
                                                          %
                                                          %     // load the knot ua
   if a ~= d                                              %     if (a != d)
      for i=0:ph-oldr-1                                   %       for (i = 0; i < ph-oldr; i++)  {
         ik(kind+1) = ua;                                 %         ik[kind] = ua;
         kind = kind + 1;                                 %         kind++;
      end
   end                                                    %       }
                                                          %
                                                          %     // load ctrl pts into ic
      for j=lbz:rbz                                       %     for (j = lbz; j <= rbz; j++)  {
         for ii=0:mc-1                                    %       for (ii = 0; ii < mc; ii++)
            ic(ii+1,cind+1) = ebpts(ii+1,j+1);            %         ictrl[cind][ii] = ebpts[j][ii];
         end
         cind = cind + 1;                                 %       cind++;
      end                                                 %     }
                                                          %
      if b < m                                            %     if (b < m)  {
                                                          %       // setup for next pass thru loop
         for j=0:r-1                                      %       for (j = 0; j < r; j++)
            for ii=0:mc-1                                 %         for (ii = 0; ii < mc; ii++)
               bpts(ii+1,j+1) = Nextbpts(ii+1,j+1);       %           bpts[j][ii] = Nextbpts[j][ii];
            end
         end
         for j=r:d                                        %       for (j = r; j <= d; j++)
            for ii=0:mc-1                                 %         for (ii = 0; ii < mc; ii++)
               bpts(ii+1,j+1) = c(ii+1,b-d+j+1);          %           bpts[j][ii] = ctrl[b-d+j][ii];
            end
         end
         a = b;                                           %       a = b;
         b = b+1;                                         %       b++;
         ua = ub;                                         %       ua = ub;
                                                          %     }
      else                                                %     else
                                                          %       // end knot
         for i=0:ph                                       %       for (i = 0; i <= ph; i++)
            ik(kind+i+1) = ub;                            %         ik[kind+i] = ub;
         end
      end
end                                                       %   }
% End big while loop                                      %   // end while loop
                                                          %
                                                          %   *nh = mh - ph - 1;
                                                          %
                                                          %   freevec2mat(ctrl);
                                                          %   freevec2mat(ictrl);
                                                          %   freematrix(bezalfs);
                                                          %   freematrix(bpts);
                                                          %   freematrix(ebpts);
                                                          %   freematrix(Nextbpts);
                                                          %   mxFree(alfs);
                                                          %
                                                          %   return(ierr);
                                                          % }

function b = bincoeff(n,k)
%  Computes the binomial coefficient.
%
%      ( n )      n!
%      (   ) = --------
%      ( k )   k!(n-k)!
%
%  b = bincoeff(n,k)
%
%  Algorithm from 'Numerical Recipes in C, 2nd Edition' pg215.

                                                          % double bincoeff(int n, int k)
                                                          % {
b = floor(0.5+exp(factln(n)-factln(k)-factln(n-k)));      %   return floor(0.5+exp(factln(n)-factln(k)-factln(n-k)));
                                                          % }

function f = factln(n)
% computes ln(n!)
if n <= 1, f = 0; return, end
f = gammaln(n+1); %log(factorial(n));</pre>


function square_symq_rule_test04 ( degree, n )

%*****************************************************************************80
%
%% SQUARE_SYMQ_RULE_TEST04 gets a rule and tests its accuracy.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    01 July 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, integer DEGREE, the desired total polynomial degree exactness
%    of the quadrature rule.  0 <= DEGREE <= 50.
%
%    Input, integer N, the number of nodes to be used by the rule.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SQUARE_SYMQ_RULE_TEST04\n' );
  fprintf ( 1, '  Get a quadrature rule for the symmetric square.\n' );
  fprintf ( 1, '  Test its accuracy.\n' );
  fprintf ( 1, '  Polynomial exactness degree DEGREE = %d\n', degree );
%
%  Retrieve a symmetric quadrature rule.
%
  [ x, w ] = square_symq ( degree, n );

  npols = ( ( degree + 1 ) * ( degree + 2 ) ) / 2;

  rints = zeros(npols,1);

  for i = 1 : n

    z(1) = x(1,i);
    z(2) = x(2,i);

    pols = lege2eva ( degree, z );

    rints(1:npols) = rints(1:npols) + w(i) * pols(1:npols);

  end

  area = 4.0;

  d = 0.0;
  d = ( rints(1) - sqrt ( area ) )^2;
  for i = 2 : npols
    d = d + rints(i)^2;
  end
  d = sqrt ( d ) / npols;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  RMS error = %g\n', d );

  return
end


%% LP1
clc
%Objective & Constraints
f = -[6 5]';
A = ([1,4; 6,4; 2, -5]);
b = [16;28;6];
lb = [0;0]; ub = [10;10];
%Setup Options
opts = optiset('solver','clp');
%Build & Solve
Opt = opti('grad',f,'ineq',A,b,'bounds',lb,ub,'options',opts)
[x,fval,exitflag,info] = solve(Opt)
%Plot
plot(Opt)
%Check Solution
[ok,msg] = checkSol(Opt)

%% LP1 ROW
rl = -Inf(3,1);
ru = b;
Opt = opti('grad',f,'lin',A,rl,ru,'bounds',lb,ub,'options',opts)
[x,fval,exitflag,info] = solve(Opt)
%Plot
plot(Opt)
%Check Solution
[ok,msg] = checkSol(Opt)

%% Raw Test
opts.debug = 1;
opts.display = 2;
opts.maxiter = 5;
% rl = zeros(3,1);
n = length(f);
H = spalloc(n,n,0);
Aeq = spalloc(0,n,0);
beq = zeros(0,1);

[x,fv,ef,iter,l] = ooqp([],f,sparse(A),rl,ru,[],[],lb,ub,opts)

%% LP2
clc
%Objective & Constraints
f = -[1 2 3]';
A = [-1,1,1; 1,-3,1];
b = [20,30]';
Aeq = [1 1 1]; beq = 40;
lb = [0 0 0]'; ub = [40 inf inf]';
%Setup Options
opts = optiset('solver','ooqp');
%Build & Solve
Opt = opti('grad',f,'ineq',A,b,'eq',Aeq,beq,'bounds',lb,ub,'options',opts)
[x,fval,exitflag,info] = solve(Opt)

%% LP2 ROW
Ar = [A;Aeq];
rl = [-Inf(2,1); beq];
ru = [b;beq];
Opt = opti('grad',f,'lin',Ar,rl,ru,'bounds',lb,ub,'options',opts)
[x,fval,exitflag,info] = solve(Opt)
%Check Solution
[ok,msg] = checkSol(Opt)

%% LP2 ROW + EQ (OOQP FORMAT) (NOT SUPPORTED CURRENTLY)
% rl = -Inf(2,1);
% ru = b;
% prob = optiprob('grad',f,'lin',A,rl,ru,'eq',Aeq,beq,'bounds',lb,ub);
% Opt = opti(prob,optiset('solver','clp'))
% [x,fval,exitflag,info] = solve(Opt)
% %Check Solution
% [ok,msg] = checkSol(Opt)

%% LP3
clc
%Objective & Constraints
f = [8,1]';
A = [-1,-2;1,-4;3,-1;1,5;-1,1;-1,0;0,-1];
b = [-4,2,21,39,3,0,0]';
%Setup Options
opts = optiset('solver','clp');
%Build & Solve
x0 = [9,6]';
Opt = opti('grad',f,'ineq',A,b,'options',opts)
[x,fval,exitflag,info] = solve(Opt,x0)
plot(Opt,10)

%% LP3 ROW
rl = -Inf(7,1);
ru = b;
Opt = opti('grad',f,'lin',A,rl,ru,'options',opts)
[x,fval,exitflag,info] = solve(Opt)
%Check Solution
[ok,msg] = checkSol(Opt)

%% MILP1
clc
%Objective & Constraints
f = -[6 5]';
A = [1,4; 6,4; 2, -5];
b = [16;28;6];
lb = [0;0]; ub=[10;10];
%Setup Options
opts = optiset('solver','glpk');
%Build & Solve
Opt = opti('grad',f,'ineq',A,b,'bounds',lb,ub,'int','II','options',opts)
[x,fval,exitflag,info] = solve(Opt)
plot(Opt)

%% MILP1 ROW
rl = -Inf(3,1);
ru = b;
Opt = opti('grad',f,'lin',A,rl,ru,'bounds',lb,ub,'int','II','options',opts)
[x,fval,exitflag,info] = solve(Opt)
plot(Opt)
%Check Solution
[ok,msg] = checkSol(Opt)

%% MILP2
clc
%Objective & Constraints
f = -[1 2 3 1]';
A = [-1 1 1 10; 1 -3 1 0];
b = [20;30];
Aeq = [0 1 0 -3.5];
beq = 0;
lb = [0 0 0 2]';
ub = [40 inf inf 3]';
int = 'CCCI';
%Setup Options
opts = optiset('solver','cbc');
%Build & Solve
Opt = opti('grad',f,'ineq',A,b,'eq',Aeq,beq,'bounds',lb,ub,'int',int,'options',opts)
[x,fval,exitflag,info] = solve(Opt)

%% MILP2 ROW
Ar = [A;Aeq];
rl = [-Inf(2,1); beq];
ru = [b;beq];
Opt = opti('grad',f,'lin',Ar,rl,ru,'bounds',lb,ub,'int',int,'options',opts)
[x,fval,exitflag,info] = solve(Opt)
%Check Solution
[ok,msg] = checkSol(Opt)

%% BILP1
clc
%Objective & Constraints
f = -[6 5]';
A = [-3,5; 6,4; 3, -5; -6, -4];
b = [6;9;1;3];
%Setup Options
opts = optiset('solver','cbc');
%Build & Solve
Opt = opti('grad',f,'ineq',A,b,'int','BB','options',opts)
[x,fval,exitflag,info] = solve(Opt)
plot(Opt,3)

%% BILP1 ROW
rl = -Inf(4,1);
ru = b;
Opt = opti('grad',f,'lin',A,rl,ru,'int','BB','options',opts)
[x,fval,exitflag,info] = solve(Opt)
plot(Opt)
%Check Solution
[ok,msg] = checkSol(Opt)

%% BILP2
clc
%Objective & Constraints
f = -[9 5 6 4]';
A = [6 3 5 2; 0 0 1 1; -1 0 1 0; 0 -1 0 1];
b = [9; 1; 0; 0];
%Setup Options
opts = optiset('solver','cbc');
%Build & Solve
Opt = opti('grad',f,'ineq',A,b,'int','BBBB','options',opts)
[x,fval,exitflag,info] = solve(Opt)

%% NLP1 Hock & Schittkowski #71
clc
%Objective & Gradient
obj = @(x) x(1)*x(4)*sum(x(1:3)) + x(3);
grad = @(x) [ x(1)*x(4) + x(4)*sum(x(1:3));
              x(1)*x(4);
              x(1)*x(4) + 1;
              x(1)*sum(x(1:3)) ];
%Linear Constraints
lb = ones(4,1);
ub = 5*ones(4,1);
%Nonlinear Constraints
nlcon = @(x) [ prod(x);
               sum(x.^2)];
nljac = @(x) [ prod(x)./x';
                2*x' ];
nlrhs = [25 40]';
nle = [1 0]'; % (>=, ==)
%Setup Options
opts = optiset('solver','matlab','warnings','on','display','iter');
%Build & Solve
x0 = [1 5 5 1]';
Opt = opti('obj',obj,'grad',grad,'nlmix',nlcon,nlrhs,nle,'nljac',nljac,'bounds',lb,ub,'options',opts)
[x,fval,exitflag,info]= solve(Opt,x0)

%% NLP3 Hock & Schittkowski #51
clc
%Objective & Gradient
obj = @(x) (x(1) - x(2))^2 + (x(2) + x(3) - 2)^2 + (x(4) - 1)^2 + (x(5) - 1)^2;
grad = @(x) 2*[ x(1) - x(2);
                x(2) + x(3) - 2 - x(1) + x(2);
                x(2) + x(3) - 2;
                x(4) - 1;
                x(5) - 1 ];

%Nonlinear Constraints & Jacobian Structure
nlcon = @(x) [ x(1) + 3*x(2);
               x(3) + x(4) - 2*x(5)];
%                x(2) - x(5)];
nljac = @(x) sparse([ 1  3  0  0  0;
	                  0  0  1  1 -2]);
% 	                  0  1  0  0 -1]);
nljacstr = @() sparse([1 1 0 0 0;
                       0 0 1 1 1]);
%                        0 1 0 0 1]);

cl = [4;0];
cu = [4;0];
nlrhs = [4 0 1 15]';
nle = [0 0 1 -1]';

A = [0 1 0 0 -1];
rl = 1;
ru = 15;

%Setup Options
opts = optiset('solver','ipopt','display','iter','maxiter',1e4);
%Build & Solve
x0 = [ 2.5 0.5 2 -1 2.5 ];
Opt = opti('obj',obj,'grad',grad,'lin',A,rl,ru,'nl',nlcon,cl,cu,'nljac',nljac,'nljacstr',nljacstr,'options',opts)
[x,fval,exitflag,info]= solve(Opt,x0)

info.Lambda
info.Lambda.ineqlin
info.Lambda.ineqnonlin
info.Lambda.eqnonlin

%% Test Conversion Function

Opt.prob.nlcon(x0)
full(Opt.prob.nljac(x0))
full(Opt.prob.nljacstr())

%% SINGLE BOUNDS NO JAC
clc
nlcon = @(x) [ x(1) + 3*x(2);
               x(3) + x(4) - 2*x(5);
               x(2) - x(5)];

cl = [-Inf 1 2];
cu = [4 1 Inf];

x0 = [ 2.5 0.5 2 -1 1.5 ];

prob = optiprob('nl',nlcon,cl,cu,'x0',x0)
prob = nrow2mix(prob)

prob.nlrhs
prob.nle
prob.nlcon(x0)

%% SINGLE BOUNDS W JAC
clc
nlcon = @(x) [ x(1) + 3*x(2);
               x(3) + x(4) - 2*x(5);
               x(2) - x(5)];

nljac = @(x) sparse([ 1 3 0 0 0;
                      0 0 1 1 -2;
                      0 1 0 0 -1]);

cl = [-Inf 1 2];
cu = [4 1 Inf];

x0 = [ 2.5 0.5 2 -1 1.5 ];

prob = optiprob('nl',nlcon,cl,cu,'nljac',nljac,'x0',x0)
prob = nrow2mix(prob)

prob.nlrhs
prob.nle
prob.nlcon(x0)

%% DUAL BOUNDS W JAC
clc
nlcon = @(x) [ x(1) + 3*x(2);
               x(3) + x(4) - 2*x(5);
               x(2) - x(5);
               x(1) + 3*x(2);
               x(3) + x(4) - 5*x(5)];

nljac = @(x) sparse([ 1 3 0 0 0;
                      0 0 1 1 -2;
                      0 1 0 0 -1;
                      1 3 0 0 0;
                      0 0 1 1 -5;]);

cl = [-Inf 1 -Inf 2 5];
cu = [4 1 12 16 Inf];

x0 = [ 2.5 0.5 2 -1 1.5 ];

prob = optiprob('nl',nlcon,cl,cu,'nljac',nljac,'x0',x0)
prob = nrow2mix(prob)

prob.nlrhs
prob.nle
prob.nlcon(x0)
full(prob.nljac(x0))

%% DUAL BOUNDS W JAC & STR
clc
nlcon = @(x) [ x(1) + 3*x(2);
               x(3) + x(4) - 2*x(5);
               x(2) - x(5);
               x(1) + 3*x(2);
               x(3) + x(4) - 5*x(5)];

nljac = @(x) sparse([ 1 3 0 0 0;
                      0 0 1 1 -2;
                      0 1 0 0 -1;
                      1 3 0 0 0;
                      0 0 1 1 -5;]);

nljacstr = @() sparse([ 1 1 0 0 0;
                        0 0 1 1 1;
                        0 1 0 0 1;
                        1 1 0 0 0;
                        0 0 1 1 1]);

cl = [-Inf 1 -Inf 2 5];
cu = [4 1 12 16 Inf];

x0 = [ 2.5 0.5 2 -1 1.5 ];

prob = optiprob('nl',nlcon,cl,cu,'nljac',nljac,'nljacstr',nljacstr,'x0',x0)
prob = nrow2mix(prob)

prob.nlrhs
prob.nle
prob.nlcon(x0)
full(prob.nljac(x0))
full(prob.nljacstr())

%% Ali Constraint Linearity Index Problem
clc
obj=@(x) (x)^2;
A=1; ru=1; rl=-1; xtype='I'; x0=1;
opt=opti('obj',obj,'xtype',xtype,'x0',x0,'lin',A,rl,ru)
opt.prob.sizes
solve(opt,x0)

%% Identical form of above
clc
obj=@(x) (x)^2;
A=[1;-1];  b=[1;1];  xtype='I'; x0=1;
opt=opti('obj',obj,'xtype',xtype,'x0',x0,'ineq',A,b);
opt.prob.sizes
solve(opt,x0)

%% Nonlinear Constraints Double Sided Row, Linear
clc
%Objective
obj = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
% Constraints
A = [-1 1; 1 1];
rl = [-3;5];
ru = [-1;5];
lb = [0;0]; ub = [4;4];
% Solve
x0 = [2;2];
Opt = opti('obj',obj,'ndec',2,'bounds',lb,ub,'ivars',1,'lin',A,rl,ru)
[x,fval,ef,info] = solve(Opt,x0)
%Plot
plot(Opt,[],1)

%% Nonlinear Constraints Double Sided Ineq, Linear
clc
%Objective
obj = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
% Constraints
A = [-1 1; 1 -1];
b = [-1;3];
Aeq = [1 1];
beq = 5;
lb = [0;0]; ub = [4;4];
% Solve
x0 = [2;2];
Opt = opti('obj',obj,'ndec',2,'bounds',lb,ub,'ivars',1,'ineq',A,b,'eq',Aeq,beq)
[x,fval,ef,info] = solve(Opt,x0)
%Plot
plot(Opt,[],1)

%% Nonlinear Constraints Double Sided Row, NonLinear
clc
%Objective
obj = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
% Constraints
nlcon = @(x) [-x(1) + x(2); x(1) + x(2)];
cl = [-3;5];
cu = [-1;5];
lb = [0;0]; ub = [4;4];
% Solve
x0 = [2;2];
Opt = opti('obj',obj,'ndec',2,'bounds',lb,ub,'ivars',1,'nl',nlcon,cl,cu)
[x,fval,ef,info] = solve(Opt,x0)
%Plot
plot(Opt,[],1)

%% Nonlinear Constraints Double Sided Ineq, Nonlinear
clc
%Objective
obj = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
% Constraints
nlcon = @(x) [-x(1) + x(2); x(1) - x(2); x(1) + x(2)];
nlrhs = [-1;3;5];
nle = [-1;-1;0];
lb = [0;0]; ub = [4;4];
% Solve
x0 = [2;2];
Opt = opti('obj',obj,'ndec',2,'bounds',lb,ub,'ivars',1,'nlmix',nlcon,nlrhs,nle)
[x,fval,ef,info] = solve(Opt,x0)
%Plot
plot(Opt,[],1)

%% Nonlinear Constraints Double Sided Ineq, Nonlinear + Linear
clc
%Objective
obj = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
% Constraints
A = [-1 1; 1 -1];
b = [0;2];
nlcon = @(x) [-x(1) + x(2); x(1) - x(2); x(1) + x(2)];
nlrhs = [-1;3;5];
nle = [-1;-1;0];
lb = [0;0]; ub = [4;4];
% Solve
x0 = [2;2];
Opt = opti('obj',obj,'ndec',2,'bounds',lb,ub,'ivars',1,'nlmix',nlcon,nlrhs,nle,'ineq',A,b)
[x,fval,ef,info] = solve(Opt,x0)
%Plot
plot(Opt,[],1)

%% Nonlinear Constraints Double Sided Row, NonLinear + Linear
clc
%Objective
obj = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
% Constraints
A = [-1 1];
rl = -2; ru = 0;
nlcon = @(x) [-x(1) + x(2); x(1) + x(2)];
cl = [-3;5];
cu = [-1;5];
lb = [0;0]; ub = [4;4];
% Solve
x0 = [2;2];
Opt = opti('obj',obj,'ndec',2,'bounds',lb,ub,'ivars',1,'nl',nlcon,cl,cu,'lin',A,rl,ru)
[x,fval,ef,info] = solve(Opt,x0)
%Plot
plot(Opt,[],1)


function [V,D]=spreadeigs(K,coef);
%SPREADEIGS  Eigenpairs of Spreading operator
%   Usage: h=spreadeigs(K,c);
%
%   `spreadeigs(K,c)` computes the *K* largest eigenvalues and eigen-
%   vectors of the spreading operator with symbol *c*.
%
%   See also:  tconv, spreadfun, spreadinv, spreadadj

complainif_argnonotinrange(nargin,2,2,mfilename);

if ndims(coef)>2 || size(coef,1)~=size(coef,2)
    error('Input symbol coef must be a square matrix.');
end;

L=size(coef,1);

% This version explicitly constucts the matrix representation T
% and then applies this matrix as the final step.
coef=fft(coef);

T=zeros(L);
for ii=0:L-1
  for jj=0:L-1
    T(ii+1,jj+1)=coef(ii+1,mod(ii-jj,L)+1);
  end;
end;

if nargout==2
  doV=1;
else
  doV=0;
end;

if doV
  [V,D]=eig(T);
else
  D=eig(T);
end;


%TRINTERP2 Interpolate SE(2) homogeneous transformations
%
% TRINTERP2(T0, T1, S) is a homogeneous transform (3x3) interpolated
% between T0 when S=0 and T1 when S=1.  T0 and T1 are both homogeneous
% transforms (4x4).  If S (Nx1) then T (3x3xN) is a sequence of
% homogeneous transforms corresponding to the interpolation values in S.
%
% TRINTERP2(T1, S) as above but interpolated between the identity matrix
% when S=0 to T1 when S=1.
%
% TRINTERP2(T0, T1, M) as above but M is a positive integer and return a
% sequence (4x4xM) of homogeneous transforms linearly interpolating between
% T0 and T1 in M steps.
%
% TRINTERP2(T1, M) as above but return a sequence (4x4xM) of
% homogeneous interpolating between identity matrix and T1 in M steps.
%
% Notes::
% - T0 or T1 can also be an SO(2) rotation matrix (2x2).
% - Rotation angle is linearly interpolated.
% - To obtain smooth continuous motion S should also be smooth and continuous,
%   such as computed by tpoly or lspb.
%
% See also TRINTERP, SE3.interp, UnitQuaternion, TPOLY, LSPB.

%## 2d homogeneous trajectory

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

function T = trinterp2(A, B, C)

    switch nargin
        case 2
            % trinterp(T, s)
            T1 = A; s = B;

            th0 = 0;
            th1 = atan2(T1(2,1), T1(1,1));
            if ~isrot2(T1)
                p0 = [0 0]';
                p1 = transl2(T1);
            end
        case 3
            % trinterp(T1, T2, s)
            T0 = A; T1 = B; s = C;
            assert(all(size(A) == size(B)), 'SMTB:trinterp2:badarg', '2 matrices must be same size');
            th0 = atan2(T0(2,1), T0(1,1));
            th1 = atan2(T1(2,1), T1(1,1));
            if ~isrot2(T0)
                p0 = transl2(T0);
                p1 =transl2(T1);
            end
        otherwise
            error('SMTB:trinterp2:badarg', 'must be 2 or 3 arguments');
    end

    if length(s) == 1 && s > 1 && (s == floor(s))
        % integer value
        s = [0:(s-1)] / (s-1);
    elseif any(s<0 | s>1)
        error('SMTB:trinterp2:badarg', 'values of S outside interval [0,1]');
    end

    if isrot2(T1)

        % SO(2) case
        for i=1:length(s)
            th = th0*(1-s(i)) + s(i)*th1;

            T(:,:,i) = rot2(th);
        end
    else
        % SE(2) case
        for i=1:length(s)
            th = th0*(1-s(i)) + s(i)*th1;
            pr = p0*(1-s(i)) + s(i)*p1;

            T(:,:,i) = rt2tr(rot2(th), pr);
        end
    end

end


function CartStates=stateRuv2Cart(x)
%%STATERUV2CART Convert a target state in 3D space in monostatic
%               r-u-v coordinates with first and possibly second time
%               derivatives of the position components into Cartesian
%               coordinates. The state has the format
%               [r;u;v;rDot;uDot;vDot;rDDot;uDDot;vDDot],
%               where two Ds indicate a second derivative with respect to
%               time. In Cartesian coordinates, the converted state has the
%               form [x;y;z;xDot;yDot;zDot;xDDot;yDDot;zDDot]. The target
%               is assumed to be in front of the radar.
%
%INPUTS: x The 6XN or 9XN set of r-u-v state vectors consisting of
%          position, velocity and possibly acceleration.  The range is a
%          one-way range.
%
%OUTPUTS: CartStates The 6XN or 9XN set of target states given in Cartesian
%                    coordinates.
%
%The derivation is given in [1]. The function aCVRuv is an implementation
%of a related linear dynamic model.
%
%EXAMPLE:
%Here we note that the results are consistent with the inverse function:
%stateCart2Ruv.
% x=[100;-60;200;-3;12;160;108;-116;2];
% xRet=stateRuv2Cart(stateCart2Ruv(x));
% max(abs(x(:)-xRet(:)))
%One will see that the error is less than 1e-13, indicating good agreement.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic linear Cartesian dynamic models in local
%    coordinates," Naval Research Laboratory, Washington, DC, Tech. Rep.
%    NRL/MR/5344-19-9882, 24 Aug. 2019.
%
%August 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDim=size(x,1);
N=size(x,2);
CartStates=zeros(numDim,N);

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

w2=max(0,1-u.^2-v.^2);
w=sqrt(w2);
diffV2=1-v.^2;
diffV=sqrt(1-v.^2);
denom2=w2.*diffV2;
denom=sqrt(denom2);

u1=[u;v;w];
u2=[w./diffV;zeros(1,N);-u./diffV];
u3=[-u.*v./diffV;diffV;-v.*(w./diffV)];

%Position
CartStates(1:3,:)=bsxfun(@times,r,u1);

c1=(uDot.*diffV2+u.*v.*vDot)./denom;
c2=vDot./diffV;

%Velocity
CartStates(4:6,:)=bsxfun(@times,rDot,u1)+bsxfun(@times,r.*c1,u2)+bsxfun(@times,r.*c2,u3);

if(numDim>6)
    %Acceleration
    rDDot=x(7,:);
    uDDot=x(8,:);
    vDDot=x(9,:);

    c3=-(((w+u.^2./w).*(uDot-uDot.*v.^2+u.*v.*vDot))./diffV.^3);
    c4=v.*(-u.^2.*(1./w)-w).*(-uDot.*diffV2-u.*v.*vDot)./diffV2.^2;
    c5=-vDot./diffV;
    c6=(w./diffV).*(-v.*uDot.*diffV2-u.*vDot)./diffV.^3+u.*(diffV./w).*(-u.*v.*uDot.*diffV2-u.^2.*vDot+vDot.*diffV2.^2)./diffV.^5;

    c1Dot=((uDot.*diffV2+u.*v.*vDot).*(v.*vDot.*(2-u.^2-2*v.^2)+u.*uDot.*diffV2))./denom.^3 ...
          +(uDDot.*diffV2-v.*uDot.*vDot+u.*(v.*vDDot+vDot.^2))./denom;
    c2Dot=(vDDot.*diffV2+v.*vDot.^2)./diffV.^3;

    a1=rDDot+r.*(c1.*c3+c2.*c5);
    a2=2*rDot.*c1+r.*(c1Dot+c2.*c6);
    a3=2*rDot.*c2+r.*(c2Dot+c1.*c4);

    CartStates(7:9,:)=bsxfun(@times,a1,u1)+bsxfun(@times,a2,u2)+bsxfun(@times,a3,u3);
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.


%%Ex 4 Combine looping and branching

sum1 = 0;
sum2 = 0;
N = 9
for k = 1:N
 sum1 = sum1+k;
 if (mod(k,3) == 0)
 sum2 = sum2+k;
 end
end
sum1
sum2

% Output:
%        sum1 = 45
%        sum2 = 18
% Remark: Sum1 = 1+2+3+4+5+6+7+8+9, while sum2 = 3+6+9.


function [H,G] = make_bank_3(h,nbands)

% make_bank_3      Creates Analysis/Synthesis Filters for Pseudo-QMF Filter Banks
%
%                  Standard type of cosine modulation where the phase reference is (L-1)/2
% Arguments:
% h                Impulse response of prototype filter
% ndands           Number of subbands
%
% by Lee, Gan, and Kuo, 2008
% Subband Adaptive Filtering: Theory and Implementation
% Publisher: John Wiley and Sons, Ltd

flen = max(size(h));
H = zeros(nbands,flen);

% Note: k starts from 1, thus (k-0.5) and (k-1) in the cosine modulations

n = 0:flen-1;
for k=1:nbands
    H(k,:) = h.*cos((pi/nbands)*(k-0.5)*(n-(flen-1)/2)+ (pi/2)*(k+0.5)*(flen-1-nbands)/nbands)*2;
end

% Synthesis filters are time-reversed versions of the analysis filters

% Gk(z) = z^(-L+1)H(z^-1)
G = fliplr(H);


function [lambda_vec, error_train, error_val] = ...
    validationCurve(X, y, Xval, yval)
%VALIDATIONCURVE Generate the train and validation errors needed to
%plot a validation curve that we can use to select lambda
%   [lambda_vec, error_train, error_val] = ...
%       VALIDATIONCURVE(X, y, Xval, yval) returns the train
%       and validation errors (in error_train, error_val)
%       for different values of lambda. You are given the training set (X,
%       y) and validation set (Xval, yval).
%

% Selected values of lambda (you should not change this)
lambda_vec = [0 0.001 0.003 0.01 0.03 0.1 0.3 1 3 10]';

% You need to return these variables correctly.
error_train = zeros(length(lambda_vec), 1);
error_val = zeros(length(lambda_vec), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return training errors in
%               error_train and the validation errors in error_val. The
%               vector lambda_vec contains the different lambda parameters
%               to use for each calculation of the errors, i.e,
%               error_train(i), and error_val(i) should give
%               you the errors obtained after training with
%               lambda = lambda_vec(i)
%
% Note: You can loop over lambda_vec with the following:
%
%       for i = 1:length(lambda_vec)
%           lambda = lambda_vec(i);
%           % Compute train / val errors when training linear
%           % regression with regularization parameter lambda
%           % You should store the result in error_train(i)
%           % and error_val(i)
%           ....
%
%       end
%
%

for i = 1:length(lambda_vec),
  lambda = lambda_vec(i);
  theta = trainLinearReg(X, y, lambda);
  error_train(i) = linearRegCostFunction(X, y, theta, 0);
  error_val(i) = linearRegCostFunction(Xval, yval, theta, 0);
endfor

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

end


%RPY2JAC Jacobian from RPY angle rates to angular velocity
%
% J = RPY2JAC(EUL) is a Jacobian matrix (3x3) that maps roll-pitch-yaw angle
% rates to angular velocity at the operating point RPY=[R,P,Y].
%
% J = RPY2JAC(R, P, Y) as above but the roll-pitch-yaw angles are passed
% as separate arguments.
%
% Notes::
% - Used in the creation of an analytical Jacobian.
%
% See also EUL2JAC, SerialLink.JACOBN.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function J = rpy2jac(r, p, y)

    if length(r) == 3
        % rpy2jac([r,p,y])
        p = r(2);
        y = r(3);
        r = r(1);
    elseif nargin ~= 3
        error('RTB:rpy2jac:badarg', 'bad arguments');
    end
	J = [
        1  0       sin(p)
        0  cos(r)  -cos(p)*sin(r)
        0  sin(r)  cos(p)*cos(r)
        ];


function m=v_pesq2mos(p)
%V_PESQ2MOS convert PESQ speech quality scores to MOS m=(p)
%Inputs:    p  is a matrix of PESQ scores
%
%Outputs:   m  is a matrix, the same size as p, of MOS scores
%
% The PESQ measure is defined in [2]. The mapping function, defined in [3],
% converts raw PESQ scores (which lie in the range -0.5 to 4.5) onto the
% MOS-LQO (Mean Opinion Score - Listening Quality Objective [2]) scale in the
% range 1 to 5. The MOS scale is defined in [1] as
%           5=Excellent, 4=Good, 3=Fair, 2=Poor, 1=Bad.
%
% Refs: [1]	ITU-T. Methods for subjective determination of transmission quality.
%           Recommendation P.800, Aug. 1996.
%       [2]	ITU-T. Mean opinion score (MOS) terminology.
%           Recommendation P.800.1, July 2006.
%       [2]	ITU-T. Perceptual evaluation of speech quality (PESQ), an objective
%           method for end-to-end speech quality assessment of narrowband telephone
%           networks and speech codecs. Recommendation P.862, Feb. 2001.
%       [3]	ITU-T. Mapping function for transforming P.862 raw result scores to MOS-LQO.
%           Recommendation P.862.1, Nov. 2003.

%      Copyright (C) Mike Brookes 2012-2013
%      Version: $Id: v_pesq2mos.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b c d
if isempty(a)
    a=0.999;
    b=4.999-a;
    c=-1.4945;
    d=4.6607;
end
if nargout>0
    m=a+b./(1+exp(c*p+d));
else
    if nargin<1 || isempty(p)
        pp=linspace(-0.5,4.5,100);
    else
        pp=p;
    end
    plot(pp,v_pesq2mos(pp));
    xlabel('PESQ (P.862)');
    ylabel('Mean Opimion Score (MOS)');
end


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

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

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

%%

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

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

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

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

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

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

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

KSstatistic = -inf;

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

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

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

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

%% Peacock Z calculation and P estimation

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

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

H = (pValue <= alpha);


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

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

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

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

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

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

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

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

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

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

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

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

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


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

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

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

[nMet,nRxn]=size(A);

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

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


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

    p(isnan(p)) = [];

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

    end

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

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

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

    return


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

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

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

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

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end