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 selected_indices=cosmo_anova_feature_selector(dataset, how_many)
% find the features that show the most variance between classes
%
% selected_indices=cosmo_anova_feature_selector(dataset, how_many)
%
% Inputs:
%  dataset          struct with .samples and .sa.targets
%  how_many         value between 0 and 1 keeps how_many*100% features;
%                   values >=1 keeps how_many features
%
% Output:
%  selected_indices   feature ids in dataset with most variance between
%                     classes.
%
% Example:
%     ds=cosmo_synthetic_dataset();
%     disp(size(ds.samples))
%     %|| [ 6 6 ]
%     cosmo_anova_feature_selector(ds,.45) % find best ~45% of features
%     %|| [ 2 4 5 ]
%     cosmo_anova_feature_selector(ds,4) % find best 4 features
%     %|| [ 2 4 5 3 ]
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

    fstat=cosmo_stat(dataset,'F');
    fvalues=fstat.samples;

    % ensure that nan values are not selected by setting them to
    % an impossible low F value
    fvalues(isnan(fvalues))=-1;

    % sort by F values, largest first
    [unused, idxs]=sort(fvalues,'descend');

    % determine features to select
    nfeatures=size(dataset.samples,2);

    if how_many>=1
        if round(how_many)~=how_many
            error('how_many>=1 is not an integer');
        elseif how_many>nfeatures
            error('dataset has %d features, cannot return %d',...
                    nfeatures,how_many);
        end
        nkeep=how_many;
    else
        nkeep=round(how_many*nfeatures);
    end

    selected_indices=idxs(1:nkeep);

    % throw an error if any indices with NaN F values
    if any(fvalues(selected_indices)<0)
        idx=find(fvalues(selected_indices)<0,1);
        error('Feature %d has NaN Fscore', selected_indices(idx));
    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


%  Figure 7.44      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
% script to generate Fig. 7.44
clf;
f=[-10 1 0;-16 0 1;0 0 0];
g=[0 0 10]';
h=[1 0 0];
j=0;
a=[-f' -h'*h;0*f f];
b=[0*g;g];
c=[g' 0*h];
d=0;
rlocus(a,b,c,d);
axis([-12, 12, -6, 6]);
grid;
title('Fig.7.44  Symmetric root locus for a DC Servo system')


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))


% Set axes limit.

clear all;
addpath('../lib');

%% lets plot 3 cycles of 50Hz AC voltage
f = 50;
Vm = 10;
phi = 0;

% generate the signal
t = [0:0.0001:3/f];
th = 2*pi*f*t;
v = Vm*sin(th+phi);

%% plot now
plt = Plot(t*1E3, v);

plt.XLabel = 'Time, t (ms)'; % xlabel
plt.YLabel = 'Voltage, V (V)'; %ylabel
plt.XLim = [0, 40]; % set x axis limit
plt.YLim = [-11, 11]; % set y axis limit

% Save? comment the following line if you do not want to save
plt.export('plotAxisLimit.jpg');


%...!
%/*************************************************************************
% Function Name : wrl2mat
% Author: Alireza Bossaghzadeh

    % This Function Extract Pointcloud from a wrl file and change it to a 2D
    % mesh matrix
    % In the process of changing to mesh, it will interpolated the hole parts
    % (Parts without any value)

%/*************************************************************************
    % Example: [ZI pointcloud]=wrl2mat('mywrl.wrl');
%     Input:         fname 	 wrl File Name
%     Outputs:       ZI          	 2D mesh File
%                    pointcloud 	 3xN  pointcloud data
    % Suggestion:
    % to have a better output you should use crop3dface.m that is available in my
    % files in mathworks file exchange.

% In the case of any problem you can call me by
% Email:Alibossagh@yahoo.co.uk
% This code is a modified version of the read_vrml.m written by "G. Akroyd"

% Version:    1.00       Published: 2008 June 07

function [ZI pointcloud]=wrl2mat(fname)

disp(['Reading The File  ', fname])
        vrfile=fopen(fname);
        p=1;counter=0;pointcloud=[0 0 0];
 while counter~=-1
    data=fgets(vrfile);
    fpoint=findstr(data,'point');% 2 checkers to find out the begining
    f2point=findstr(data,'[');   %of the x,y,z " point [ "
    while ~isempty(fpoint) & ~isempty(f2point)
      data=fgets(vrfile);
        if isempty(findstr(data,']'));
          t=sscanf(data,'%f %f %f');
          pointcloud(p,:)=t';
          p=p+1;
        else fpoint=[];counter=-1;
        end
    end
 end
 disp(['Convert to 2D Image'])
X=pointcloud(:,1);Y=pointcloud(:,2);Z=pointcloud(:,3);%Load positions to X,Y,Z
X=X-min(X);Y=Y-min(Y);%change X,Y to start from 0
x=[min(X):1:max(X)];y=[min(Y):1:max(Y)];%create image dimentions
[XI,YI] = meshgrid(x,y);%create matrix of positions
YI=max(YI(:))-YI;
Z=Z-min(Z(:));%Change Z data to start from 0
ZI=griddata(X,Y,Z,XI,YI,'linear'); % calculate Z values from noninteger positions
    figure;imshow(ZI,[]);title('Extracted Image')


function linplus_test19 ( )

%*****************************************************************************80
%
%% TEST19 tests R8CBB_FA, R8CBB_PRINT, R8CBB_RANDOM, R8CBB_SL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n1 = 8;
  n2 = 2;
  n = n1 + n2;
  ml = 1;
  mu = 1;
  na = ( ml + mu + 1 ) * n1 + 2 * n1 * n2 + n2 * n2;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST19\n' );
  fprintf ( 1, '  For a compressed border banded matrix:\n' );
  fprintf ( 1, '  R8CBB_RANDOM randomly generates;\n' );
  fprintf ( 1, '  R8CBB_PRINT prints;\n' );
  fprintf ( 1, '  R8CBB_FA factors (no pivoting);\n' );
  fprintf ( 1, '  R8CBB_SL solves.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N     = %d\n', n );
  fprintf ( 1, '  Matrix suborder N1 = %d\n', n1 );
  fprintf ( 1, '  Matrix suborder N2 = %d\n', n2 );
  fprintf ( 1, '  Lower bandwidth ML = %d\n', ml );
  fprintf ( 1, '  Upper bandwidth MU = %d\n', mu );
%
%  Set the matrix.
%
  [ a, seed ] = r8cbb_random ( n1, n2, ml, mu, seed );

  r8cbb_print ( n1, n2, ml, mu, a, '  The R8CBB matrix:' );
%
%  Set the desired solution.
%
  x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
  b = r8cbb_mxv ( n1, n2, ml, mu, a, x );
%
%  Factor the matrix
%
  [ a_lu, info ] = r8cbb_fa ( n1, n2, ml, mu, a );

  r8cbb_print ( n1, n2, ml, mu, a_lu, '  The factored matrix:' );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST19 - Fatal error!\n' );
    fprintf ( 1, '  R8CBB_FA claims the matrix is singular.\n' );
    fprintf ( 1, '  The value of INFO is %d\n', info );
    return
  end
%
%  Solve the system.
%
  r8vec_print ( n, b, '  The right hand side vector:' );

  x = r8cbb_sl ( n1, n2, ml, mu, a_lu, b );

  r8vec_print ( n, x, '  Solution:' );

  return
end


%Kalman Demo 1 (Sinc Regression)
%
%  The benefit from the state space formulation is that the
%  computational complexity is linear with respect to the number
%  of data points. Due to efficient matrix solvers in Matlab
%  (favoring the traditional GP solution over sequential looping),
%  the advantages in speed start to show in datasets with thousands
%  of data points.
%
%  The take-home message from this demo is that the GP can be
%  set up exactly as any other GP regression model in GPstuff,
%  and then solved by Kalman filtering methods by specifying the
%  'type' in the GP structure to be 'KALMAN'.
%
%  The implementation that is used below is primarily based on the
%  methods presented in the following publications:
%
%  [1] Simo Sarkka, Arno Solin, Jouni Hartikainen (2013).
%      Spatiotemporal learning via infinite-dimensional Bayesian
%      filtering and smoothing. IEEE Signal Processing Magazine,
%      30(4):51-61.
%
%  [2] Jouni Hartikainen and Simo Sarkka (2010). Kalman filtering and
%      smoothing solutions to temporal Gaussian process regression
%      models. Proceedings of IEEE International Workshop on Machine
%      Learning for Signal Processing (MLSP).
%
%  [3] Simo Sarkka (2013). Bayesian filtering and smoothing. Cambridge
%      University Press.
%
%  Copyright (c) 2014 Arno Solin and Jukka Koskenranta

%  This software is distributed under the GNU General Public
%  License (version 3 or later); please refer to the file
%  License.txt, included with the software, for details.

%% Generate data

  % Discretize x (measurement points)
  x = 10*rand(32,1)-5;

  % Test points (evaluation points)
  xt = linspace(-5,5,128)';

  % Measurement noise variance
  sigma2 = .01;

  % Simulate data
  y = sinc(x) + sqrt(sigma2)*randn(size(x));

%% Do the inference

  % Noise variance prior
  ps2 = prior_logunif();

  % The likelihood model
  lik = lik_gaussian('sigma2', 1, 'sigma2_prior', ps2);

  % Covariance function hyperparameter priors
  pl = prior_logunif();
  pm = prior_logunif();

  % The GP covariance function
  gpcf = gpcf_matern52('lengthScale', 1, ...
                       'magnSigma2', 1, ...
                       'lengthScale_prior', pl, ...
                       'magnSigma2_prior', pm);

  % Define Gaussian process model using type 'KALMAN'
  gp = gp_set('lik', lik, 'cf', gpcf, 'type', 'KALMAN');

  % Hyperparameter optimization (full model)
  gp_full = gp_optim(gp_set(gp,'type','FULL'), x, y);

  % Hyperparameter optimization (state space model)
  gp = gp_optim(gp, x, y);

  % Predict values at test inputs xt
  [Eft,Varft] = gp_pred(gp, x, y, 'xt', xt);

%% Compare against full GP solution (table)

  % Show table with comparison to full GP results
  fprintf('\n%12s | %8s | %8s \n', ...
      'Parameter','FULL', 'KALMAN')

  fprintf('-----------------------------------\n')

  fprintf('%12s | %8.4f | %8.4f \n', ...
      'magnSigma2',gp_full.cf{1}.magnSigma2,gp.cf{1}.magnSigma2)
  fprintf('%12s | %8.4f | %8.4f \n', ...
      'lengthScale',gp_full.cf{1}.lengthScale,gp.cf{1}.lengthScale)
  fprintf('%12s | %8.4f | %8.4f \n', ...
      'sigma2',gp_full.lik.sigma2,gp.lik.sigma2)

%% Show result

  % Plot the state space prediction
  figure(1); clf; hold on

    % Draw 95% uncertainty interval
    color=0.85*[1,1,1];
    p=patch([xt; flipud(xt)], ...
            [Eft+1.96*sqrt(Varft); flipud(Eft-1.96*sqrt(Varft))],color);
    set(p,'EdgeColor','none')

    % Show data and predicted mean
    h=plot(xt,sinc(xt),'-r', ...
           x,y,'+k', ...
           xt,Eft,'--','LineWidth', 1, 'MarkerSize',5);

    % Legend and labels
    xlabel('Input, x');
    ylabel('Output, y');
    title('Kalman Demo 1 (Sinc Regression)');
    legend([h;p], 'Sinc function', ...
                  'Noisy measurements', ...
                  'Estimated mean', ...
                  '95% confidence region');

    % Bring axes to front
    box on; set(gca,'Layer','top')

%% Compare the full posteriors

  % Return the posterior mean EFT and covariance COVFT of latent
  % variables:
  %     Eft =  E[f | xt,x,y,th]  = K_fy*(Kyy+s^2I)^(-1)*y
  %   Covft = Var[f | xt,x,y,th] = K_fy - K_fy*(Kyy+s^2I)^(-1)*K_yf.

  figure(2); clf

  % The full GP
  subplot(121)

    % Calculate
    [Eft,Covft] = gp_jpred(gp_full,x,y,xt);

    % Plot
    imagesc(Covft)
    axis equal tight

    % Title
    title('The full GP posterior covariance')

  % The state space model ('KALMAN')
  subplot(122)

    % Calculate
    [Eft,Covft] = gp_jpred(gp,x,y,xt);

    % Plot
    imagesc(Covft)
    axis equal tight

    % Title
    title('The state space GP posterior covariance')


function [XN,IOTA,EPS,U]=quasi_idempotence(X,K)
%QUASI_IDEMPOTENCE     Connection matrix quasi-idempotence
%
%   [XN,IOTA,EPS,U]=quasi_idempotence(X)
%   [XN,IOTA,EPS,U]=quasi_idempotence(X,K)
%
%   The degree of quasi-idempotence of a matrix represents how close it
%   is to being idempotent, i.e. invariant to squaring. In turn, this
%   reflects how closely related the edges in the graph it corresponds to
%   are to the sums of all triangles between the corresponding nodes,
%   spanning the entirety of the network. This probes a form of
%   self-similarity, intended not as between nodes and modules, but
%   between as edges and triangles (or more generally paths).
%   Networks wherein the edge strengths are weakly related to the
%   nodal strengths have low quasi-idempotence, and networks wherein the
%   edge strengths are significantly influenced by the strengths of the
%   corresponding nodes have high quasi-idempotence. In other words the
%   degree of quasi-idempotence may be viewed as a measure of
%   "collectivity", meaning how strongly individual nodes participate
%   in collective dynamics.
%
%   Inputs:
%       X,
%           undirected weighted/binary connection matrix with
%           non-negative weights,
%       K,
%           number of iterations (optional),
%               K<inf,        iterate a predetermined number of times,
%               K=inf,        iterate until convergence is attained (default).
%
%       Note: see Minati et al. (2017) for a discussion of the issues
%       associated with applying this measure to binary graphs.
%
%   Outputs:
%       XN,
%           the final matrix, from the last iteration,
%       IOTA,
%           the vector of correlation coefficients, one per iteration,
%       EPS,
%           the vector of errors, one per iteration,
%       U,
%           the number of iterations performed.
%
%       Note: see Minati et al. (2017) for a discussion of the
%       significance of IOTA(1) and IOTA(end).
%
%   Example:
%       % Create a random undirected weighted connection matrix
%       N=100;
%       X=rand(N);
%       X=X+X';
%       [XN,IOTA,EPS,U]=quasi_idempotence(X);
%       % observe that IOTA~0
%       disp(IOTA);
%       %
%       % make the connection matrix idempotent by squaring it
%       X=X^2;
%       [XN,IOTA,EPS,U]=quasi_idempotence(X);
%       % observe that IOTA~1
%       disp(IOTA);
%
%   Reference:
%       Minati et al. (2017) CHAOS, 043115
%
%   Ludovico Minati, University of Trento, Trento, Italy
%   and Institute of Nuclear Physics, Polish Academy of Sciences, Krakow,
%   Poland, 2017-2018
%   lminati@ieee.org
%
%   Modification history
%   2018: Original

X=cast(X,'double'); % enforce double-precision format
if any(any(isnan(X))) % bail out if any nan found
    error('found nan, giving up!');
end
if any(any(X<0)) % bail out if any negative elements found
    error('found a negative element, giving up!');
end
if ~exist('K','var')||isempty(K)
    K=inf;
end
N=length(X); % get matrix size
X(eye(N)>0)=0; % null the diagonal in the initial matrix
X=X/norm(X); % normalize to unit norm
XN=X;
mask=triu(ones(N),1)>0; % create mask for superdiagonal elements
U=0; % initialize iterations counter
IOTA=[]; % this vector will contain the correlation coefficients
EPS=inf; % and this will contain the errors
if isinf(K)
    while EPS(end)>eps('double') % iterate until error below precision
        U=U+1; % increase iteration counter
        XN_hat=XN; % save the initial matrix
        XN=XN^2; % square the matrix
        XN=XN/norm(XN); % normalize it again (for numerical reasons)
        IOTA(end+1)=corr(X(mask),XN(mask)); % calculate correlation coefficient
        EPS(end+1)=norm(XN_hat-XN); % calculate error
    end
else
    while U<K % iterate a prescribed number of times
        U=U+1; % increase iteration counter
        XN_hat=XN; % save the initial matrix
        XN=XN^2; % square the matrix
        XN=XN/norm(XN); % normalize it again (for numerical reasons)
        IOTA(end+1)=corr(X(mask),XN(mask)); % calculate correlation coefficient
        EPS(end+1)=norm(XN_hat-XN); % calculate error
    end
end
EPS(1)=[];


%THIS FUMCTION ILLUSTRATES HOW TO USE THE USER-DEFINED ALGORITHM
%
% This is the second-order NMF algorithm based on the Newton method.
% The algorithm is presented by R. Zdunek and A. Cichocki in Signal
% Processing (2007)
function [A,X] = user_alg1(Y,R,Xexact)
%
% The example of implementing the user-defined algorithm
% (this is the second-order algorithm applied to the Euclidean function)
%
% INPUTS:
% Y - mixed signals (matrix of size [M by T])
% R - number of estimated signals
% Xexact - true source signals
%
% OUTPUTS
% A - estimated mixing matrix (matrix of size [M by R])
% X - estimated source signals (matrix of size [R by T])
%
% #########################################################################
% Initialization
MaxIter = 1000; % Number of alternating steps
Alpha0 = 100; Tau = 50; % Parameters in regularization for estimation of X
%
Y(Y <=0) = eps; % this enforces the positive value in the data
% Scaling to unit-variance
mx = 1./sqrt(var(Y,0,2) + eps);  Y = repmat(mx,[1,size(Y,2),1]).*Y;

% Settings
[M,T]=size(Y); I = eye(R); A = []; X = [];
lambda = 1E-12; % Levenberg-Marquardt regularization of Hessian
HA = spalloc(M*R,M*R,M*R^2); % Spase allocation for Hessian
while 1 A = rand(M,R); if cond(A) < 50 break; end; end % Initialization

% Alternatings
k = 0;
while k <=MaxIter
    k = k + 1;

  % Estimation of X
    alpha_reg = Alpha0*exp(-k/Tau); % exponential rule for regular.param.
    if isnan(A)  disp('Matrix A is too much ill-conditioned. Try again.');
        break; end
    if cond(A) > 1E6 alphaX = 1E-6; else alphaX = 0; end
    X = max(1E6*eps,pinv(A'*A + alpha_reg + alphaX*I)*A'*Y); %Updating of X

  % Estimation of A
    hA = X*X';
    hA = hA + (lambda + 1E8*exp(-k))*eye(R);  % Levenberg-Marquardt regularization
    GA = X*Y' - hA*A';  % Gradient
    HA = kron(speye(M),-hA); % Hessian

cond_HA = condest(HA);
if cond_HA > 1E17
fprintf(1,'Hessian is singular in %d iteration(s). Restart is needed.\n',k)
break; end
A = A'; A(:) = A(:) - .9*inv(HA)*GA(:); % Newton iterations
A(A <= 0) = 1E2*eps;  A = A';
A = A*diag(1./sum(A,1)); % Normalization to unit L1-column norm
end
A = repmat(1./mx,[1,size(A,2),1]).*A; % De-scaling


function r8poly_values_horner_test ( )

%*****************************************************************************80
%
%% R8POLY_VALUES_HORNER_TEST tests R8POLY_VALUES_HORNER.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 December 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8POLY_VALUES_HORNER_TEST\n' );
  fprintf ( 1, '  R8POLY_VALUES_HORNER evaluates a polynomial at a\n' );
  fprintf ( 1, '  point, using Horner''s method.\n' );

  m = 4;
  c = [ 24.0, -50.0, +35.0, -10.0, 1.0 ];
  r8poly_print ( m, c, '  The polynomial:' );

  n = 16;
  x_lo = 0.0;
  x_hi = 5.0;
  x = r8vec_linspace ( n, x_lo, x_hi );

  p = r8poly_values_horner ( m, c, n, x );

  r8vec2_print ( n, x, p, '  X, P(X):' );

  return
end


function [intersects, edgeIndices] = intersectLinePolyline(line, poly, varargin)
%INTERSECTLINEPOLYLINE Intersection points between a line and a polyline
%
%   P = intersectLinePolyline(LINE, POLY)
%   Returns the intersection points of the lines LINE with polyline POLY.
%   LINE is a 1-by-4 row vector containing parametric representation of the
%   line (in the format [x0 y0 dx dy], see the function 'createLine' for
%   details).
%   POLY is a NV-by-2 array containing coordinates of the polyline vertices
%   P is a K-by-2 array containing the coordinates of the K intersection
%   points.
%
%   P = intersectLinePolyline(LINE, POLY, TOL)
%   Specifies the tolerance for geometric tests. Default is 1e-14.
%
%   [P INDS] = intersectLinePolyline(...)
%   Also returns the indices of edges involved in intersections. INDS is a
%   K-by-1 column vector, such that P(i,:) corresponds to intersection of
%   the line with the i-th edge of the polyline. If the intersection occurs
%   at a polyline vertex, the index of only one of the two neighbor edges
%   is returned.
%   Note that due to numerical approximations, the use of function
%   'isPointOnEdge' may give results not consistent with this function.
%
%
%   Examples
%   % compute intersections between a square and an horizontal line
%     poly = [0 0;10 0;10 10;0 10];
%     line = [5 5 1 0];
%     intersectLinePolyline(line, poly)
%     ans =
%           10     5
%     % also return indices of edges
%     [inters inds] = intersectLinePolyline(line, poly)
%     inters =
%           10     5
%     inds =
%           2
%
%   % compute intersections between a square and a diagonal line
%     poly = [0 0;10 0;10 10;0 10];
%     line = [5 5 1 1];
%     intersectLinePolyline(line, poly)
%     ans =
%            0     0
%           10    10
%
%   See Also
%   lines2d, polylines2d, intersectLines, intersectLinePolygon
%

%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 31/10/2003.
%

%   HISTORY
%   2008-11-24 rename 'pi' as 'intersects', update doc
%   2009-07-23 removed forgotten occurence of 'pi' variable (thanks to Bala
%       Krishnamoorthy)
%   2010-01-26 rewrite using vectorisation
%   2011-05-20 returns unique results
%   2011-07-20 returns intersected edge indices
%   2012-11-33 add 'diag' option for linePosition

% get computation tolerance
tol = 1e-14;
if ~isempty(varargin)
    tol = varargin{1};
end

% create the array of edges
N = size(poly, 1);
edges = [poly(1:N-1, :) poly(2:N, :)];

% compute intersections with supporting lines of polyline edges
supportLines = edgeToLine(edges);
intersects = intersectLines(line, supportLines, tol);

% find edges that are not parallel to the input line
inds = find(isfinite(intersects(:, 1)));

% compute position of intersection points on corresponding lines
pos = linePosition(intersects(inds, :), supportLines(inds, :), 'diag');

% and keep only intersection points located on edges
b = pos > -tol & pos < 1+tol;
inds = inds(b);
intersects = intersects(inds, :);

% remove multiple vertices (can occur for intersections located at polyline
% vertices)
[intersects, I, J] = unique(intersects, 'rows'); %#ok<ASGLU>

if nargout > 1
    % return indices of edges involved in intersection
    % (in case of intersection located at a vertex, only one of the
    % neighbor edges is returned)
    edgeIndices = inds(I);
end