function [ TS ] = TSEstimate( data, d, varargin )
%TSESTIMATE Tangent Space Estimation
%   Estimate the tangent space at each data point
%
% Required Input:
%  - data: data matrix, each row is a data point
%  - d:    the dimension of the manifold
%
% Optional Input:
%  - method: method to perform estimation
%     -> 'PCA': Local PCA
%  - W:      weight matrix.
%  - k:      number of nearest neighbors (default max(5, d)), if W is not
%            given, then k is used to construct W.
%
% Output:
%  - TS: m-by-d-by-n matrix, n is number of samples, m is the ambient
%        dimension and d is the manifold intrinsic dimension. TS(:,:,i) is
%        a orthonormal basis for the tangent space at point i.
% 
% Examples:
%   data = rand(100,3);
%   TS = TSEstimate(data, d, varargin);
% 
    ip = inputParser;
    ip.FunctionName = 'TSEstimate';
    ip.addRequired('data', @isnumeric);
    ip.addRequired('d', @isnumeric);
    ip.addOptional('W', [], @isnumeric);
    ip.addOptional('k', max(5,d), @isnumeric);
    ip.addOptional('method', 'PCA', @ischar);
    
    ip.parse(data, d, varargin{:});
    par = ip.Results;

    if strcmp(par.method, 'PCA')
        [n, m] = size(data);
        if ~isempty(par.W)
            W = par.W;
        else
            W = constructW(data, struct('k', par.k,'bSelfConnected',false));
        end
        
        TS = zeros(m, d, n);
        for i = 1:n
            nei = W(:, i); % column sparse matrix access is faster than row
            nei(i) = 1;
            X = data(find(nei), :);
            %TS(:,:,i) = algor.proj.PCA(X, 'ReducedDim', d, 'ExactDim', true);
            TS(:,:,i) = LocalPCA(X,d);
        end
    else
        error('Unknown tangent space estimation method: %s', par.method);
    end
end

function A=LocalPCA(X, dim)
    [n, m] = size(X);
    X = bsxfun(@minus,X,mean(X));   
    if n > m
        K = cov(X);
        A = LocalEig(K, m, dim);
    else
        % use the same trick as in kernel PCA
        K = X*X';
        [V, E] = LocalEig(K, m, dim);
        A = X'*V * diag(sqrt(1./E));
    end
    s = size(A);
    for i=1:s(2)
       A(:,i)=A(:,i)./norm(A(:,i)); 
    end
end
function [V, E]=LocalEig(K, m, dim)
    if m < 500
        [eigvec, eigval] = eig(K);
        eigval = diag(eigval);
        [~,ord]=sort(eigval,'descend');
        V = eigvec(:,ord(1:dim));
        E = eigval(ord(1:dim));
       % if eigval(1,1) < eigval(end,end)
       %     V = eigvec(:,end-dim+1:end);
       %     E = eigval(end-dim+1:end);
       % else
       %     V = eigvec(:,1:dim);
       %     E = eigval(1:dim);
       % end
    else
        opt = struct('issym',1,'isreal',1);
        [V, E]= eigs(K, dim, 'LM', opt);
        E = diag(E);
    end
end

    
