function [ mats ] = gdl_MakeMats( X, d, W, Query, varargin )  %
%gdl_MakeMats Matrix Constructions
%
%
% Input:
%  - X: m-by-n matrix, each column is a sample
%  - d: manifold dimension
%  - W: n-by-n weight matrix
%  - Query: an index number
%
% Optional Input:
%  - T       : []
%      tangent space, will be estimated automatically if not given
%  - use_parfor: false
%      whether use parfor for matrix constructing. If use this, matlabpool
%      should be called to open workers in order to get performance gain.
%  - use_sparse: false
%      whether use sparse matrix for constructing.
%          
% Output:
%  - mats: a struct containing useful matrices, and times for computing

ip = inputParser;
ip.FunctionName = 'gdl_MakeMats';
ip.addRequired('X', @isnumeric);
ip.addRequired('d', @isnumeric);
ip.addRequired('W', @isnumeric);
ip.addRequired('Quary', @isnumeric);   %

ip.addOptional('T', [], @isnumeric);
ip.addOptional('use_parfor', false, @islogical);
ip.addOptional('use_sparse', false, @islogical);
ip.addOptional('nb', 500, @isnumeric);

ip.parse(X, d, W, Query, varargin{:});  %
par = ip.Results;

times = [];

%************************************************
% preparation
%************************************************
ts_prepare = cputime;

[n] = size(X,2);
% add more points to V0
nb=par.nb;
wm=zeros(n,1);
queryIdx = find(Query);
queryIdx = queryIdx(1);
dist=EuDist2(X(:, queryIdx)',X');
[~,nbidx]=sort(dist);
wm(nbidx(2:nb))=1;

%*************************************************
% ensure W is not self-connected
W(sub2ind([n n], 1:n, 1:n)) = 0;

L  = diag(sum(W)) - W;

times.prepare = cputime - ts_prepare;

if ~isempty(par.T)
    Tcache = par.T;
    times.tangent_space = 0;
else
    ts_tangent_space = cputime;
    Tcache = TSEstimate(X', d, 'W', W);
    times.tangent_space = cputime - ts_tangent_space;
end

%************************************************
% matrix construction
%************************************************
ts_matrix = cputime;

if par.use_sparse
    mat_fun = @sparse;
else
    mat_fun = @zeros;
end

G     = mat_fun(d*n, d*n);
beta  = mat_fun(d*n, n);
B     = mat_fun(d*n, d*n);
Id    = eye(d);

if par.use_parfor
    Gc     = cell(n,1);
    betac  = cell(n,1);
    Bc     = cell(n,1);
    
    parfor i = 1:n
        Gbk = zeros(d);
        bbk = zeros(d,n);
        Bbk = zeros(d);
        Xi  = X(:,i);
        Ti  = Tcache(:,:,i);
        
        nei = find(W(:,i))'; % sparse column access faster than row access
        Bcc = cell(size(nei));
        
        for ij = 1:length(nei)
            j = nei(ij);
            Mt1 = Ti'*(X(:,j)-Xi);
            Qij = Ti'*Tcache(:,:,j);
            Mt2 = zeros(d, n); Mt2(:,i) = -W(i,j)*Mt1; Mt2(:,j) = W(i,j)*Mt1;
            
            Gbk = Gbk + W(i,j)*(Mt1*Mt1');
            bbk = bbk + Mt2;
            Bbk = Bbk + W(i,j) * (Qij*Qij' + Id);
            Bcc{ij} = -2*W(i,j)*Qij;
        end
        
        Gc{i} = Gbk;
        betac{i} = bbk;
        Bc{i} = [Bbk Bcc];
    end
    
    for i = 1:n
        idx = (i-1)*d+1:i*d;
        nei = find(W(i,:));
        Bcc = Bc{i};
        for ij = 1:length(nei)
            j = nei(ij);
            jdx = (j-1)*d+1:j*d;
            B(idx,jdx) = Bcc{ij+1};
        end
        G(idx,idx)   = Gc{i};
        beta(idx,:)  = betac{i};
        B(idx,idx)   = Bcc{1};
    end
else
    for i = 1:n
        idx = (i-1)*d+1:i*d;
        Gbk = zeros(d);
        bbk = zeros(d,n);
        Bbk = zeros(d);
        Xi  = X(:,i);
        Ti  = Tcache(:,:,i);
        
        for j = find(W(i,:))
            jdx = (j-1)*d+1:j*d;
            Mt1 = Ti'*(X(:,j)-Xi);
            Qij = Ti'*Tcache(:,:,j);
            Mt2 = zeros(d, n); Mt2(:,i) = -W(i,j)*Mt1; Mt2(:,j) = W(i,j)*Mt1;
            
            Gbk = Gbk + W(i,j)*(Mt1*Mt1');
            bbk = bbk + Mt2;
            Bbk = Bbk + W(i,j) * (Qij*Qij' + Id);
            B(idx,jdx) = -2*W(i,j)*Qij;
        end
        
        G(idx,idx)   = Gbk;
        beta(idx,:)  = bbk;
        B(idx,idx)   = Bbk;
    end
end

D     = mat_fun(d*n, d*n);  
H     = mat_fun(d*n, 1); 
for j = find(W(:,queryIdx)')
%for j = find(wm')
    Xj  = X(:,j);
    Tj  = Tcache(:,:,j);
    jdx = (j-1)*d+1:j*d;
    Dbk = Id;
    Hbk = Tj'*(X(:,queryIdx)-Xj);
    Hbk = Hbk./norm(Hbk).*10000;
    D(jdx, jdx) = Dbk;
    H(jdx) = Hbk;
end   

nD = zeros(d*n, d*n);
nI = zeros(d*n, d*n);
nB = zeros(d*n, d*n);
for i = 1:n
   degi = 0;
   for j = find(W(i,:))
      degi = degi + W(i,j);
      Qij = Tcache(:,:,i)'*Tcache(:,:,j);
      nB((i-1)*d+1:i*d, (j-1)*d+1:j*d)=W(i,j).*Qij;
      nI((i-1)*d+1:i*d,(i-1)*d+1:i*d)=nI((i-1)*d+1:i*d,(i-1)*d+1:i*d)+W(i,j)*(Qij*Qij'+eye(d));
   end
   degi = 1/degi;
   nD((i-1)*d+1:i*d,(i-1)*d+1:i*d) = eye(d)*degi;   
   nI((i-1)*d+1:i*d,(i-1)*d+1:i*d) = 0.5*degi*nI((i-1)*d+1:i*d,(i-1)*d+1:i*d);   
end
%nI = eye(d*n);

NCL = nI - nD*nB;
NCL2 = 0.5*nD*B;
%compareMats(NCL, NCL2);
times.matrix = cputime - ts_matrix;

mats = [];
mats.L = L;
mats.G = G;
mats.beta = beta;  %C
mats.B = B;
mats.T = Tcache; %(m*d*n)
mats.W = W;
mats.D = D;      %
mats.H = H;      %(dn*dn)
mats.d = d;
mats.n = n;
mats.V0 = -H;
mats.NCL = NCL;  %(dn*dn)
mats.NCL2 = NCL2;
mats.nI = nI;
mats.times = times;

end
