function x = r8sp_cg ( n, nz_num, row, col, a, b, x )

%*****************************************************************************80
%
%% R8SP_CG uses the conjugate gradient method on an R8SP system.
%
%  Discussion:
%
%    The R8SP storage format stores the row, column and value of each nonzero
%    entry of a sparse matrix.
%
%    It is possible that a pair of indices (I,J) may occur more than
%    once.  Presumably, in this case, the intent is that the actual value
%    of A(I,J) is the sum of all such entries.  This is not a good thing
%    to do, but I seem to have come across this in MATLAB.
%
%    The R8SP format is used by CSPARSE ("sparse triplet"), DLAP/SLAP
%    ("nonsymmetric SLAP triad"), by MATLAB, and by SPARSEKIT ("COO" format).
%
%    The matrix A must be a positive definite symmetric matrix.
%
%    The method is designed to reach the solution after N computational
%    steps.  However, roundoff may introduce unacceptably large errors for
%    some problems.  In such a case, calling the routine again, using
%    the computed solution as the new starting estimate, should improve
%    the results.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frank Beckman,
%    The Solution of Linear Equations by the Conjugate Gradient Method,
%    in Mathematical Methods for Digital Computers,
%    edited by John Ralston, Herbert Wilf,
%    Wiley, 1967,
%    ISBN: 0471706892,
%    LC: QA76.5.R3.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer NZ_NUM, the number of nonzero elements in
%    the matrix.
%
%    Input, integer ROW(NZ_NUM), COL(NZ_NUM), the row and
%    column indices of the nonzero elements.
%
%    Input, real A(NZ_NUM), the nonzero elements of the matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input/output, real X(N).
%    On input, an estimate for the solution, which may be 0.
%    On output, the approximate solution vector.
%
  b = b(:);
  x = x(:);
%
%  Initialize
%    AP = A * x,
%    R  = b - A * x,
%    P  = b - A * x.
%
  ap = r8sp_mv ( n, n, nz_num, row, col, a, x );

  r(1:n,1) = b(1:n,1) - ap(1:n,1);
  p(1:n,1) = b(1:n,1) - ap(1:n,1);
%
%  Do the N steps of the conjugate gradient method.
%
  for it = 1 : n
%
%  Compute the matrix*vector product AP=A*P.
%
    ap = r8sp_mv ( n, n, nz_num, row, col, a, p );
%
%  Compute the dot products
%    PAP = P*AP,
%    PR  = P*R
%  Set
%    ALPHA = PR / PAP.
%
    pap = p' * ap;
    pr = p' * r;

    if ( pap == 0.0 )
      return
    end

    alpha = pr / pap;
%
%  Set
%    X = X + ALPHA * P
%    R = R - ALPHA * AP.
%
    x(1:n,1) = x(1:n,1) + alpha * p(1:n,1);
    r(1:n,1) = r(1:n,1) - alpha * ap(1:n,1);
%
%  Compute the vector dot product
%    RAP = R*AP
%  Set
%    BETA = - RAP / PAP.
%
    rap = r' * ap;

    beta = - rap / pap;
%
%  Update the perturbation vector
%    P = R + BETA * P.
%
    p(1:n,1) = r(1:n,1) + beta * p(1:n,1);

  end

  return
end


function [Q,J,F,T,I,V,E] = spinv(A,varargin)
  % SPINV Compute a sparse factorization of the Moore-Penrose pseudoinverse A⁺
  % of a sparse matrix A.  This is useful for solving
  % under-and-over-constrained systems Ax = b: where A is m by n with
  % rank r < min(m,n). Then to solve the system:
  %
  %     x = E*(V*(I'*(T'\(F'*(J'*(Q'*b))))))
  %
  % Note: Store these sparse factors and sovle using the routine above **with
  % parentheses**. Do not precompute A⁺ as it will be dense.
  %
  % Inputs:
  %   A  m by n sparse
  %   Optional:
  %     'Epsilon' followed by epsilon used to determine rank from qr {1e-15}
  % Outputs:
  %   Q  m by m sparse orthogonal matrix
  %   J  m by r sparse rectangular identity matrix
  %   F  r by r sparse permutation matrix
  %   T  r by r sparse triangular matrix (diagonal?)
  %   I  r by n sparse rectangular identity matrix
  %   V  n by n sparse orthogonal matrix
  %   E  n by n sparse permutation matrix
  %
  % Example:
  %   % over and under determined system of 3 variables
  %   A = [1 0 0;1 0 0;0 1 1;0 1 1];
  %   b = [4;5;6;7];
  %   % Compute facotrization of sparse pseudo inverse
  %   [Q,J,F,T,I,V,E] = spinv(A);
  %   % Compute minimum norm best fit
  %   xs = E*(V*(I'*(T'\(F'*(J'*(Q'*b))))));
  %   % Compare to Tikhonov regularization
  %   xt = (A'*A+1e-10*speye(3,3))\(A'*b);
  %

  % default values
  epsilon = 1e-15;
  % Map of parameter names to variable names
  params_to_variables = containers.Map( {'Epsilon'}, {'epsilon'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end
  % http://www.irisa.fr/sage/wg-statlin/WORKSHOPS/LEMASSOL05/SLIDES/QR/Guyomarch.pdf

  % dimensions of A
  m = size(A,1);
  n = size(A,2);
  % QR factorization of AE
  [Q,R,E] = qr(A);
  % We have that:
  %     AE = QR
  % rank
  r = find(any(abs(R)>epsilon,2),1,'last');
  % Only keep upper, non-zero block
  R1R2 = R(1:r,:);
  J = speye(m,r);
  % We have that:
  %    AE = Q*J*R1R2
  % QR factorization of R1R2'
  [V,Tfull,F] = qr(R1R2');
  % This means:
  %    R1R2'F = V*Tfull
  % only keep upper, non-zero block
  T = Tfull(1:r,:);
  I = speye(r,n);
  % and now:
  %    R1R2'F = V*I'*T
  %    R1R2' = V*I'*T*F'
  % Now we have that
  %     AE = Q*J*F*T'*I*V'
  % We want to solve:
  %     Ax = b
  %     AEE'x = b
  %     E'x = (Q*J*F*T'*I*V')⁺b
  %     x = E*(Q*J*F*T'*I*V')⁺b
  %     x = E*(V*(I'*(T'\(F'*(J'*(Q'*b))))))
  %
end


function varargout = trigonometric(X)
% Trigonometric function
%
%   TRIGONOMETRIC([x1, x2, ..., xn]) returns the value of the
%   trigonometric fnuction at the specified points. All [xi] may be
%   vectors.
%
%   The global minimum is
%
%               f(x1, x2, ..., xn) = f(0, 0, ..., 0) = 0.

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

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

    % otherwise, output function value
    else

        % create basevectors
        n = size(X, 2);
        i = repmat(1:n, size(X, 1), 1);

        % compute cosine only once
        cosX = cos(X);

        % compute intermediate sum
        sumcosX = repmat(sum(cosX, 2), 1, n);

        % output rowsum
        varargout{1} = sum( (n + i.*(1 - cosX) - sin(X) - sumcosX).^2, 2);

    end

end


function [fr,th_val,phi_val] = tess_parametrize(vert, th_val, phi_val, p)
% TESS_PARAMETRIZE_NEW: Get a parametric representation of a closed and non-overlapping surface.
%
% USAGE:  tess_parametrize(vert, th_val, phi_val, p)
%         tess_parametrize(vert, th_val, phi_val)
%
% INPUTS:
%    - vert    : x,y,z coordinates of the surface to parametrize
%    - th_val  : Row array of theta values
%    - phi_val : Row array of phi values
%    - p       : Pad the function of p values in both direction with circular values

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2011

% Parse inputs
if (nargin < 4) || isempty(p)
    p = 0;
end

% Convert surface to spherical coordinates
[th,phi,r] = cart2sph(vert(:,1), vert(:,2), vert(:,3));
% Replicate the values to get a full coverage at the edges
th  = [th;  th;        th;        th+2*pi;  th+2*pi;  th+2*pi;   th-2*pi;  th-2*pi;  th-2*pi  ];
phi = [phi; phi+2*pi;  phi-2*pi;  phi;      phi+2*pi; phi-2*pi;  phi;      phi+2*pi; phi-2*pi ];
r   = [r;   r;         r;         r;        r;        r;         r;        r;        r        ];

% Build grid of (theta,phi) values
[fth, fphi] = meshgrid(th_val, phi_val);
% Estimate radius values for all those points
fr = griddata(th, phi, r, fth, fphi);

% Pad function with circular values, to avoid edge irregularities
if (p > 0)
    fr_pad  = bst_pad(fr, [0  p], 'circular');
    fr_pad  = bst_pad(fr_pad, [p  0], 'mirror');
    fr = fr_pad;
    th_val  = [th_val(end-p+1:end)-2*pi,  th_val,  th_val(1:p)+2*pi];
    phi_val = [phi_val(end-p+1:end)-2*pi, phi_val, phi_val(1:p)+2*pi];
end


function value = r8_lgic ( a, x, alx )

%*****************************************************************************80
%
%% R8_LGIC evaluates the log complementary incomplete gamma function for large X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real A, the parameter.
%
%    Input, real X, the argument.
%
%    Input, real ALX, the logarithm of X.
%
%    Output, real VALUE, the log complementary incomplete
%    gamma function.
%
  persistent eps

  if ( isempty ( eps ) )
    eps = 0.5 * r8_mach ( 3 );
  end

  xpa = x + 1.0 - a;
  xma = x - 1.0 - a;

  r = 0.0;
  p = 1.0;
  s = p;
  for k = 1 : 300
    fk = k;
    t = fk * ( a - fk ) * ( 1.0 + r );
    r = - t / ( ( xma + 2.0 * fk ) * ( xpa + 2.0 * fk ) + t );
    p = r * p;
    s = s + p;
    if ( abs ( p ) < eps * s )
      value = a * alx - x + log ( s / xpa );
      return
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8_LGIC - Fatal error!\n' );
  fprintf ( 1, '  No convergence in 300 iterations.\n' );

  error ( 'R8_LGIC - Fatal error!' )
end


function determ = r8mat_gedet ( a, n, pivot )

%*****************************************************************************80
%
%% R8MAT_GEDET computes the determinant of a matrix factored by R8MAT_GEFA.
%
%  Discussion:
%
%    This is a modified version of the LINPACK routine DGEDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(N,N), the LU factors computed by R8MAT_GEFA.
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer PIVOT(N), as computed by R8MAT_GEFA.
%
%    Output, real DETERM, the determinant of the matrix.
%
  diag = r8mat_diag_get_vector ( n, a );

  determ = prod ( diag(1:n) );

  for i = 1 : n
    if ( pivot(i) ~= i )
      determ = - determ;
    end
  end

  return
end


function [ n_data, x, fx ] = sin_values ( n_data )

%*****************************************************************************80
%
%% SIN_VALUES returns some values of the sine function.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      Sin[x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%  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, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 13;

  fx_vec = [ ...
     0.00000000000000000000, ...
     0.25881904510252076235, ...
     0.47942553860420300027, ...
     0.50000000000000000000, ...
     0.70710678118654752440, ...
     0.84147098480789650665, ...
     0.86602540378443864676, ...
     1.00000000000000000000, ...
     0.90929742682568169540, ...
     0.14112000805986722210, ...
     0.00000000000000000000, ...
    -0.75680249530792825137, ...
    -0.95892427466313846889 ];

  x_vec = [ ...
    0.0000000000000000000, ...
    0.26179938779914943654, ...
    0.50000000000000000000, ...
    0.52359877559829887308, ...
    0.78539816339744830962, ...
    1.0000000000000000000, ...
    1.0471975511965977462, ...
    1.5707963267948966192, ...
    2.0000000000000000000, ...
    3.0000000000000000000, ...
    3.1415926535897932385, ...
    4.0000000000000000000, ...
    5.0000000000000000000  ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function varargout = gallery2(name)
%CHEB.GALLERY2   Chebfun2 example functions.
%   F = CHEB.GALLERY2(NAME) returns a chebfun2 corresponding to NAME. See the
%   listing below for available names.
%
%   For example,  plot(cheb.gallery2('peaks'))  plots the classic MATLAB peaks
%   function represented as a chebfun2. For details of how each function is
%   constructed, try type +cheb/gallery2 or edit cheb.gallery2.
%
%   [F,FA] = CHEB.GALLERY2(NAME) also returns the anonymous function FA used to
%   define the function.
%
%   CHEB.GALLERY2 with no input argument returns a function selected at
%   random from the gallery.
%
%   CHEB.GALLERY2 with no output argument creates a plot of the selected
%   function.
%
%   airyreal     Real part of Airy Ai function
%   airycomplex  Imaginary part of Airy Ai function
%   bump         2D C-infinity function with compact support
%   challenge    Function from SIAM 100-digit challenge
%   peaks        Classic MATLAB peaks function
%   rosenbrock   Challenging test function in optimization
%   roundpeg     Approx characteristic function of a disk, rank 45
%   smokering    A halo, hoop, or hole
%   squarepeg    Approx characteristic function of a square, rank 1
%   tiltedpeg    A tilted variant of squarepeg, rank 100
%   waffle       Function with horizontal and vertical ridges
%
%   Gallery functions are subject to change in future releases of Chebfun.
%
% See also CHEB.GALLERY, CHEB.GALLERYTRIG, CHEB.GALLERY3, CHEB.GALLERYDISK, CHEB.GALLERYSPHERE.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% If the user did not supply an input, return a function selected at
% random from the gallery.
if ( nargin == 0 )
    names = {'airyreal', 'airycomplex', 'challenge', 'bump', 'peaks', ...
        'rosenbrock', 'roundpeg', 'smokering', 'squarepeg', 'tiltedpeg', ...
        'waffle'};
    name = names{randi(length(names))};
end

% The main switch statement.
switch lower(name)

    % Real part of Airy Ai function:
    case 'airyreal'
        fa = @(z) real(airy(z));
        f = chebfun2(fa, [-10 10 0 1]);

    % Imaginary part of Airy Ai function:
    case 'airycomplex'
        fa = @(z) airy(z);
        f = chebfun2(fa, [-10 10 -5 5]);

    % Two-dimensional bump function:
    case 'bump'
        fa = @(x,y) (x.^2 + y.^2 < 1).*exp(-(x.^2 + y.^2 < 1)./ ...
            (1 - x.^2 - y.^2));
        f = chebfun2(fa, [-1 1 -1 1]*2);

    % Function from SIAM 100-digit challenge:
    case 'challenge'
        fa = @(x,y) exp(sin(50*x)) + sin(60*exp(y)) + sin(70*sin(x)) + ...
            sin(sin(80*y)) - sin(10*(x+y)) + (x.^2+y.^2)./4;
        f = chebfun2(fa);

    % The classic MATLAB peaks function:
    case 'peaks'
        fa = @(x,y) 3*(1-x).^2.*exp(-x.^2 - (y+1).^2) ...
            - 10*(x/5 - x.^3 - y.^5).*exp(-x.^2 - y.^2) ...
            - 1/3*exp(-(x+1).^2 - y.^2);
        f = chebfun2(fa, [-3 3 -3 3]);

    % A challenging test function in optimization:
    case 'rosenbrock'
        fa = @(x,y) (1-x).^2 + 100*(y-x.^2).^2;
        f = chebfun2(fa, [-2 2 -1 3]);

    % Approximately the characteristic function of a disk:
    case 'roundpeg'
        fa = @(x,y) 1./(1+((2*x).^2+(2*y).^2).^10);
        f = chebfun2(fa);

    % A halo, hoop, or hole:
    case 'smokering'
        fa = @(x,y) exp(-100*(x.^2 - x.*y + 2*y.^2 - 1/2).^2);
        f = chebfun2(fa);

    % Approximately the characteristic function of a square:
    case 'squarepeg'
        fa = @(x,y) 1./((1+(2*x).^20).*(1+(2*y).^20));
        f = chebfun2(fa);

    % A tilted version of squarepeg:
    case 'tiltedpeg'
        fa = @(x,y) 1./((1+(2*x+.4*y).^20).*(1+(2*y-.4*x).^20));
        f = chebfun2(fa);

    % A function with horizontal and vertical ridges:
    case 'waffle'
        fa = @(x,y) 1./(1 + 1e3*((x.^2-.25).^2.*(y.^2-.25).^2));
        f = chebfun2(fa);

    % Raise an error if the input is unknown.
    otherwise
        error('CHEB:GALLERY2:unknown:unknownFunction', ...
            'Unknown function.')
end

% Only return something if there is an output argument.
if ( nargout > 0 )
    varargout = {f, fa};
else
    % Otherwise, plot the function.
    plot(f)
    title([name ', rank = ' num2str(length(f))])
end

end


function [lp,dlp] = priorT(mu,s2,nu,x)

% Univariate Student's t hyperparameter prior distribution.
% Compute log-likelihood and its derivative or draw a random sample.
% The prior distribution is parameterized as:
%
%   p(x) = ( 1 + (x-mu)^2/(s2*(nu-2)) )^(-(nu+1)/2) / Z, where
%      Z = gamma(nu/2) * sqrt(pi*(nu-2)*s2) / gamma((nu+1)/2),
%
% mu(1x1) is the mean parameter, s2(1x1) is the variance parameter,
% nu(1x1) > 2 is the degrees of freedom parameter and x(1xN) contains query
% hyperparameters for prior evaluation.
%
% For more help on design of priors, try "help priorDistributions".
%
% Copyright (c) by Roman Garnett and Hannes Nickisch, 2014-10-16.
%
% See also PRIORDISTRIBUTIONS.M.

if nargin<3, error('mu, s2 and nu parameters need to be provided'), end
if ~(isscalar(mu)&&isscalar(s2)&&isscalar(nu))
  error('mu, s2 and nu parameters need to be scalars')
end
if nargin<4, lp = mu + sqrt(s2*(nu-2)/priorGamma(nu/2,2))*randn; return, end

lZ = gammaln((nu+1)/2)-gammaln(nu/2)-log(pi*(nu-2)*s2)/2;
lp = -(nu+1)/2*log(1+(x-mu).^2/(s2*(nu-2))) + lZ;
dlp = (nu+1)*(mu-x)./(mu^2-2*mu*x+s2*(nu-2)+x.^2);


function cvx_optval = norm_largest( x, k )

%NORM_LARGEST Sum of the k largest magnitudes of a vector.
%   NORM_LARGEST( X, k ) computes the 'largest-k' norm; that is, it computes
%   the sum of the magnitudes of the k largest elements in X. X must
%   be a vector, and k must be a real scalar.
%
%   Disciplined convex programming information:
%       NORM_LARGEST is convex and nonmonotonic of X, though it is monotonic for
%       positive values of X. So when used in CVX expressions, X must be affine,
%       monomial, or posynomial. k must be a real scalar constant.

if ~any( size( x ) ~= 1 ),
    cvx_throw( 'The first argument must be a vector.' );
elseif ~isnumeric( k ) || ~isreal( k ) || length( k ) ~= 1,
    cvx_throw( 'Third argument must be a scalar.' );
else
    cvx_optval = sum_largest( abs( x ), k );
end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function value = diaedg ( x0, y0, x1, y1, x2, y2, x3, y3 )

%*****************************************************************************80
%
%% DIAEDG chooses a diagonal edge.
%
%  Discussion:
%
%    The routine determines whether 0--2 or 1--3 is the diagonal edge
%    that should be chosen, based on the circumcircle criterion, where
%    (X0,Y0), (X1,Y1), (X2,Y2), (X3,Y3) are the vertices of a simple
%    quadrilateral in counterclockwise order.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2005
%
%  Author:
%
%    Original FORTRAN77 version by Barry Joe,
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, real X0, Y0, X1, Y1, X2, Y2, X3, Y3, the
%    coordinates of the vertices of a quadrilateral, given in
%    counter clockwise order.
%
%    Output, integer VALUE, chooses a diagonal:
%    +1, if diagonal edge 02 is chosen;
%    -1, if diagonal edge 13 is chosen;
%     0, if the four vertices are cocircular.
%
  tol = 100.0 * eps;

  dx10 = x1 - x0;
  dy10 = y1 - y0;
  dx12 = x1 - x2;
  dy12 = y1 - y2;
  dx30 = x3 - x0;
  dy30 = y3 - y0;
  dx32 = x3 - x2;
  dy32 = y3 - y2;

  tola = tol * max ( abs ( dx10 ), ...
               max ( abs ( dy10 ), ...
               max ( abs ( dx30 ), abs ( dy30 ) )));

  tolb = tol * max ( abs ( dx12 ), ...
               max ( abs ( dy12 ), ...
               max ( abs ( dx32 ), abs ( dy32 ) )));

  ca = dx10 * dx30 + dy10 * dy30;
  cb = dx12 * dx32 + dy12 * dy32;

  if ( tola < ca & tolb < cb )

    value = -1;

  elseif ( ca < -tola & cb < -tolb )

    value = 1;

  else

    tola = max ( tola, tolb );
    s = ( dx10 * dy30 - dx30 * dy10 ) * cb ...
      + ( dx32 * dy12 - dx12 * dy32 ) * ca;

    if ( tola < s )
      value = -1;
    elseif ( s < -tola )
      value = 1;
    else
      value = 0;
    end

  end

  return
end


function Test_fixedrankembeddedfactory()
% Given partial observation of a low rank matrix, attempts to complete it.
%
% function low_rank_matrix_completion()
%
% This example demonstrates how to use the geometry factory for the
% embedded submanifold of fixed-rank matrices, fixedrankembeddedfactory.
% This geometry is described in the paper
% "Low-rank matrix completion by Riemannian optimization"
% Bart Vandereycken - SIAM Journal on Optimization, 2013.
%
% This can be a starting point for many optimization problems of the form
%
% minimize f(X) such that rank(X) = k, size(X) = [m, n].
%
% Input:  None. This example file generate random data.
%
% Output: None.

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, July 15, 2014
% Contributors:
%
% Change log:
%

    % Choose the size of the problem:
    % We will complete a matrix of size mxn of rank k.
    m = 2000;
    n = 5000;
    k = 10;
    % Generate a random mxn matrix A of rank k
    L = randn(m, k);
    R = randn(n, k);
    A = L*R';
    % Generate a random mask for observed entries: P(i, j) = 1 if the entry
    % (i, j) of A is observed, and 0 otherwise.
    fraction = 4 * k*(m+n-k)/(m*n);
    P = sparse(rand(m, n) <= fraction);
    % Hence, we know the nonzero entries in PA:
    PA = P.*A;

    % Pick the manifold of matrices of size mxn of fixed rank k.
    problem.M = fixedrankembeddedfactory(m, n, k);

    % Define the problem cost function. The input X is a structure with
    % fields U, S, V representing a rank k matrix as U*S*V'.
    % f(X) = 1/2 * || P.*(X-A) ||^2
    problem.cost = @cost;
    function f = cost(X)
        % Note that it is very much inefficient to explicitly construct the
        % matrix X in this way. Seen as we only need to know the entries
        % of Xmat corresponding to the mask P, it would be far more
        % efficient to compute those only.
        Xmat = X.U*X.S*X.V';
        f = .5*norm( P.*Xmat - PA , 'fro')^2;
    end

    % Define the Euclidean gradient of the cost function, that is, the
    % gradient of f(X) seen as a standard function of X.
    % nabla f(X) = P.*(X-A)
    problem.egrad = @egrad;
    function G = egrad(X)
        % Same comment here about Xmat.
        Xmat = X.U*X.S*X.V';
        G = P.*Xmat - PA;
    end

    % Define the Euclidean Hessian of the cost at X, along H, where H is
    % represented as a tangent vector.
    % nabla^2 f(X)[Xdot] = P.*Xdot
    % (This is the directional derivative of nabla f(X) at X along Xdot.)
    problem.ehess = @euclidean_hessian;
    function ehess = euclidean_hessian(X, H)
        % The function tangent2ambient transforms H (a tangent vector) into
        % its equivalent ambient vector representation. The output is a
        % structure with fields U, S, V such that U*S*V' is an mxn matrix
        % corresponding to the tangent vector H. Note that there are no
        % additional guarantees about U, S and V. In particular, U and V
        % are not orthonormal.
        ambient_H = problem.M.tangent2ambient(X, H);
        Xdot = ambient_H.U*ambient_H.S*ambient_H.V';
        % Same comment here about explicitly constructing the ambient
        % vector as an mxn matrix Xdot: we only need its entries
        % corresponding to the mask P, and this could be computed
        % efficiently.
        ehess = P.*Xdot;
    end

    % Check consistency of the gradient and the Hessian. Useful if you
    % adapt this example for a now cost function and you would like to make
    % sure there is no mistake.
    % warning('off', 'manopt:fixedrankembeddedfactory:exp');
    % checkgradient(problem); pause;
    % checkhessian(problem); pause;

    % Compute an initial guess. Points on the manifold are represented as
    % structures with three fields: U, S and V. U and V need to be
    % orthonormal, S needs to be diagonal.
    [U, S, V] = svds(PA, k);
    X0.U = U;
    X0.S = S;
    X0.V = V;

    % Minimize the cost function using Riemannian trust-regions, starting
    % from the initial guess X0.
    options.stopfun = @stopifclosedfigure;
    X = trustregions(problem, X0, options);

    % The reconstructed matrix is X, represented as a structure with fields
    % U, S and V.
    Xmat = X.U*X.S*X.V';
    fprintf('||X-A||_F = %g\n', norm(Xmat - A, 'fro'));

    % If the problem has a small enough dimension, we may (for analysis
    % purposes) compute the spectrum of the Hessian at a point X. This may
    % help in studying the conditioning of a problem.
    if problem.M.dim() < 100
        fprintf('Computing the spectrum of the Hessian...');
        s = hessianspectrum(problem, X);
        hist(s);
    end

end


% Figure 6.2: Penalty function approximation
% Section 6.1.2
% Boyd & Vandenberghe "Convex Optimization"
% Original by Lieven Vandenberghe
% Adapted for CVX Argyris Zymnis - 10/2005
%
% Comparison of the ell1, ell2, deadzone-linear and log-barrier
% penalty functions for the approximation problem:
%       minimize phi(A*x-b),
%
% where phi(x) is the penalty function
% Log-barrier will be implemented in the future version of CVX

% Generate input data
randn('state',0);
m=100; n=30;
A = randn(m,n);
b = randn(m,1);

% ell_1 approximation
% minimize   ||Ax+b||_1
disp('ell-one approximation');
cvx_begin
    variable x1(n)
    minimize(norm(A*x1+b,1))
cvx_end

% ell_2 approximation
% minimize ||Ax+b||_2
disp('ell-2');
x2=-A\b;

% deadzone penalty approximation
% minimize sum(deadzone(Ax+b,0.5))
% deadzone(y,z) = max(abs(y)-z,0)
dz = 0.5;
disp('deadzone penalty');
cvx_begin
    variable xdz(n)
    minimize(sum(max(abs(A*xdz+b)-dz,0)))
cvx_end

% log-barrier penalty approximation
%
% minimize -sum log(1-(ai'*x+bi)^2)

disp('log-barrier')

% parameters for Newton Method & line search
alpha=.01; beta=.5;

% minimize linfty norm to get starting point
cvx_begin
    variable xlb(n)
    minimize norm(A*xlb+b,Inf)
cvx_end
linf = cvx_optval;
A = A/(1.1*linf);
b = b/(1.1*linf);

for iters = 1:50

   yp = 1 - (A*xlb+b);  ym = (A*xlb+b) + 1;
   f = -sum(log(yp)) - sum(log(ym));
   g = A'*(1./yp) - A'*(1./ym);
   H = A'*diag(1./(yp.^2) + 1./(ym.^2))*A;
   v = -H\g;
   fprime = g'*v;
   ntdecr = sqrt(-fprime);
   if (ntdecr < 1e-5), break; end;

   t = 1;
   newx = xlb + t*v;
   while ((min(1-(A*newx +b)) < 0) || (min((A*newx +b)+1) < 0))
       t = beta*t;
       newx = xlb + t*v;
   end;
   newf = -sum(log(1 - (A*newx+b))) - sum(log(1+(A*newx+b)));
   while (newf > f + alpha*t*fprime)
       t = beta*t;
       newx = xlb + t*v;
       newf = -sum(log(1-(A*newx+b))) - sum(log(1+(A*newx+b)));
   end;
   xlb = xlb+t*v;
end

% Plot histogram of residuals

ss = max(abs([A*x1+b; A*x2+b; A*xdz+b;  A*xlb+b]));
tt = -ceil(ss):0.05:ceil(ss);  % sets center for each bin
[N1,hist1] = hist(A*x1+b,tt);
[N2,hist2] = hist(A*x2+b,tt);
[N3,hist3] = hist(A*xdz+b,tt);
[N4,hist4] = hist(A*xlb+b,tt);

range_max=2.0;  rr=-range_max:1e-2:range_max;

figure(1), clf, hold off
subplot(4,1,1),
bar(hist1,N1);
hold on
plot(rr, abs(rr)*40/3, '-');
ylabel('p=1')
axis([-range_max range_max 0 40]);
hold off

subplot(4,1,2),
bar(hist2,N2);
hold on;
plot(rr,2*rr.^2),
ylabel('p=2')
axis([-range_max range_max 0 11]);
hold off

subplot(4,1,3),
bar(hist3,N3);
hold on
plot(rr,30/3*max(0,abs(rr)-dz))
ylabel('Deadzone')
axis([-range_max range_max 0 25]);
hold off

subplot(4,1,4),
bar(hist4,N4);
rr_lb=linspace(-1+(1e-6),1-(1e-6),600);
hold on
plot(rr_lb, -3*log(1-rr_lb.^2),rr,2*rr.^2,'--')
axis([-range_max range_max 0 11]);
ylabel('Log barrier'),
xlabel('r')
hold off


function [x, c, funVal, ValueL]=LogisticC(A, y, z, opts)
%
%%
% Function LogisticC
%      Logistic Loss with the L1 ball constraint
%
%% Problem
%
%  min  f(x,c) = \sum_i - weight_i * log (p_i) + 1/2 rsL2 * ||x||_2^2
%  s.alpha. \|x\|_1 <=z
%
%  a_i denotes a training sample,
%      and a_i' corresponds to the i-th row of the data matrix A
%
%  y_i (either 1 or -1) is the response
%
%  p_i= 1/ (1+ exp(-y_i (x' * a_i + c) ) ) denotes the probability
%
%  weight_i denotes the weight for the i-th sample
%
%% Input parameters:
%
%  A-         Matrix of size m x n
%                A can be a dense matrix
%                         a sparse matrix
%                         or a DCT matrix
%  y -        Response vector (of size mx1)
%  z -        Radius of the L1 ball
%  opts-      Optional inputs (default value: opts=[])
%
%% Output parameters:
%  x-         The obtained weight of size n x 1
%  c-         The obtained intercept (scalar)
%  funVal-    Function value during iterations
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
%% Related papers
%
% [1]  Jun Liu, Jianhui Chen, and Jieping Ye, Large-Scale Sparse Logistic
%      Regression, KDD 2009
%
% [2]  Jun Liu and Jieping Ye, Efficient Euclidean Projections
%      in Linear Time, ICML 2009.
%
%% Related functions
%
%  sll_opts, initFactor, pathSolutionLeast,
%  LogisticR, nnLogisticR, nnLogisticC,
%  eplb
%
%%

%% Verify and initialize the parameters
%%
if (nargin <3)
    error('\n Inputs: A, y and z should be specified!\n');
end

[m,n]=size(A);

if (length(y) ~=m)
    error('\n Check the length of y!\n');
end

if (z<=0)
    error('\n z should be positive!\n');
end

opts=sll_opts(opts); % run sll_opts to set default values (flags)

%% Detailed initialization
%% Normalization

% Please refer to sll_opts for the definitions of mu, nu and nFlag
%
% If .nFlag =1, the input matrix A is normalized to
%                     A= ( A- repmat(mu, m,1) ) * diag(nu)^{-1}
%
% If .nFlag =2, the input matrix A is normalized to
%                     A= diag(nu)^{-1} * ( A- repmat(mu, m,1) )
%
% Such normalization is done implicitly
%     This implicit normalization is suggested for the sparse matrix
%                                    but not for the dense matrix
%

if (opts.nFlag~=0)
    if (isfield(opts,'mu'))
        mu=opts.mu;
        if(size(mu,2)~=n)
            error('\n Check the input .mu');
        end
    else
        mu=mean(A,1);
    end

    if (opts.nFlag==1)
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=n)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,1)/m).^(0.5); nu=nu';
        end
    else % .nFlag=2
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=m)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,2)/n).^(0.5);
        end
    end

    ind_zero=find(abs(nu)<= 1e-10);    nu(ind_zero)=1;
    % If some values in nu is typically small, it might be that,
    % the entries in a given row or column in A are all close to zero.
    % For numerical stability, we set the corresponding value to 1.
end

if (~issparse(A)) && (opts.nFlag~=0)
    fprintf('\n -----------------------------------------------------');
    fprintf('\n The data is not sparse or not stored in sparse format');
    fprintf('\n The code still works.');
    fprintf('\n But we suggest you to normalize the data directly,');
    fprintf('\n for achieving better efficiency.');
    fprintf('\n -----------------------------------------------------');
end

%% Group & Others

% The parameter 'weight' contains the weight for each training sample.
% See the definition of the problem above.
% The summation of the weights for all the samples equals to 1.
if (isfield(opts,'sWeight'))
    sWeight=opts.sWeight;

    if ( length(sWeight)~=2 || sWeight(1) <=0 || sWeight(2) <= 0)
        error('\n Check opts.sWeight, which contains two positive values');
    end

    % we process the weight, so that the summation of the weights for all
    % the samples is 1.

    p_flag=(y==1);                  % the indices of the postive samples
    m1=sum(p_flag) * sWeight(1);    % the total weight for the positive samples
    m2=sum(~p_flag) * sWeight(2);   % the total weight for the positive samples

    weight(p_flag,1)=sWeight(1)/(m1+m2);
    weight(~p_flag,1)=sWeight(2)/(m1+m2);
else
    weight=ones(m,1)/m;             % if not specified, we apply equal weight
end

%% Starting point initialization

% L2 norm regularization
if isfield(opts,'rsL2')
    rsL2=opts.rsL2;
    if (rsL2<0)
        error('\n opts.rsL2 should be nonnegative!');
    end
else
    rsL2=0;
end

% initialize a starting point
if opts.init==2
    x=zeros(n,1); c=0;
else
    if isfield(opts,'x0')
        x=opts.x0;
        if (length(x)~=n)
            error('\n Check the input .x0');
        end
    else
        x=zeros(n,1);
    end

    if isfield(opts,'c0')
        c=opts.c0;
    else
        c=0;
    end
end

% compute A x
if (opts.nFlag==0)
    Ax=A* x;
elseif (opts.nFlag==1)
    invNu=x./nu; mu_invNu=mu * invNu;
    Ax=A*invNu -repmat(mu_invNu, m, 1);
else
    Ax=A*x-repmat(mu*x, m, 1);    Ax=Ax./nu;
end

%% The main program

%% Nemirovski's line search

if (opts.lFlag==0)

    lambda0=0;
    % a guess of the root

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1/m + rsL2; % the intial guess of the Lipschitz continuous gradient

    weighty=weight.*y;
    % the product between weight and y

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,1);
    cp=c;         ccp=0;

    %% The Armijo Goldstein line search schemes + accelearted gradient descent

    alphap=0; alpha=1;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    s=x + beta* xxp;  sc=c + beta* ccp;

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute As=A*s
        As=Ax + beta* (Ax-Axp);

        % aa= - diag(y) * (A * s + sc)
        aa=- y.*(As+ sc);

        % fun_s is the logistic loss at the search point
        bb=max(aa,0);
        fun_s= weight' * ( log( exp(-bb) +  exp(aa-bb) ) + bb )+...
            rsL2/2 * s'*s;

        % compute prob=[p_1;p_2;...;p_m]
        prob=1./( 1+ exp(aa) );

        % b= - diag(y.* weight) * (1 - prob)
        b= -weighty.*(1-prob);

        gc=sum(b); % the gradient of c

        % compute g= AT b, the gradient of x
        if (opts.nFlag==0)
            g=A'*b;
        elseif (opts.nFlag==1)
            g=A'*b - sum(b) * mu';  g=g./nu;
        else
            invNu=b./nu;              g=A'*invNu-sum(invNu)*mu';
        end

        g=g+ rsL2 * s;  % add the squared L2 norm regularization term

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;
        cp=c;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the Lq/L1-norm regularized projection
            v=s-g/L;  c= sc- gc/L;

            % projection
            [x, lambda, zf_step]=eplb(v, n, z, lambda0);
            lambda0=lambda;

            v=x-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute A x
            if (opts.nFlag==0)
                Ax=A* x;
            elseif (opts.nFlag==1)
                invNu=x./nu; mu_invNu=mu * invNu;
                Ax=A*invNu -repmat(mu_invNu, m, 1);
            else
                Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
            end

            % aa= - diag(y) * (A * x + c)
            aa=- y.*(Ax+ c);

            % fun_x is the logistic loss at the new approximate solution
            bb=max(aa,0);
            fun_x= weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb )+...
                rsL2/2 * x'*x;

            r_sum= (v'*v + (c-sc)^2) / 2;
            l_sum=fun_x - fun_s - v'* g - (c-sc)* gc;

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is fun_x <= fun_s + v'* g + c * gc
            %                           + L/2 * (v'*v + (c-sc)^2 )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                %fprintf('\n L=%e, r_sum=%e',L, r_sum);
            end
        end

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        ValueL(iterStep)=L;
        % store values for L

        xxp=x-xp;    ccp=c-cp;
        funVal(iterStep)=fun_x;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp);    norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end

%% adaptive line search

if (opts.lFlag==1)

    lambda0=0;
    % a guess of the root

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1/m + rsL2; % the intial guess of the Lipschitz continuous gradient

    gamma=1;
    % we shall set the value of gamma = L,
    % and L is appropriate for the starting point

    weighty=weight.*y;
    % the product between weight and y

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,1);
    cp=c;         ccp=0;

    for iterStep=1:opts.maxIter
        while (1)
            if (iterStep~=1)
                alpha= (-gamma+ sqrt(gamma*gamma + 4* L * gamma)) / (2*L);
                beta= (gamma - gamma* alphap) / (alphap * gamma + alphap* L * alpha);
                % beta is the coefficient for generating search point s

                s=x + beta* xxp;  sc=c + beta* ccp;
                As=Ax + beta* (Ax-Axp);
            else
                alpha= (-1+ sqrt(5)) / 2;  beta=0;
                s=x; sc= c;
                As=Ax;
            end

            % aa= - diag(y) * (A * s + sc)
            aa=- y.*(As+ sc);

            % fun_s is the logistic loss at the search point
            bb=max(aa,0);
            fun_s= weight' * ( log( exp(-bb) +  exp(aa-bb) ) + bb )+...
                rsL2/2 * s'*s;

            % compute prob=[p_1;p_2;...;p_m]
            prob=1./( 1+ exp(aa) );

            % b= - diag(y.* weight) * (1 - prob)
            b= -weighty.*(1-prob);

            gc=sum(b); % the gradient of c

            % compute g= AT b, the gradient of x
            if (opts.nFlag==0)
                g=A'*b;
            elseif (opts.nFlag==1)
                g=A'*b - sum(b) * mu';  g=g./nu;
            else
                invNu=b./nu;              g=A'*invNu-sum(invNu)*mu';
            end

            g=g+ rsL2 * s;  % add the squared L2 norm regularization term

            % let s walk in a step in the antigradient of s to get v
            % and then do the Lq/L1-norm regularized projection
            v=s-g/L;  cnew= sc- gc/L;

            % projection
            [xnew, lambda, zf_step]=eplb(v, n, z, lambda0);
            lambda0=lambda;

            v=xnew-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute A xnew
            if (opts.nFlag==0)
                Axnew=A* xnew;
            elseif (opts.nFlag==1)
                invNu=xnew./nu; mu_invNu=mu * invNu;
                Axnew=A*invNu -repmat(mu_invNu, m, 1);
            else
                Axnew=A*xnew-repmat(mu*xnew, m, 1);     Axnew=Axnew./nu;
            end

            % aa= - diag(y) * (A * x + c)
            aa=- y.*(Axnew+ cnew);

            % fun_x is the logistic loss at the new approximate solution
            bb=max(aa,0);
            fun_x= weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb )+...
                rsL2/2 * xnew'*xnew;

            r_sum= (v'*v + (cnew-sc)^2) / 2;
            l_sum=fun_x - fun_s - v'* g - (cnew-sc)* gc;

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is fun_x <= fun_s + v'* g + c * gc
            %                           + L/2 * (v'*v + (c-sc)^2 )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                %fprintf('\n L=%e, r_sum=%e',L, r_sum);
            end
        end

        gamma=L* alpha* alpha;    alphap=alpha;
        % update gamma, and alphap

        ValueL(iterStep)=L;
        % store values for L

        tao=L * r_sum / l_sum;
        if (tao >=5)
            L=L*0.8;
        end
        % decrease the value of L

        xp=x;     x=xnew; xxp=x-xp;
        Axp=Ax;   Ax=Axnew;
        cp=c;     c=cnew; ccp=c-cp;
        % update x and Ax with xnew and Axnew

        funVal(iterStep)=fun_x;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp);    norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end


%P2.9a
%T1[x(n)]=2^x(n)
n=[0:200];x=cos(0.2*pi*n)+0.5*cos(0.6*pi*n);
t1=2.^x
[x1,n1]=sigshift(t1,n,50); %x(n-k) -> k=50
[x2,n2]=sigfold(t1,n); %x(-n)
[x3,n3]=sigfold(x1,n1); %x(k-n)
[rxx,nrxx]=conv_m(t1,n,x2,n2);
[rxk,nrkx]=conv_m(x1,n1,x3,n3);
[y,m]=sigadd(rxx,nrxx,0.1*rxk,nrkx);
figure(1);
subplot(3,1,1); stem(m,y,'k');title('Autocorrelation ryy');xlabel('m');

%T2[x(n)]=3x(n)+4
n=[0:200];x=cos(0.2*pi*n)+0.5*cos(0.6*pi*n);
t2=3*x+4
[x1,n1]=sigshift(t2,n,50); %x(n-k) -> k=50
[x2,n2]=sigfold(t2,n); %x(-n)
[x3,n3]=sigfold(x1,n1); %x(k-n)
[rxx,nrxx]=conv_m(t2,n,x2,n2);
[rxk,nrkx]=conv_m(x1,n1,x3,n3);
[y,m]=sigadd(rxx,nrxx,0.1*rxk,nrkx);
subplot(3,1,2); stem(m,y,'k');title('Autocorrelation ryy');xlabel('m');

%T3[x(n)]=x(n)+2x(n-1)-x(n-2)
n=[0:200];x=cos(0.2*pi*n)+0.5*cos(0.6*pi*n);
[xa,nxa]=sigshift(x,n,1);
[xb,nxb]=sigshift(x,n,2);
[xa,nxa]=sigadd(2*xa,nxa,-1*xb,nxb);
[t3,n]=sigadd(x,n,xa,nxa)
[x1,n1]=sigshift(t3,n,50); %x(n-k) -> k=50
[x2,n2]=sigfold(t3,n); %x(-n)
[x3,n3]=sigfold(x1,n1); %x(k-n)
[rxx,nrxx]=conv_m(t3,n,x2,n2);
[rxk,nrkx]=conv_m(x1,n1,x3,n3);
[y,m]=sigadd(rxx,nrxx,0.1*rxk,nrkx);
subplot(3,1,3); stem(m,y,'k');title('Autocorrelation ryy');xlabel('m');

%P2.9b
%T1[x(n)]=2^x(n)
%x(n)=random sequence
nrand=[0:100];xrand=rand(size(nrand));
t1=2.^xrand
[x1,n1]=sigshift(t1,n,50); %x(n-k) k=50
[x2,n2]=sigfold(t1,n); %x(-n)
[x3,n3]=sigfold(x1,n1); %x(k-n)
[rxx,nrxx]=conv_m(t1,n,x2,n2);
[rxk,nrkx]=conv_m(x1,n1,x3,n3);
[y,m]=sigadd(rxx,nrxx,0.1*rxk,nrkx);
figure(2);
subplot(3,1,1); stem(m,y,'k');title('Autocorrelation ryy');xlabel('m');

%T2[x(n)]=3x(n)+4
%x(n)=random sequence
nrand=[0:100];xrand=rand(size(nrand));
t2=3*xrand+4
[x1,n1]=sigshift(t2,n,50); %x(n-k) -> k=50
[x2,n2]=sigfold(t2,n); %x(-n)
[x3,n3]=sigfold(x1,n1); %x(k-n)
[rxx,nrxx]=conv_m(t2,n,x2,n2);
[rxk,nrkx]=conv_m(x1,n1,x3,n3);
[y,m]=sigadd(rxx,nrxx,0.1*rxk,nrkx);
subplot(3,1,2); stem(m,y,'k');title('Autocorrelation ryy');xlabel('m');

%T3[x(n)]=x(n)+2x(n-1)-x(n-2)
%x(n)=Random Sequence
nrand=[0:100];xrand=rand(size(nrand));
[xa,nxa]=sigshift(xrand,nrand,1);
[xb,nxb]=sigshift(xrand,nrand,2);
[xa,nxa]=sigadd(2*xa,nxa,-1*xb,nxb);
[t3,n]=sigadd(xrand,nrand,xa,nxa);
[x1,n1]=sigshift(t3,n,50); %x(n-k) -> k=50
[x2,n2]=sigfold(t3,n); %x(-n)
[x3,n3]=sigfold(x1,n1); %x(k-n)
[rxx,nrxx]=conv_m(t3,n,x2,n2);
[rxk,nrkx]=conv_m(x1,n1,x3,n3);
[y,m]=sigadd(rxx,nrxx,0.1*rxk,nrkx);
subplot(3,1,3); stem(m,y,'k');title('Autocorrelation ryy');xlabel('m');

%P2.9c
%T1[x(n)]=2^x(n)
%x(n)=Gaussian random sequence
nrand=[0:100];xrand=randn(size(nrand))
t1=2.^xrand
[x1,n1]=sigshift(t1,n,50); %x(n-k) k=50
[x2,n2]=sigfold(t1,n); %x(-n)
[x3,n3]=sigfold(x1,n1); %x(k-n)
[rxx,nrxx]=conv_m(t1,n,x2,n2);
[rxk,nrkx]=conv_m(x1,n1,x3,n3);
[y,m]=sigadd(rxx,nrxx,0.1*rxk,nrkx);
figure(3);
subplot(3,1,1); stem(m,y,'k');title('Autocorrelation ryy');xlabel('m');

%T2[x(n)]=3x(n)+4
%x(n)=Gaussian random sequence
nrand=[0:100];xrand=randn(size(nrand))
t2=3*xrand+4
[x1,n1]=sigshift(t2,n,50); %x(n-k) -> k=50
[x2,n2]=sigfold(t2,n); %x(-n)
[x3,n3]=sigfold(x1,n1); %x(k-n)
[rxx,nrxx]=conv_m(t2,n,x2,n2);
[rxk,nrkx]=conv_m(x1,n1,x3,n3);
[y,m]=sigadd(rxx,nrxx,0.1*rxk,nrkx);
subplot(3,1,2); stem(m,y,'k');title('Autocorrelation ryy');xlabel('m');

%T3[x(n)]=x(n)+2x(n-1)-x(n-2)
%x(n)=Gaussian Random Sequence
nrand=[0:100];xrand=randn(size(nrand));
[xa,nxa]=sigshift(xrand,nrand,1);
[xb,nxb]=sigshift(xrand,nrand,2);
[xa,nxa]=sigadd(2*xa,nxa,-1*xb,nxb);
[t3,n]=sigadd(xrand,nrand,xa,nxa);
[x1,n1]=sigshift(t3,n,50); %x(n-k) -> k=50
[x2,n2]=sigfold(t3,n); %x(-n)
[x3,n3]=sigfold(x1,n1); %x(k-n)
[rxx,nrxx]=conv_m(t3,n,x2,n2);
[rxk,nrkx]=conv_m(x1,n1,x3,n3);
[y,m]=sigadd(rxx,nrxx,0.1*rxk,nrkx);
subplot(3,1,3); stem(m,y,'k');title('Autocorrelation ryy');xlabel('m');


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

function [Eigen_Vector,Eigen_Value]=Find_K_Max_Gen_Eigen(Matrix1,Matrix2,Eigen_NUM)

[NN,NN]=size(Matrix1);
[V,S]=eig(Matrix1,Matrix2); %Note this is equivalent to; [V,S]=eig(St,SL); also equivalent to [V,S]=eig(Sn,St); %

S=diag(S);
[S,index]=sort(S);

Eigen_Vector=zeros(NN,Eigen_NUM);
Eigen_Value=zeros(1,Eigen_NUM);

p=NN;
for t=1:Eigen_NUM
    Eigen_Vector(:,t)=V(:,index(p));
    Eigen_Value(t)=S(p);
    p=p-1;
end


% ZCA & l1-norm operation
function output = whitening_norm(features)
    features = im2double(features);
%     output = features;
    output = zca(features);
    output = l1_norm(output);
%     output = l2_norm(output);
%     output = nuclear(output);
end

% l1-norm
function output = l1_norm(features)
    [h,w,c] = size(features);
    output = zeros(h, w);
    unit = 5;
    % padding
    pad = (unit-1)/2;
    feature_temp = zeros(h+unit-1, w+unit-1, c);
    feature_temp(1+pad:h+pad, 1+pad:w+pad, :) = features;
    for i=1+pad:h+pad
        for j=1+pad:w+pad
            temp = feature_temp(i-pad:i+pad, j-pad:j+pad, :);
            norm = sum(sum(sum(abs(temp),3)))/(unit*unit);
            output(i-pad, j-pad) = norm;
        end
    end
end

% l2-norm
function output = l2_norm(features)
    [h,w,c] = size(features);
    output = zeros(h, w);
    unit = 5;
    % padding
    pad = (unit-1)/2;
    feature_temp = zeros(h+unit-1, w+unit-1, c);
    feature_temp(1+pad:h+pad, 1+pad:w+pad, :) = features;
    for i=1+pad:h+pad
        for j=1+pad:w+pad
            temp = feature_temp(i-pad:i+pad, j-pad:j+pad, :);
            temp = temp.*temp;
            norm = sum(sum(sqrt(sum(temp,3))))/(unit*unit);
            output(i-pad, j-pad) = norm;
        end
    end
end

% nuclear-norm
function output = nuclear(features)
    [h,w,c] = size(features);
    output = zeros(h, w);
    unit = 5;
    % padding
    pad = (unit-1)/2;
    feature_temp = zeros(h+unit-1, w+unit-1, c);
    feature_temp(1+pad:h+pad, 1+pad:w+pad, :) = features;
    for i=1+pad:h+pad
        for j=1+pad:w+pad
            temp = reshape(feature_temp(i-pad:i+pad, j-pad:j+pad, :), [unit*unit c]);
            [U, S, V] = svd(temp, 'econ');
            nu_norm = sum(diag(S));
            output(i-pad, j-pad) = nu_norm;
        end
    end

end


function fem1d_heat_explicit_test01 ( )

%*****************************************************************************80
%
%% FEM1D_HEAT_EXPLICIT_TEST01 runs a simple test.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_HEAT_EXPLICIT_TEST01:\n' );
  fprintf ( 1, '  The time dependent 1D heat equation is\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Ut - k * Uxx = f(x,t)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  for space interval A <= X <= B with boundary conditions\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    U(A,t) = UA(t)\n' );
  fprintf ( 1, '    U(B,t) = UB(t)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  and time interval T0 <= T <= T1 with initial condition\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    U(X,T0) = U0(X).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  To compute an approximate solution:\n' );
  fprintf ( 1, '    the interval [A,B] is replace by a discretized mesh Xi;\n' );
  fprintf ( 1, '    a set of finite element functions PSI(X) are determined,\n' );
  fprintf ( 1, '    the solution U is written as a weighted sum of the basis functions,\n' );
  fprintf ( 1, '    the weak form of the differential equation is written,\n' );
  fprintf ( 1, '    a time grid Tj is imposed, and time derivatives replaced by\n' );
  fprintf ( 1, '    an explicit forward Euler first difference,\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    The continuous PDE has now been transformed into a set of algebraic\n' );
  fprintf ( 1, '    equations for the coefficients C(Xi,Tj).\n' );
%
%  Set the nodes.
%
  x_num = 21;
  x_min = 0.0;
  x_max = 1.0;
  dx = ( x_max - x_min ) / ( x_num - 1 );
  x = ( linspace ( x_min, x_max, x_num ) )';
%
%  Set the times.
%
  t_num = 401;
  t_min = 0.0;
  t_max = 80.0;
  dt = ( t_max - t_min ) / ( t_num - 1 );
  t = ( linspace ( t_min, t_max, t_num ) )';
%
%  Set finite element information.
%
  element_num = x_num - 1;
  element_node(1,1:element_num) = 1 : x_num - 1;
  element_node(2,1:element_num) = 2 : x_num;
  quad_num = 3;
  mass = assemble_mass ( x_num, x, element_num, element_node, quad_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of X nodes = %d\n', x_num );
  fprintf ( 1, '  X interval = [ %f, %f ]\n', x_min, x_max );
  fprintf ( 1, '  X step size = %f\n', dx );
  fprintf ( 1, '  Number of T steps = %d\n', t_num );
  fprintf ( 1, '  T interval = [ %f, %f ]\n', t_min, t_max );
  fprintf ( 1, '  T step size = %f\n', dt );
  fprintf ( 1, '  Number of elements = %d\n', element_num );
  fprintf ( 1, '  Number of quadrature points = %d\n', quad_num );

  u_mat = zeros ( x_num, t_num );

  for j = 1 : t_num

    if ( j == 1 )

      u(1:x_num,1) = ic_test01 ( x_num, x, t(j) );
      u(1:x_num,1) = bc_test01 ( x_num, x, t(j), u(1:x_num,1) );

    else

      u(1:x_num,1) = fem1d_heat_explicit ( x_num, x, t(j-1), dt, @k_test01, ...
        @rhs_test01, @bc_test01, element_num, element_node, quad_num, mass, u );

    end

    u_mat(1:x_num,j) = u(1:x_num,1);

  end
%
%  Make a product grid of T and X for plotting.
%
  [ t_mat, x_mat ] = meshgrid ( t, x );
%
%  Make a mesh plot of the solution.
%
  mesh ( t_mat, x_mat, u_mat )
  xlabel ( '<--Time-->' )
  ylabel ( '<--X-->' )
  zlabel ( '<--H(X,T)-->' )
  title ( 'H(X,T) for TEST01, by FEM1D\_HEAT\_EXPLICIT' )
%
%  Write the data to files.
%
  r8mat_write ( 'h_test01.txt', x_num, t_num, u_mat );
  r8vec_write ( 't_test01.txt', t_num, t );
  r8vec_write ( 'x_test01.txt', x_num, x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  H(X,T) written to "h_test01.txt"\n' );
  fprintf ( 1, '  T values written to "t_test01.txt"\n' );
  fprintf ( 1, '  X values written to "x_test01.txt"\n' );

  return
end


function [X, info] = dominant_invariant_subspace(A, p)
% Returns an orthonormal basis of the dominant invariant p-subspace of A.
%
% function X = dominant_invariant_subspace(A, p)
%
% Input: A real, symmetric matrix A of size nxn and an integer p < n.
% Output: A real, orthonormal matrix X of size nxp such that trace(X'*A*X)
%         is maximized. That is, the columns of X form an orthonormal basis
%         of a dominant subspace of dimension p of A. These are thus
%         eigenvectors associated with the largest eigenvalues of A (in no
%         particular order). Sign is important: 2 is deemed a larger
%         eigenvalue than -5.
%
% The optimization is performed on the Grassmann manifold, since only the
% space spanned by the columns of X matters. The implementation is short to
% show how Manopt can be used to quickly obtain a prototype. To make the
% implementation more efficient, one might first try to use the caching
% system, that is, use the optional 'store' arguments in the cost, grad and
% hess functions. Furthermore, using egrad2rgrad and ehess2rhess is quick
% and easy, but not always efficient. Having a look at the formulas
% implemented in these functions can help rewrite the code without them,
% possibly more efficiently.
%
% See also: dominant_invariant_subspace_complex

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, July 5, 2013
% Contributors:
%
% Change log:
%
%   NB Dec. 6, 2013:
%       We specify a max and initial trust region radius in the options.

    % Generate some random data to test the function
    if ~exist('A', 'var') || isempty(A)
        A = randn(128);
        A = (A+A')/2;
    end
    if ~exist('p', 'var') || isempty(p)
        p = 3;
    end

    % Make sure the input matrix is square and symmetric
    n = size(A, 1);
	assert(isreal(A), 'A must be real.')
    assert(size(A, 2) == n, 'A must be square.');
    assert(norm(A-A', 'fro') < n*eps, 'A must be symmetric.');
	assert(p<=n, 'p must be smaller than n.');

    % Define the cost and its derivatives on the Grassmann manifold
    Gr = grassmannfactory(n, p);
    problem.M = Gr;
    problem.cost = @(X)    -trace(X'*A*X);
    problem.grad = @(X)    -2*Gr.egrad2rgrad(X, A*X);
    problem.hess = @(X, H) -2*Gr.ehess2rhess(X, A*X, A*H, H);

    % Execute some checks on the derivatives for early debugging.
    % These can be commented out.
    % checkgradient(problem);
    % pause;
    % checkhessian(problem);
    % pause;

    % Issue a call to a solver. A random initial guess will be chosen and
    % default options are selected except for the ones we specify here.
    options.Delta_bar = 8*sqrt(p);
    [X, costX, info, options] = trustregions(problem, [], options); %#ok<ASGLU>

    fprintf('Options used:\n');
    disp(options);

    % For our information, Manopt can also compute the spectrum of the
    % Riemannian Hessian on the tangent space at (any) X. Computing the
    % spectrum at the solution gives us some idea of the conditioning of
    % the problem. If we were to implement a preconditioner for the
    % Hessian, this would also inform us on its performance.
    %
    % Notice that (typically) all eigenvalues of the Hessian at the
    % solution are positive, i.e., we find an isolated minimizer. If we
    % replace the Grassmann manifold by the Stiefel manifold, hence still
    % optimizing over orthonormal matrices but ignoring the invariance
    % cost(XQ) = cost(X) for all Q orthogonal, then we see
    % dim O(p) = p(p-1)/2 zero eigenvalues in the Hessian spectrum, making
    % the optimizer not isolated anymore.
    if Gr.dim() < 512
        evs = hessianspectrum(problem, X);
        stairs(sort(evs));
        title(['Eigenvalues of the Hessian of the cost function ' ...
               'at the solution']);
        xlabel('Eigenvalue number (sorted)');
        ylabel('Value of the eigenvalue');
    end

end


function [ n_data, n, c ] = mertens_values ( n_data )

%*****************************************************************************80
%
%% MERTENS_VALUES returns some values of the Mertens function.
%
%  Discussion:
%
%    The Mertens function M(N) is the sum from 1 to N of the Moebius
%    function MU.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    M Deleglise, J Rivat,
%    Computing the Summation of the Moebius Function,
%    Experimental Mathematics,
%    Volume 5, 1996, pages 291-295.
%
%    Eric Weisstein,
%    CRC Concise Encyclopedia of Mathematics,
%    CRC Press, 2002,
%    Second edition,
%    ISBN: 1584883472,
%    LC: QA5.W45.
%
%  Parameters:
%
%    Input, integer N_DATA, indicates the index of the previous test data
%    returned, or is 0 if this is the first call.  For repeated calls,
%    set the input value of N_DATA to the output value of N_DATA
%    from the previous call.
%
%    Output, integer N_DATA, the index of the test data.
%
%    Output, integer N, the argument of the Mertens function.
%
%    Output, integer C, the value of the Mertens function.
%
  n_max = 15;
  c_vec = [ ...
      1,   0,  -1,   -1,  -2,  -1,  -2,  -2,   -2,  -1, ...
     -2,  -2,   1,    2, -23 ];
  n_vec = [ ...
      1,   2,   3,   4,   5,   6,   7,   8,   9,  10, ...
     11,  12,  100, 1000, 10000 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    c = 0;
  else
    n = n_vec(n_data);
    c = c_vec(n_data);
  end

  return
end


function X=get_X_from_xP_lin(xn,Pcomp)
%get_X_from_xP_lin  Estimation of 3D point from image matches and camera matrices, linear.
%  return a 3D point X (column 4-vector, homogeneous, last element 1)
%   from its projections in K images xn (2-by-K matrix) and camera matrices
% P (K-cell of 3-by-4 matrices) by minimizing algebraic distance.
%   xn 3 x K matrix, homogeneous normalized image coordinates,
% last rows of 1, the other values usu. falls between -0.5 to 0.5
K=size(xn,2);
A = zeros(2*K, 4);
for k = 1:K
  A(2*k+(-1:0),:)=[xn(1,k)*Pcomp(3,:,k)-Pcomp(1,:,k); xn(2,k)*Pcomp(3,:,k)-Pcomp(2,:,k)];
end
% A = normx(A')';
[dummy,dummy,X] = svd(A,0);
X = X(:,end);
X=X/X(4);


function xy = perform_classical_mds(D,ndims)

N = size(D,1);
opt.disp = 0; opt.isreal = 1; opt.issym = 1;
M = -.5*(D.^2 - sum(D.^2)'*ones(1,N)/N - ones(N,1)*sum(D.^2)/N + sum(sum(D.^2))/(N^2));
[xy, val] = eigs(M, ndims, 'LR', opt);

for i=1:ndims
    xy(:,i) = xy(:,i)*sqrt(val(i,i));
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

%problem 2 - Fourier Transform of sinc(t)

syms t w
x=sin(pi*t)/(pi*t);
X=fourier(x,w);
ezplot(X, [-10 10])
legend('X(\Omega)');
xlabel('\Omega');


function [ H, Lh ] = homographyHarker( DataA, DataB, LA, LB)
%
% Purpose : Computes the general projective transformation between two sets
% of 2D data using a linear algorithm (the homography).
%
% Uses (syntax) :
%   H = homography( DataA, DataB )
%
% Input Parameters :
%   DataA, DataB := 2D homogeneous data sets in matrix form (3xn)
%
% Return Parameters :
%   H := the 3x3 homography
%
% Description and algorithms:
%   The algorithm is based on the Direct Linear Transform (DLT) method
%   outlined in Hartley et al.  The method uses orthogonal projections of
%   matrices, such that the vanishing line is treated as the principal
%   component of the reduction.  In this manner, the statistical behaviour
%   of the errors in variables are treated uniformly, see Harker and
%   O'Leary 2005.
%
% References :
%   Harker, M., O'Leary, P., Computation of Homographies, to appear in
%   Proceedings of the British Machine Vision Conference 2005, Oxford,
%   England.
%   Hartley, R., Zisserman, A., Multiple View Geometry in Computer Vision,
%   Cambridge University Press, Cambridge, 2001
%
% Cite this as :
%
% Author : Matthew Harker
% Date : July 25, 2005
% Version : 1.0
%--------------------------------------------------------------------------
% (c) 2005, O'Leary, Harker, University of Leoben, Leoben, Austria
% email: automation@unileoben.ac.at, url: automation.unileoben.ac.at
%--------------------------------------------------------------------------
% History:
%   Date:           Comment:
%   July 25, 2005   Original Version 1.0
%--------------------------------------------------------------------------
%
% Check input parameters:
%
if nargin == 2
    %
    if ~is2DData( DataA ) | ~is2DData( DataB )
        %
        error('Input does not represent 2D data') ;
        %
    end
    %
    [mA,nA] = size( DataA ) ;
    [mB,nB] = size( DataB ) ;
    %
    if (nA ~= nB)
        %
        error('Data sets must be the same size') ;
        %
    end
    %
    %
elseif nargin == 4
    %
    if ~is2dcovdata( DataA, LA ) | ~is2dcovdata( DataB, LB )
        %
        error('Input data does not match') ;
        %
    end
    %
    [mA,nA] = size( DataA ) ;
    [mB,nB] = size( DataB ) ;
    %
    if (nA ~= nB)
        %
        error('Data sets must be the same size') ;
        %
    end
    %
    %
    error('Error propagation not implemented as of yet') ;
    %
else
    %
    error('Incorrect number of input parameters') ;
    %
end
%
% Normalize the input data:
%
if nargin == 2
    %
    [DataA,TA,TAi] = normalizeData( DataA ) ;
    [DataB,TB,TBi] = normalizeData( DataB ) ;
    %
elseif nargin == 4
    %
    [DataA,TA,TAi,LA] = normalizeData( DataA, LA ) ;
    [DataB,TB,TBi,LB] = normalizeData( DataB, LB ) ;
    %
end
%
% Construct the orthogonalized design matrix :
%
C1 = -DataB(1,:) .* DataA(1,:) ;
C2 = -DataB(1,:) .* DataA(2,:) ;
%
C3 = -DataB(2,:) .* DataA(1,:) ;
C4 = -DataB(2,:) .* DataA(2,:) ;
%
mC1 = mean( C1 ) ;
mC2 = mean( C2 ) ;
mC3 = mean( C3 ) ;
mC4 = mean( C4 ) ;
%
Mx = [ C1' - mC1, C2' - mC2, -DataB(1,:)' ] ;
My = [ C3' - mC3, C4' - mC4, -DataB(2,:)' ] ;
%
Pp = pinv(DataA(1:2,:)') ;
%
Bx = Pp * Mx ;
By = Pp * My ;
%
D = [ Mx - DataA(1:2,:)'*Bx ;...
      My - DataA(1:2,:)'*By ] ;
%
% Find v_min and backsubstitute :
%
[U,S,V] = svd( D, 0 ) ;
%
h789 = V(:,end) ;
h12 = -Bx * h789 ;
h45 = -By * h789 ;
h3 = -[mC1, mC2] * h789(1:2) ;
h6 = -[mC3, mC4] * h789(1:2) ;
%
% Reshape vector h to matrix H, and transform :
%
H = reshape( [h12; h3; h45; h6; h789], 3, 3)' ;
%
H = TB * H * TAi ;
%
% Compute the covariance of the output :
%
%
%
% END
%

function [DataN, T, Ti, LN] = normalizeData( Data, L ) ;
%
% Purpose : Computes a set of data corresponding to the input data with its
% centroid subtracted, and scaled such that the root-mean-square distance
% to the origin is sqrt(2).  The transformation T, carries the scaled data
% back to its original form.  Optionally, the first order estimation of
% covariance matrices are computed.
%
% Uses (syntax) :
%   [DataN, T, LN] = normalizeData( Data, L )
%   [DataN, T] = normalizeData( Data )
%
% Input Parameters :
%   Data := a 3xn matrix of homogeneous points.
%   L    := is a 3x3 covariance matrix (all points have identical covariance), or
%           a 3x3xn array of n covariance matrices.
%
% Return Parameters :
%   DataN := mean-free data scaled s.t. d_RMS = sqrt(2)
%   T     := transformation to bring DataN to the Affine coordinates
%            corresponding to Data (NOTE: T*DataN is in affine coords).
%   LN    := the covariance of the scaled normalized data (size is
%            generally 2x2xn, due to the normalization)
%
% Description and algorithms:
%
% References :
%   Clarke, J.C., Modelling Uncertainty: A Primer, Dept. of Engineering
%   Science, Oxford University, Technical Report.
%
% Cite this as :
%
% Author : Matthew Harker
% Date : July 7, 2005
% Version :
%
% (c) 2005, Institute for Automation, University of Leoben, Leoben, Austria
% email: automation@unileoben.ac.at, url: automation.unileoben.ac.at
%
% History:
%   Date:           Comment:
%                   Original Version 1.0
%--------------------------------------------------------------------------
%
% Check input arguments :
%
if nargin == 1
    %
    if ~is2DData( Data )
        %
        error('Input does not represent 2D data') ;
        %
    end
    %
elseif nargin == 2
    %
    if ~is2dcovdata( Data, L )
        %
        error('Input data and covariances do not match') ;
        %
    end
    %
else
    %
    error('Not enough input arguments') ;
    %
end
%
s = Data(1,:) ;
t = Data(2,:) ;
u = Data(3,:) ;
%
x = s./u ;
y = t./u ;
%
xm = mean( x ) ;
ym = mean( y ) ;
%
xh = x - xm ;
yh = y - ym ;
%
n = length( xh ) ;
%
kappa = sum( xh.^2 + yh.^2 ) ;
%
beta = sqrt( 2*n / kappa ) ;
%
xn = beta * xh ;
yn = beta * yh ;
%
DataN = [ xn; yn; ones(size(xn)) ] ;
%
T = [ 1/beta,   0   , xm ;...
        0   , 1/beta, ym ;...
        0   ,   0   ,  1 ] ;
%
Ti = [ beta ,  0   , -beta * xm ; ...
        0   , beta , -beta * ym ; ...
        0   ,  0   ,       1    ] ;
%
%
%
if nargin == 1
    %
    % Just the data is needed.
    %
    if nargout == 4
        warning('No covariance matrix for the input data given.') ;
        LN = [] ;
    end
    %
    %
    %
elseif nargin == 2
    %
    % There is a 3x3 homogeneous covariance matrix given
    %
    if nargout == 4
        %
        % The 2x2 covariance matrix of the Euclidean output data is required
        %
        dim = length( size( L ) ) ;
        %
        % dim == 2 --> all covariance matrices are assumed the same
        % dim == 3 --> each point has a given covariance
        %
        % DEHOMOGENIZE
        %
        for k = 1:n
            %
            %
            % Jacobian for dehomogenizing:
            Jd = ( 1/u(k)^2 ) * [ u(k),  0  , -s(k) ;...
                                   0  , u(k), -t(k) ] ;
            %
            %
            if dim == 2
                LN(:,:,k) = Jd * L * Jd' ;
            else
                LN(:,:,k) = Jd * L(:,:,k) * Jd' ;
            end
            %
            % NOTE: If all data has the same covariance, after
            % dehomogenizing they will generally all have different
            % covariance matrices (as implemented here).  Only if the data
            % is already affine, and the covariance matrix is Euclidean
            % will the "normalized" points all have the same covariance.
            %
        end
        %
        % SUBTRACT MEAN, SCALE:
        %
        % Jacobian for subtracting mean:
        Jm = [ (1-(1/n)),   0       , -1 ,  0 ;...
                  0     , (1-(1/n)) ,  0 , -1 ] ;
        %
        % Covariance of the mean value:
        Lm = (1/n^2) * [ sum(LN(1,1,:)), sum(LN(1,2,:)) ;...
                         sum(LN(2,1,:)), sum(LN(2,2,:)) ] ;
        %
        %
        Lkappa = 0 ;
        %
        for k = 1:n
            %
            % Subtract Mean Covariance
            %
            LN(:,:,k) = Jm * [ LN(:,:,k) , zeros(2,2) ; zeros(2,2) , Lm ] * Jm' ;
            %
            % Compute Covariance of 'kappa':
            %
            Jk = 2 * [ xh(k), yh(k) ] ;
            Lkappa = Lkappa + Jk*LN(:,:,k)*Jk' ;
            %
        end
        %
        %
        for k = 1:n
            %
            % Jacobian for scaling the data:
            %
            a = kappa^(-3/2) ;
            b = kappa^(-1/2) ;
            %
            Js = sqrt(2*n) * [ (b - a*xh(k)^2) , -a*xh(k)*yh(k)  , -(a*xh(k))/2 ;...
                                -a*xh(k)*yh(k) , (b - a*yh(k)^2) , -(a*yh(k))/2 ] ;
            %
            LN(:,:,k) = Js * [ LN(:,:,k), [0;0] ; [0,0] , Lkappa ] * Js' ;
            %
            %
            %
        end
        %
        %
        %
    elseif nargout == 3
        %
        warning('No covariance matrix has been returned') ;
        %
    end
    %
    %
end
%
% END
%


function dd=tritrig(a,b,c,A,B,C)
if A==90 | B==90
    'error, right angle can only be C'

end
x=0;
if C==90
    if a~=0 & b~=0
        c=sqrt(a^2+b^2);
        A=asind(a/c);
        B=asind(b/c);
        x=1;

    end
    if a~=0 & c~=0 & x==0
        b=sqrt(c^2-a^2);
        A=asind(a/c);
        B=asind(b/c);
        x=1;

    end
    if b~=0 & c~=0 & x==0
        a=sqrt(c^2-b^2);
        A=asind(a/c);
        B=asind(b/c);
        x=1;

    end
    if A~=0 & x==0
        B=90-A;
        if a~=0
            b=a/tand(A);
            c=a/sind(a);
            x=1;

        end
        if b~=0 & x==0
            a=tand(A)*b;
            c=b/cosd(A);
            x=1;

        end
        if c~=0 & x==0
            a=sind(A)*c;
            b=cosd(A)*c;
            x=1;

        end
    end
    if B~=0 & x==0
        A=90-B;
        if a~=0
            b=a/tand(A);
            c=a/sind(a);
            x=1;

        end
        if b~=0 & x==0
            a=tand(A)*b;
            c=b/cosd(A);
            x=1;

        end
        if c~=0 & x==0
            a=sind(A)*c;
            b=cosd(A)*c;
            x=1;

        end
    end
end

if a~=0 & b~=0 & c~=0 & x==0
    A=acosd((b^2+c^2-a^2)/2/b/c);
    B=asind(sind(A)*b/a);
    C=180-A-B;
    x=1;

end
if a~=0 & b~=0 & C~=0 & x==0
    c=sqrt(a^2+b^2-2*a*b*cosd(C));
    A=asind(sind(B)*a/b);
    B=asind(sind(A)*b/a);
    x=1;

end
if a~=0 & c~=0 & B~=0 & x==0
    b=sqrt(a^2+c^2-2*a*c*cosd(B));
    A=asind(sind(B)*a/b);
    C=180-B-A;
    x=1;

end
if b~=0 & c~=0 & A~=0 & x==0
    a=sqrt(b^2+c^2-2*b*c*cosd(A));
    B=asind(sind(A)*b/a);
    C=180-A-B;
    x=1;

end
if A~=0 & B~=0 & x==0
    C=180-A-B;
    if a~=0
        b=sind(B)*a/sind(A);
        c=sind(C)*a/sind(A);
        x=1;

    end
    if b~=0 & x==0
        a=sind(A)*b/sind(B);
        c=sind(C)*b/sind(B);
        x=1;

    end
    if c~=0 & x==0
        a=sind(A)*c/sind(C);
        b=sind(B)*c/sind(C);
        x=1;

    end
end
if A~=0 & C~=0 & x==0
    B=180-A-C;
    if a~=0
        b=sind(B)*a/sind(A);
        c=sind(C)*a/sind(A);
        x=1;

    end
    if b~=0 & x==0
        a=sind(A)*b/sind(B);
        c=sind(C)*b/sind(B);
        x=1;

    end
    if c~=0 & x==0
        a=sind(A)*c/sind(C);
        b=sind(B)*c/sind(C);
        x=1;

    end
end
if B~=0 & C~=0 & x==0
    A=180-B-C;
    if a~=0
        b=sind(B)*a/sind(A);
        c=sind(C)*a/sind(A);
        x=1;

    end
    if b~=0 & x==0
        a=sind(A)*b/sind(B);
        c=sind(C)*b/sind(B);
        x=1;

    end
    if c~=0 & x==0
        a=sind(A)*c/sind(C);
        b=sind(B)*c/sind(C);
        x=1;

    end
end
if x==0
    'not enough information was given'
else
a
b
c
A
B
C
end
%How to use tritrig:
%tritrig is in the form 'tritrig(a,b,c,A,B,C)' where a b c A B C are the
%angles and the lengths of a triangle. If your triangle has a right angle
%in it, it must be 'C' and the hypotenuse must be 'c'. The angles and
%lengths that you do not know must be typed as '0'
%Eg.
%tritrig(3,4,5,0,0,0)
%A =
%    36.8699
%B=
%    53.1301
%C=
%    90.0000


%MVDR_FF: Minimum Variance Distortionless Response Beamformer using
%Free-field Steering Vector
%--------------------
% Input parameters
%--------------------
%nChannels :   The steering vector, or relative transfer function [numChannels x numFreqs ]
%dMic : The distance between two microphones [a real positive value]
%DOA: The direction of arrival in degree [ 0~360 ]
%freqVec: The frequencies vector [ numFreqs]
%Phi_u: The PSD matrix of the interfering signal [numChannels x numChannels x numFreqs ]
%--------------------
% Output parameters
%--------------------
%H_mvdr: An mvdr beamformer [ numChannels x numFreqs ]
function H_mvdr = mvdrFF(nChannels, dMic, DOA, freqVec, Phi_u)

c = 340;
theta0 = DOA * pi / 180;
d = exp(-1j*[0:nChannels-1]'*2*pi*freqVec*cos(theta0)*dMic/c) ./ nChannels;  % steering vector

H_mvdr = zeros(nChannels, length(freqVec));
%Phi_u_inv = zeros(nChannels, nChannels);
for f = 1:length(freqVec)
   Phi_u_inv = pinv(Phi_u(:,:,f));
   df = d(:, f);
   H_mvdr(:, f) = (Phi_u_inv * df) / (df' * Phi_u_inv * df);
end

end


function [idx,D]=fastknnsearch(varargin)
% KNNSEARCH   Linear k-nearest neighbor (KNN) search
% IDX = knnsearch(Q,R,K) searches the reference data set R (n x d array
% representing n points in a d-dimensional space) to find the k-nearest
% neighbors of each query point represented by eahc row of Q (m x d array).
% The results are stored in the (m x K) index array, IDX.
%
% IDX = knnsearch(Q,R) takes the default value K=1.
%
% IDX = knnsearch(Q) or IDX = knnsearch(Q,[],K) does the search for R = Q.
%
% Rationality
% Linear KNN search is the simplest appraoch of KNN. The search is based on
% calculation of all distances. Therefore, it is normally believed only
% suitable for small data sets. However, other advanced approaches, such as
% kd-tree and delaunary become inefficient when d is large comparing to the
% number of data points. On the other hand, the linear search in MATLAB is
% relatively insensitive to d due to the vectorization. In  this code, the
% efficiency of linear search is further improved by using the JIT
% aceeleration of MATLAB. Numerical example shows that its performance is
% comparable with kd-tree algorithm in mex.
%
% See also, kdtree, nnsearch, delaunary, dsearch

% By Yi Cao at Cranfield University on 25 March 2008

% Example 1: small data sets
%{
R=randn(100,2);
Q=randn(3,2);
idx=knnsearch(Q,R);
plot(R(:,1),R(:,2),'b.',Q(:,1),Q(:,2),'ro',R(idx,1),R(idx,2),'gx');
%}

% Example 2: ten nearest points to [0 0]
%{
R=rand(100,2);
Q=[0 0];
K=10;
idx=knnsearch(Q,R,10);
r=max(sqrt(sum(R(idx,:).^2,2)));
theta=0:0.01:pi/2;
x=r*cos(theta);
y=r*sin(theta);
plot(R(:,1),R(:,2),'b.',Q(:,1),Q(:,2),'co',R(idx,1),R(idx,2),'gx',x,y,'r-','linewidth',2);
%}

% Example 3: cputime comparion with delaunay+dsearch I, a few to look up
%{
R=randn(10000,4);
Q=randn(500,4);
t0=cputime;
idx=knnsearch(Q,R);
t1=cputime;
T=delaunayn(R);
idx1=dsearchn(R,T,Q);
t2=cputime;
fprintf('Are both indices the same? %d\n',isequal(idx,idx1));
fprintf('CPU time for knnsearch = %g\n',t1-t0);
fprintf('CPU time for delaunay  = %g\n',t2-t1);
%}
% Example 4: cputime comparion with delaunay+dsearch II, lots to look up
%{
Q=randn(10000,4);
R=randn(500,4);
t0=cputime;
idx=knnsearch(Q,R);
t1=cputime;
T=delaunayn(R);
idx1=dsearchn(R,T,Q);
t2=cputime;
fprintf('Are both indices the same? %d\n',isequal(idx,idx1));
fprintf('CPU time for knnsearch = %g\n',t1-t0);
fprintf('CPU time for delaunay  = %g\n',t2-t1);
%}
% Example 5: cputime comparion with kd-tree by Steven Michael (mex file)
% kd-tree by Steven Michael
%{
Q=randn(10000,10);
R=randn(500,10);
t0=cputime;
idx=knnsearch(Q,R);
t1=cputime;
tree=kdtree(R);
idx1=kdtree_closestpoint(tree,Q);
t2=cputime;
fprintf('Are both indices the same? %d\n',isequal(idx,idx1));
fprintf('CPU time for knnsearch = %g\n',t1-t0);
fprintf('CPU time for delaunay  = %g\n',t2-t1);
%}

% Check inputs
[Q,R,K,fident] = parseinputs(varargin{:});

% Check outputs
nargoutchk(0,2);

% C2 = sum(C.*C,2)';
[N,M] = size(Q);
L=size(R,1);
idx = zeros(N,K);
D = idx;

if K==1
    % Loop for each query point
    for k=1:N
        d=zeros(L,1);
        for t=1:M
            d=d+(R(:,t)-Q(k,t)).^2;
        end
        if fident
            d(k)=inf;
        end
        [D(k),idx(k)]=min(d);
    end
else
    for k=1:N
        d=zeros(L,1);
        for t=1:M
            d=d+(R(:,t)-Q(k,t)).^2;
        end
        if fident
            d(k)=inf;
        end
        [s,t]=sort(d);
        idx(k,:)=t(1:K);
        D(k,:)=s(1:K);
    end
end
if nargout>1
    D=sqrt(D);
end

function [Q,R,K,fident] = parseinputs(varargin)
% Check input and output
narginchk(1,3);

Q=varargin{1};

if nargin<2
    R=Q;
    fident = true;
else
    fident = false;
    R=varargin{2};
end

if isempty(R)
    fident = true;
    R=Q;
end

if ~fident
    fident = isequal(Q,R);
end

if nargin<3
    K=1;
else
    K=varargin{3};
end


function centroid = voronoi_polygon_centroid ( node, neighbor_num, ...
  neighbor_index, node_num, node_xy )

%*****************************************************************************80
%
%% VORONOI_POLYGON_CENTROID computes the centroid of a Voronoi polygon.
%
%  Discussion:
%
%    It is assumed that the Voronoi polygon is finite!  Every Voronoi
%    diagram includes some regions which are infinite, and for those,
%    this formula is not appropriate.
%
%    The routine is given the indices of the nodes that are neighbors of a
%    given "center" node.  A node is a neighbor of the center node if the
%    Voronoi polygons of the two nodes share an edge.  The triangles of the
%    Delaunay triangulation are formed from successive pairs of these neighbor
%    nodes along with the center node.
%
%    The assumption that the polygon is a Voronoi polygon is
%    used to determine the location of the boundaries of the polygon,
%    which are the perpendicular bisectors of the lines connecting
%    the center point to each of its neighbors.
%
%    The finiteness assumption is employed in part in the
%    assumption that the polygon is bounded by the finite
%    line segments from point 1 to 2, 2 to 3, ...,
%    M-1 to M, and M to 1, where M is the number of neighbors.
%
%    It is assumed that this subroutine is being called by a
%    process which has computed the Voronoi diagram of a large
%    set of nodes, so the arrays X and Y are dimensioned by
%    NODE_NUM, which may be much greater than the number of neighbor
%    nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Atsuyuki Okabe, Barry Boots, Kokichi Sugihara, Sung Nok Chiu,
%    Spatial Tessellations: Concepts and Applications of Voronoi Diagrams,
%    Second Edition,
%    Wiley, 2000, page 490.
%
%  Parameters:
%
%    Input, integer NODE, the index of the node whose Voronoi
%    polygon is to be analyzed.  1 <= NODE <= NODE_NUM.
%
%    Input, integer NEIGHBOR_NUM, the number of neighbor nodes of
%    the given node.
%
%    Input, integer NEIGHBOR_INDEX(NEIGHBOR_NUM), the indices
%    of the neighbor nodes.  These indices are used to access the
%    X and Y arrays.  The neighbor nodes should be listed in the
%    (counter-clockwise) order in which they occur as one circles
%    the center node.
%
%    Input, integer NODE_NUM, the total number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the nodes.
%
%    Output, real CENTROID(2), the coordinates of the centroid
%    of the Voronoi polygon of node NODE.
%
  dim_num = 2;

  centroid(1:dim_num,1) = 0.0;

  if ( node < 1 | node_num < node )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'VORONOI_POLYGON_CENTROID - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of input parameter NODE.\n' );
    error ( 'VORONOI_POLYGON_CENTROID - Fatal error!' )
  end

  pc(1:dim_num) = node_xy(1:dim_num,node)';

  i = neighbor_num;
  i = neighbor_index(i);

  pi(1:dim_num) = node_xy(1:dim_num,i)';

  j = 1;
  j = neighbor_index(j);

  pj(1:dim_num) = node_xy(1:dim_num,j)';

  a = ( pi(1) * pi(1) + pi(2) * pi(2) - pc(1) * pc(1) - pc(2) * pc(2) );
  b = ( pj(1) * pj(1) + pj(2) * pj(2) - pc(1) * pc(1) - pc(2) * pc(2) );
  c = 2.0 * ( ( pi(1) - pc(1) ) * ( pj(2) - pc(2) ) ...
            - ( pj(1) - pc(1) ) * ( pi(2) - pc(2) ) );
  uj(1) = ( a * ( pj(2) - pc(2) ) - b * ( pi(2) - pc(2) )  ) / c;
  uj(2) = ( a * ( pj(1) - pc(1) ) - b * ( pi(1) - pc(1) )  ) / c;

  for i = 1 : neighbor_num

    pi(1:dim_num) = pj(1:dim_num);
    ui(1:dim_num) = uj(1:dim_num);

    j = i4_wrap ( i+1, 1, neighbor_num );
    pj(1:dim_num) = node_xy(1:dim_num,j)';

    a = ( pi(1) * pi(1) + pi(2) * pi(2) - pc(1) * pc(1) - pc(2) * pc(2) );
    b = ( pj(1) * pj(1) + pj(2) * pj(2) - pc(1) * pc(1) - pc(2) * pc(2) );
    c = 2.0 * ( ( pi(1) - pc(1) ) * ( pj(2) - pc(2) ) ...
              - ( pj(1) - pc(1) ) * ( pi(2) - pc(2) ) );
    uj(1) = ( a * ( pj(2) - pc(2) ) - b * ( pi(2) - pc(2) )  ) / c;
    uj(2) = ( a * ( pj(1) - pc(1) ) - b * ( pi(1) - pc(1) )  ) / c;

    centroid(1) = centroid(1) + ( ui(2) - uj(2) ) ...
      * ( ( uj(1) + ui(1) )^2 - uj(1) * ui(1) );
    centroid(2) = centroid(2) + ( ui(1) - uj(1) ) ...
      * ( ( uj(2) + ui(2) )^2 - uj(2) * ui(2) );

  end

  area = voronoi_polygon_area ( node, neighbor_num, neighbor_index, ...
    node_num, node_xy );

  centroid(1:dim_num) = centroid(1:dim_num) / ( 6.0 * area );

  return
end


function AIC=aic(nParameters, nObservations, Residuals)
%Computes Akaike information criterion for a model
%
%
%
% Implements http://en.wikipedia.org/wiki/Akaike_information_criterion

% HISTORY:
% ER wrote it 01/2010

AIC =2*(nParameters)+nObservations*(log(sum(Residuals.^2)/nObservations));


function [soln,eqn,info] = PoissonP2(node,elem,bdFlag,pde,option)
%% POISSONP2 Poisson equation: P2 quadratic element.
%
% u = PoissonP2(node,elem,bdFlag,pde,option) produces the quadratic
%   finite element approximation of the Poisson equation
%
%       -div(d*grad(u))=f  in \Omega, with
%       Dirichlet boundary condition u=g_D on \Gamma_D,
%       Neumann boundary condition   d*grad(u)*n=g_N on \Gamma_N,
%       Robin boundary condition     g_R*u + d*grad(u)*n=g_N on \Gamma _R
%
% [soln,eqn,info] = PoissonP2(node,elem,pde,bdFlag,option)
%
% The usage is the same as Poisson. Quadratic element on a triangle is
% summarized in PoissonP2femrate for detail.
%
%   Example
%     clear all
%     node = [0,0; 1,0; 1,1; 0,1];
%     elem = [2,3,1; 4,1,3];
%     for k = 1:3
%       [node,elem] = uniformrefine(node,elem);
%     end
%     % Homogenous Dirichlet boundary condition
%     pde.f = inline('ones(size(p,1),1)','p');
%     pde.g_D = 0;
%     u = PoissonP2(node,elem,[],pde);
%     figure(1);
%     showresult(node,elem,u);
%
% See also Poisson, Poisson3, Poisson3P2
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

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

%% Construct data structure
time = cputime;  % record assembling time
[elem2dof,edge,bdDof] = dofP2(elem);
% important constants
N = size(node,1);  NT = size(elem,1); NE = size(edge,1);
Ndof = N + NE;
elem2edge = elem2dof(:,4:6) - N;

%% Compute geometric quantities and gradient of local basis
[Dlambda,area] = gradbasis(node,elem);

%% Assemble stiffness matrix
% generate sparse pattern
ii = zeros(21*NT,1);
jj = zeros(21*NT,1);
index = 0;
for i = 1:6
    for j = i:6
        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        index = index + NT;
    end
end
% quadrature points
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'quadorder')
    % diffusion is piecewise constant
    option.quadorder = 2;        % default order
    if ~isempty(pde.d) && isnumeric(pde.d) % numerical diffusion
        option.quadorder = 3;    % exact for linear diffusion coefficient
    end
end
[lambda, w] = quadpts(option.quadorder);
nQuad = size(lambda,1);
% compute non-zeros
sA = zeros(21*NT,nQuad);
for p = 1:nQuad
    % Dphi at quadrature points
    Dphip(:,:,6) = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
    Dphip(:,:,1) = (4*lambda(p,1)-1).*Dlambda(:,:,1);
    Dphip(:,:,2) = (4*lambda(p,2)-1).*Dlambda(:,:,2);
    Dphip(:,:,3) = (4*lambda(p,3)-1).*Dlambda(:,:,3);
    Dphip(:,:,4) = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
    Dphip(:,:,5) = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
    index = 0;
    for i = 1:6
        for j = i:6
            Aij = 0;
            if isempty(pde.d) || isnumeric(pde.d)
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2);
            else
                pxy = lambda(p,1)*node(elem(:,1),:) ...
                    + lambda(p,2)*node(elem(:,2),:) ...
                    + lambda(p,3)*node(elem(:,3),:);
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2).*pde.d(pxy);
            end
            if ~isempty(pde.d) && isnumeric(pde.d) % d is piecewise constant
                Aij = pde.d.*Aij;
            end
            Aij = Aij.*area;
            sA(index+1:index+NT,p) = Aij;
            index = index + NT;
        end
    end
end
sA = sum(sA,2);
% assemble the matrix
diagIdx = (ii == jj);
upperIdx = ~diagIdx;
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
A = A + AU + AU';
clear Aij ii jj sA

%% Assemble right hand side by high order quadrature rule
b = zeros(Ndof,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 4;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    % quadrature points in the barycentric coordinate
    [lambda,w] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
    phi(:,6) = 4*lambda(:,1).*lambda(:,2);
    phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
    phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
    phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
    phi(:,4) = 4*lambda(:,2).*lambda(:,3);
    phi(:,5) = 4*lambda(:,3).*lambda(:,1);
    bt = zeros(NT,6);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        if isfield(pde,'f') && isnumeric(pde.f)
            fp = pde.f;        % piecewise constant
        else
            fp = pde.f(pxy);   % function handle
        end
        for j = 1:6
            bt(:,j) = bt(:,j) + w(p)*phi(p,j)*fp;
        end
    end
    bt = bt.*repmat(area,1,6);
    b = accumarray(elem2dof(:),bt(:),[Ndof 1]);
end

%% Boundary Conditions
[AD,b,u,freeDof,isPureNeumann] = getbdP2(b);

%% Record assembeling time
assembleTime = cputime - time;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(freeDof), return; end
% Set up solver
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % Multigrid-type  solver for large size systems
        option.solver = 'mg';
    end
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeDof) = AD(freeDof,freeDof)\b(freeDof);
        residual = norm(b - AD*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
        option.x0 = u;
        option.solver = 'CG';
        option.tol = 1e-9;
        [u,info] = mg(AD,b,elem,option,edge);
    case 'amg'
        option.solver = 'CG';
        option.tol = 1e-9;
        [u(freeDof),info] = amg(AD(freeDof,freeDof),b(freeDof),option);
end
% post-process for pure Neumann problem
if isPureNeumann
      patchArea = accumarray(elem2edge(:),[area;area;area]/3, [NE 1]);
             uc = sum(u(N+1:end).*patchArea)/sum(area);
              u = u - uc;   % normalization for pure Neumann problem
% Here we use 3 middle points rule to compute the integral which is exact
% for quadratic elements. We can also use the following formulae
%      intuh = sum(1/3*u(elem2dof(:,4:6)),2).*area; %Compute the integrable of uh
%         uc = sum(intuh)/sum(area);
%          u = u -uc;
end

%% Compute Du
Du = [];

%% Output
if nargout == 1
    soln = u;
else
    soln = struct('u',u,'Du',Du);
    eqn = struct('A',AD,'b',b,'edge',edge,'freeDof',freeDof,'Lap',A);
    info.assembleTime = assembleTime;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbdP2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,b,u,freeDof,isPureNeumann] = getbdP2(b)
    %% Boundary conditions for Poisson equation: P2 quadratic FEM.
    %
    % The set up of boundary condition consists of two parts:
    %
    % 1) Modify the matrix for Dirichlet boundary nodes, which are not degree
    % of freedom. Values at these nodes are evaluatation of pde.g_D. The
    % original stiffness matrix A is turn into the matrix AD by enforcing
    % AD(fixedDof,fixedDof)=I, AD(fixedDof,freeDof)=0, AD(freeDof,fixedDof)=0.
    %
    % 2) Modify the right hand side b. The Neumann boundary integral is added
    % to b. For Dirichlet boundary ndoes, b(fixedDof) is the evaluation of
    % pde.g_D.
    %
    % Special attentation should be given for the pure Neumann boundary
    % condition. To enforce the compatible condition, the vector b should have
    % mean value zero. To avoid a singular matrix, the 1st node is chosen as
    % fixedDof.
    %
    % The order of assigning Neumann and Dirichlet boundary condition is
    % important to get the right setting at the intersection nodes of Dirichlet
    % and Neumann boundary edges.

    u = zeros(Ndof,1);
    %% Set up boundary and basic parameter
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Modify the matrix for Dirichlet and Robin condition
    % Robin boundary condition
    Robin = [];
    idxR = (bdFlag(:) == 3);      % index of Robin edges in bdFlag
    if any(idxR)
        isRobin = false(NE,1);
        isRobin(elem2edge(idxR)) = true;
        Robin = edge(isRobin,:);  % Robin edges
    end
    if ~isempty(Robin) && ~isempty(pde.g_R) && ~(isnumeric(pde.g_R) && (pde.g_R == 0))
        if ~isfield(option,'gRquadorder')
            option.gRquadorder = 5;   % default order exact for linear gR
        end
        [lambdagR,weightgR] = quadpts1(option.gRquadorder);
        nQuadgR = size(lambdagR,1);
        % quadratic bases (1---3---2)
        bdphi = zeros(nQuadgR,3);
        bdphi(:,1) = (2*lambdagR(:,1)-1).*lambdagR(:,1);
        bdphi(:,2) = (2*lambdagR(:,2)-1).*lambdagR(:,2);
        bdphi(:,3) = 4*lambdagR(:,1).*lambdagR(:,2);
        % length of edge
        el = sqrt(sum((node(Robin(:,1),:) - node(Robin(:,2),:)).^2,2));
        NR = size(Robin,1);
        ss = zeros(NR,3,3);
        for pp = 1:nQuadgR
            ppxy = lambdagR(pp,1)*node(Robin(:,1),:) ...
                 + lambdagR(pp,2)*node(Robin(:,2),:);
            gRp = pde.g_R(ppxy);
            for iR = 1:3
                for jR = iR:3   % only compute half of the off-diagonal part
                    ss(:,iR,jR) = ss(:,iR,jR) + ...
                    weightgR(pp)*gRp*bdphi(pp,iR).*bdphi(pp,jR);
                end
            end
        end
        ss(:) = ss(:).*repmat(el,9,1);
        index = 0;
        Robin(:,3) = find(isRobin)+N;  % the third one maps to corresponding dof
        for iR = 1:3
            for jR = 1:3
                iiR(index+1:index+NR) = double(Robin(:,iR));
                jjR(index+1:index+NR) = double(Robin(:,jR));
                if jR>=iR
                    ssR(index+1:index+NR) = ss(:,iR,jR);
                else
                    ssR(index+1:index+NR) = ss(:,jR,iR);
                end
                index = index + NR;
            end
        end
        A = A + sparse(iiR,jjR,ssR,Ndof,Ndof);
    end

    % Find Dirichlet boundary dof: fixedDof
    fixedDof = [];
    freeDof = [];
    isFixedDof = false(Ndof,1);
    if ~isempty(bdFlag)
        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        isFixedDof(edge(isDirichlet,:)) = true;
        isFixedDof(N + find(isDirichlet')) = true;
        fixedDof = find(isFixedDof);
        freeDof = find(~isFixedDof);
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        fixedDof = bdDof;
        isFixedDof(fixedDof) = true;
        freeDof = find(~isFixedDof);
    end
    isPureNeumann = false;
    if isempty(fixedDof) && isempty(Robin)  % pure Neumann boundary condition
        % pde.g_N could be empty which is homogenous Neumann boundary condition
        isPureNeumann = true;
        fixedDof = 1;
        freeDof = 2:Ndof;    % eliminate the kernel by enforcing u(1) = 0;
    end
    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % |AD(fixedDof,fixedDof)=I, AD(fixedDof,freeDof)=0,
    % AD(freeDof,fixedDof)=0|.
    if ~isempty(fixedDof)
        bdidx = zeros(Ndof,1);
        bdidx(fixedDof) = 1;
        Tbd = spdiags(bdidx,0,Ndof,Ndof);
        T = spdiags(1-bdidx,0,Ndof,Ndof);
        AD = T*A*T + Tbd;
    else
        AD = A;
    end

    %% Part 2: Find boundary edges and modify the load b
    % Find boundary edges: Neumann and Robin
    Neumann = [];
    if ~isempty(bdFlag)
        idxN = (bdFlag(:) == 2);      % all Neumann edges in bdFlag
        Neumannidx = elem2edge(idxN | idxR); % index of Neumann and Robin edges
        % since boundary integral is also needed for Robin edges
        Neumann   = edge(Neumannidx,:);
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        Neumannidx = find(bdDof>N);
        Neumann = edge(Neumannidx,:);
    end

    % Neumann boundary condition
    if ~isempty(pde.g_N) && ~isempty(Neumann) && ~(isnumeric(pde.g_N) && (pde.g_N == 0))
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 4;   % default order
        end
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
        nQuadgN = size(lambdagN,1);
        % quadratic bases on an edge (1---3---2)
        bdphi = zeros(nQuadgN,3);
        bdphi(:,1) = (2*lambdagN(:,1)-1).*lambdagN(:,1);
        bdphi(:,2) = (2*lambdagN(:,2)-1).*lambdagN(:,2);
        bdphi(:,3) = 4*lambdagN(:,1).*lambdagN(:,2);
        % length of edge
        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        ge = zeros(size(Neumann,1),3);
        for pp = 1:nQuadgN
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNp = pde.g_N(ppxy);
            ge(:,1) = ge(:,1) + weightgN(pp)*gNp*bdphi(pp,1);
            ge(:,2) = ge(:,2) + weightgN(pp)*gNp*bdphi(pp,2);
            ge(:,3) = ge(:,3) + weightgN(pp)*gNp*bdphi(pp,3); % interior bubble
        end
        % update RHS
        ge = ge.*repmat(el,1,3);
        b(1:N) = b(1:N) + accumarray(Neumann(:), [ge(:,1); ge(:,2)],[N,1]);
        b(N+Neumannidx) = b(N+Neumannidx) + ge(:,3);
    end

    % Dirichlet boundary conditions
    if ~isPureNeumann && ~isempty(fixedDof) && ...
       ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && (pde.g_D == 0))
        idx = (fixedDof > N);                         % index of edge nodes
        u(fixedDof(~idx)) = pde.g_D(node(fixedDof(~idx),:)); % bd value at vertex dofs
        bdEdgeIdx = fixedDof(idx) - N;
        bdEdgeMid = (node(edge(bdEdgeIdx,1),:) + node(edge(bdEdgeIdx,2),:))/2;
        u(fixedDof(idx)) = pde.g_D(bdEdgeMid);
        b = b - A*u;
    end
    if ~isPureNeumann % non-empty Dirichlet boundary condition
        b(fixedDof) = u(fixedDof);
    end

    % Pure Neumann boundary condition
    if isPureNeumann
        b = b - mean(b);   % compatilbe condition: sum(b) = 0
        b(1) = 0;
    end
    end % end of getbdP2
end % end of function PoissonP2


%% check Clebsch Gordan Tensor

%% the reference

% some arbitrary rotation
g = rotation.byEuler(-72*degree,-88*degree,-134*degree);

% we want to express the product of two wigner D functions
D1 = WignerD(g,'order',1);
D2 = WignerD(g,'order',2);
D1D2_ref = D1(:) * D2(:).'

%% expansion into Wigner functions of lower order

% zero order component
D0 = WignerD(g,'order',0);
CG0 = ClebschGordanTensor(1,2,0);
C0 = EinsteinSum(CG0,[1 3 -1],D0,[-1,-2],CG0,[2 4 -2])

% first order component
D1 = WignerD(g,'order',1);
CG1 = ClebschGordanTensor(1,2,1);
C1 = EinsteinSum(CG1,[1 3 -1],D1,[-1 -2],CG1,[2 4 -2])

% second order component
D2 = WignerD(g,'order',2);
CG2 = ClebschGordanTensor(1,2,2);
C2 = EinsteinSum(CG2,[1 3 -1],D2,[-1 -2],CG2,[2 4 -2])

% third order component
D3 = WignerD(g,'order',3);
CG3 = ClebschGordanTensor(1,2,3);
C3 = EinsteinSum(CG3,[1 3 -1],D3,[-1 -2],CG3,[2 4 -2])

a = reshape(matrix(C0 + C1 + C2 + C3),[9,25]) ./ D1D2_ref;

imagesc(real(a))