function [Q_k,T_k,r,anorm,ierr,work] = lanpro(A,nin,kmax,r,options,...
    Q_k,T_k,anorm)

%LANPRO   Lanczos tridiagonalization with partial reorthogonalization
%   LANPRO computes the Lanczos tridiagonalization of a real symmetric
%   matrix using the symmetric Lanczos algorithm with partial
%   reorthogonalization.
%
%   [Q_K,T_K,R,ANORM,IERR,WORK] = LANPRO(A,K,R0,OPTIONS,Q_old,T_old)
%   [Q_K,T_K,R,ANORM,IERR,WORK] = LANPRO('Afun',N,K,R0,OPTIONS,Q_old,T_old)
%
%   Computes K steps of the Lanczos algorithm with starting vector R0,
%   and returns the K x K tridiagonal T_K, the N x K matrix Q_K
%   with semiorthonormal columns and the residual vector R such that
%
%        A*Q_K = Q_K*T_K + R .
%
%   Partial reorthogonalization is used to keep the columns of Q_K
%   semiorthogonal:
%        MAX(DIAG((eye(k) - Q_K'*Q_K))) <= OPTIONS.delta.
%
%
%   The first input argument is either a real symmetric matrix, a struct with
%   components A.L and A.U or a string containing the name of an M-file which
%   applies a linear operator to the columns of a given matrix.  In the latter
%   case, the second input argument must be N, the order of the problem.
%
%   If A is a struct with components A.L and A.U, such that
%   L*U = (A - sigma*I), a shift-and-invert Lanczos iteration is performed
%
%   The OPTIONS structure is used to control the reorthogonalization:
%     OPTIONS.delta:  Desired level of orthogonality
%                     (default = sqrt(eps/K)).
%     OPTIONS.eta  :  Level of orthogonality after reorthogonalization
%                     (default = eps^(3/4)/sqrt(K)).
%     OPTIONS.cgs  :  Flag for switching between different reorthogonalization
%                     algorithms:
%                      0 = iterated modified Gram-Schmidt  (default)
%                      1 = iterated classical Gram-Schmidt
%     OPTIONS.elr  :  If OPTIONS.elr = 1 (default) then extended local
%                     reorthogonalization is enforced.
%     OPTIONS.Y    :  The lanczos vectors are reorthogonalized against
%                     the columns of the matrix OPTIONS.Y.
%
%   If both R0, Q_old and T_old are provided, they must contain
%   a partial Lanczos tridiagonalization of A on the form
%
%        A Q_old = Q_old T_old + R0 .
%
%   In this case the factorization is extended to dimension K x K by
%   continuing the Lanczos algorithm with R0 as starting vector.
%
%   On exit ANORM contains an approximation to ||A||_2.
%     IERR = 0  :  K steps were performed succesfully.
%     IERR > 0  :  K steps were performed succesfully, but the algorithm
%                  switched to full reorthogonalization after IERR steps.
%     IERR < 0  :  Iteration was terminated after -IERR steps because an
%                  invariant subspace was found, and 3 deflation attempts
%                  were unsuccessful.
%   On exit WORK(1) contains the number of reorthogonalizations performed, and
%   WORK(2) contains the number of inner products performed in the
%   reorthogonalizations.
%
%   See also LANEIG, REORTH, COMPUTE_INT

% References:
% R.M. Larsen, Ph.D. Thesis, Aarhus University, 1998.
%
% G. H. Golub & C. F. Van Loan, "Matrix Computations",
% 3. Ed., Johns Hopkins, 1996.  Chapter 9.
%
% B. N. Parlett, ``The Symmetric Eigenvalue Problem'',
% Prentice-Hall, Englewood Cliffs, NJ, 1980.
%
% H. D. Simon, ``The Lanczos algorithm with partial reorthogonalization'',
% Math. Comp. 42 (1984), no. 165, 115--142.

% Rasmus Munk Larsen, DAIMI, 1998

% Check input arguments.
if nargin<1, error('Not enough input arguments.');  end
if isnumeric(A) | isstruct(A)
  if isnumeric(A)
    [m n] = size(A);
    if m~=n | ~isequal(A,A') | ~isreal(A)
      error('A must be real symmetric')
    end
  elseif isstruct(A)
    [m n] = size(A.L);
  end

  if nargin<7 | isempty(T_k),
    anorm = []; est_anorm=1;
  else
    anorm = T_k; est_anorm=0;
  end
  if nargin<6,  Q_k=[]; T_k=[]; else,  T_k = Q_k; Q_k = options; end
  if nargin<4 | isempty(r),  options = []; else,  options = r;  end
  if nargin<3 | isempty(kmax),
    r = rand(n,1)-0.5;
  else
    r = kmax;
  end
  if nargin<2 | isempty(nin);  kmax = max(10,n/10); else,  kmax = nin;  end
else
  if nargin<2
    error('Not enough input arguments.');
  end
  % Check input functions and parse to create an internal object
  % if an explicit expression is given.
  [A, msg] = fcnchk(A);
  if ~isempty(msg)
    error(msg);
  end
  n = nin;
  if nargin<8 | isempty(anorm), anorm = []; est_anorm=1; else est_anorm=0; end
  if nargin<7,  Q_k=[]; T_k=[]; end
  if nargin<5 | isempty(options),  options = [];          end
  if nargin<4 | isempty(r),  r = rand(n,1)-0.5;   end
  if nargin<3 | isempty(kmax);  kmax = max(10,n/10); end
end

% Set options.
delta = sqrt(eps/kmax); % Desired level of orthogonality.
eta = eps^(3/4)/sqrt(kmax);     % Level of orth. after reorthogonalization.
cgs = 0;                % Flag for switching between iterated CGS and MGS.
elr = 1;                % Flag for switching extended local
                        % reorthogonalization on and off.
deflate = 0;              % Flag for deflation against OPTIONS.Y

% Parse options struct
if ~isempty(options) & isstruct(options)
  c = fieldnames(options);
  for i=1:length(c)
    if strmatch(c(i),'delta'), delta = getfield(options,'delta');  end
    if strmatch(c(i),'eta'), eta = getfield(options,'eta'); end
    if strmatch(c(i),'cgs'), cgs = getfield(options,'cgs'); end
    if strmatch(c(i),'elr'), elr = getfield(options,'elr'); end
    if strmatch(c(i),'Y'), deflate = ~isempty(options.Y);  end
  end
end

np = 0;  nr = 0; ierr=0;

% Rule-of-thumb estimate on the size of round-off terms:
eps1 = sqrt(n)*eps/2; % Notice that {\bf u} == eps/2.
gamma = 1/sqrt(2);

% Prepare Lanczos iteration
if isempty(Q_k) % New Lanczos tridiagonalization.
  % Allocate space
  alpha = zeros(kmax+1,1);  beta = zeros(kmax+1,1);
  Q_k = zeros(n,kmax);
  q = zeros(n,1); beta(1)=norm(r);
  omega = zeros(kmax,1); omega_max = omega;  omega_old = omega;
  omega(1) = 0;   force_reorth= 0;
  j0 = 1;
else            % Extending existing Lanczos tridiagonalization.
  j = size(Q_k,2); % Size of existing factorization
  % Allocate space
  Q_k = [Q_k zeros(n,kmax-j)];
  alpha = zeros(kmax+1,1);  beta = zeros(kmax+1,1);
  alpha(1:j) = diag(T_k);
  if j>1
    beta(2:j) = diag(T_k,-1);
  end
  q = Q_k(:,j);
  % Reorthogonalize r.
  beta(j+1) = norm(r);
  if j<kmax & beta(j+1)*delta < anorm*eps1,
    fro = 1;
  end
  if isfinite(delta)
    int = 1:j;
    [r,beta(j+1),rr] = reorth(Q_k,r,beta(j+1),int,gamma,cgs);
    np = rr*j;    nr = 1;   force_reorth = 1;
  else
     force_reorth = 0;
  end
  % Compute Gerscgorin bound on ||T_k||_2 as SQRT(||T_k'*T_k||_1)
  if est_anorm
    anorm = sqrt(norm(T_k'*T_k,1));
  end
  omega = eps1*ones(kmax,1); omega_max = omega;  omega_old = omega;
  j0 = j+1;
end

if delta==0
  fro = 1; % The user has requested full reorthogonalization.
else
  fro = 0;
end

for j=j0:kmax,
  % Lanczos Step:
  q_old = q;
  if beta(j)==0
    q = r;
  else
    q = r / beta(j);
  end
  Q_k(:,j) = q;
  if isnumeric(A)
    u = A*q;
  elseif isstruct(A)
    u = A.U \ ( A.L \ q);
  else
    u = feval(A,q);
  end
  r = u - beta(j)*q_old;
  alpha(j) = q'*r;
  r = r - alpha(j)*q;

  % Extended local reorthogonalization:
  beta(j+1) = sqrt(r'*r); % Quick and dirty estimate.
  if beta(j+1)<gamma*beta(j) & elr
    if  j==1
      t1=0;
      for i=1:2
	t = q'*r;
	r = r-q*t;
	t1 = t1+t;
      end
      alpha(j) = alpha(j) + t1;
    elseif j>1
      t1 = q_old'*r;
      t2 = q'*r;
      r = r  - (q_old*t1 + q*t2); % Add small terms together first to
      if beta(j)~=0               % reduce risk of cancellation.
	beta(j) = beta(j) + t1;
      end
      alpha(j) = alpha(j) + t2;
    end
    beta(j+1) = sqrt(r'*r); % Quick and dirty estimate.
  end

  % Update Gersgorin estimate of ||T_k|| if required
%  if est_anorm & beta(j+1)~=0
%    T_k = spdiags([[beta(2:j);0] alpha(1:j) beta(1:j)],-1:1,j,j);
%    anorm = sqrt(norm(T_k'*T_k,1))
%  end
  if  est_anorm & beta(j+1)~=0
    anorm = update_gbound(anorm,alpha,beta,j);
  end

  % Update omega-recurrence
  if j>1 & ~fro & beta(j+1)~=0
    [omega,omega_old] = update_omega(omega,omega_old,j,alpha,beta,...
	eps1,anorm);
    omega_max(j) = max(abs(omega));
  end

  % Reorthogonalize if required
  if j>1 & (fro  | force_reorth | omega_max(j)>delta) & beta(j+1)~=0
    if fro
      int = 1:j;
    else
      if force_reorth == 0
	force_reorth= 1; % Do forced reorth to avoid spill-over from q_{j-1}.
	int = compute_int(omega,j,delta,eta,0,0,0);
      else
	force_reorth= 0;
      end
    end
    [r,beta(j+1),rr] = reorth(Q_k,r,beta(j+1),int,gamma,cgs);
    omega(int) = eps1;
    np = np + rr*length(int(:));    nr = nr + 1;
  else
    beta(j+1) = norm(r); % compute norm accurately.
  end

  if deflate
    [r,beta(j+1),rr] = reorth(options.Y,r,beta(j+1),1:size(options.Y,2), ...
			      gamma,cgs);
  end

  if  j<kmax & beta(j+1) < n*anorm*eps  ,
    % If beta is "small" we deflate by setting the off-diagonals of T_k
    % to 0 and attempt to restart with a basis for a new
    % invariant subspace by replacing r with a random starting vector:
    beta(j+1) = 0;
    bailout = 1;
    for attempt=1:3
      r = rand(n,1)-0.5;
      if isnumeric(A)
	r = A*r;
      elseif isstruct(A)
	r = A.U \ ( A.L \ r);
      else
	r = feval(A,r);
      end
      nrm=sqrt(r'*r); % not necessary to compute the norm accurately here.
      int = 1:j;
      [r,nrmnew,rr] = reorth(Q_k,r,nrm,int,gamma,cgs);
      omega(int) = eps1;
      np = np + rr*length(int(:));    nr = nr + 1;
      if nrmnew > 0
	% A vector numerically orthogonal to span(Q_k(:,1:j)) was found.
	% Continue iteration.
	bailout=0;
	break;
      end
    end
    if bailout
      ierr = -j;
      break;
    else
      r=r/nrmnew; % Continue with new normalized r as starting vector.
      force_reorth = 1;
      if delta>0
	fro = 0;    % Turn off full reorthogonalization.
      end
    end
  elseif j<kmax & ~fro & beta(j+1)*delta < anorm*eps1,
    % If anorm*eps1/beta(j+1) > delta then  omega(j+1) will
    % immediately exceed delta, and thus forcing a reorth. to occur at the
    % next step. The components of omega will mainly be determined
    % by the initial value and not the recurrence, and therefore we
    % cannot tell reliably which components exceed eta => we might
    % as well switch to full reorthogonalization to avoid trouble.
    % The user is probably trying to determine pathologically
    % small ( < sqrt(eps)*||A||_2 ) eigenvalues.
    %    warning(['Semiorthogonality cannot be maintained at iteration ', ...
    %	  num2str(j),'. The matrix is probably ill-conditioned.', ...
    %	  ' Switching to full reorthogonalization.'])
    fro = 1;
    ierr = j;
  end
end

% Set up tridiagonal T_k in sparse matrix data structure.
T_k = spdiags([[beta(2:j);0] alpha(1:j) beta(1:j)],-1:1,j,j);
if nargout<2
  Q_k = T_k;
elseif j~=size(Q_k,2)
  Q_k = Q_k(:,1:j);
end
work = [nr np];

function [omega,omega_old] = update_omega(omega, omega_old, j, ...
    alpha,beta,eps1,anorm)
% UPDATE_OMEGA:  Update Simon's omega_recurrence for the Lanczos vectors.
%
% [omega,omega_old] = update_omega(omega, omega_old,j,eps1,alpha,beta,anorm)
%

% Rasmus Munk Larsen, DAIMI, 1998.

% Estimate of contribution to roundoff errors from A*v
%   fl(A*v) = A*v + f,
% where ||f|| \approx eps1*||A||.
% For a full matrix A, a rule-of-thumb estimate is eps1 = sqrt(n)*eps.
T = eps1*anorm;
binv = 1/beta(j+1);

omega_old = omega;
% Update omega(1) using omega(0)==0.
omega_old(1)= beta(2)*omega(2)+ (alpha(1)-alpha(j))*omega(1) -  ...
    beta(j)*omega_old(1);
omega_old(1) = binv*(omega_old(1) + sign(omega_old(1))*T);
% Update remaining components.
k=2:j-2;
omega_old(k) = beta(k+1).*omega(k+1) + (alpha(k)-alpha(j)).*omega(k) ...
     + beta(k).*omega(k-1) - beta(j)*omega_old(k);
omega_old(k) = binv*(omega_old(k) + sign(omega_old(k))*T);
omega_old(j-1) = binv*T;
% Swap omega and omega_old.
temp = omega;
omega = omega_old;
omega_old = omega;
omega(j) =  eps1;

function anorm = update_gbound(anorm,alpha,beta,j)
%UPDATE_GBOUND   Update Gerscgorin estimate of 2-norm
%  ANORM = UPDATE_GBOUND(ANORM,ALPHA,BETA,J) updates the Gerscgorin bound
%  for the tridiagonal in the Lanczos process after the J'th step.
%  Applies Gerscgorins circles to T_K'*T_k instead of T_k itself
%  since this gives a tighter bound.

if j==1 % Apply Gerscgorin circles to T_k'*T_k to estimate || A ||_2
  i=j;
  % scale to avoid overflow
  scale = max(abs(alpha(i)),abs(beta(i+1)));
  alpha(i) = alpha(i)/scale;
  beta(i+1) = beta(i+1)/scale;
  anorm = 1.01*scale*sqrt(alpha(i)^2+beta(i+1)^2 + abs(alpha(i)*beta(i+1)));
elseif j==2
  i=1;
  % scale to avoid overflow
  scale = max(max(abs(alpha(1:2)),max(abs(beta(2:3)))));
  alpha(1:2) = alpha(1:2)/scale;
  beta(2:3) = beta(2:3)/scale;

  anorm = max(anorm, scale*sqrt(alpha(i)^2+beta(i+1)^2 + ...
      abs(alpha(i)*beta(i+1) + alpha(i+1)*beta(i+1)) + ...
      abs(beta(i+1)*beta(i+2))));
  i=2;
  anorm = max(anorm,scale*sqrt(abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1))) );
elseif j==3
  % scale to avoid overflow
  scale = max(max(abs(alpha(1:3)),max(abs(beta(2:4)))));
  alpha(1:3) = alpha(1:3)/scale;
  beta(2:4) = beta(2:4)/scale;
  i=2;
  anorm = max(anorm,scale*sqrt(abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1) + alpha(i+1)*beta(i+1)) + ...
      abs(beta(i+1)*beta(i+2))) );
  i=3;
  anorm = max(anorm,scale*sqrt(abs(beta(i)*beta(i-1)) + ...
      abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1))) );
else
  % scale to avoid overflow
  %  scale = max(max(abs(alpha(j-2:j)),max(abs(beta(j-2:j+1)))));
  %  alpha(j-2:j) = alpha(j-2:j)/scale;
  %  beta(j-2:j+1) = beta(j-2:j+1)/scale;

  % Avoid scaling, which is slow. At j>3 the estimate is usually quite good
  % so just make sure that anorm is not made infinite by overflow.
  i = j-1;
  anorm1 = sqrt(abs(beta(i)*beta(i-1)) + ...
      abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1) + alpha(i+1)*beta(i+1)) + ...
      abs(beta(i+1)*beta(i+2)));
  if isfinite(anorm1)
    anorm = max(anorm,anorm1);
  end
  i = j;
  anorm1 = sqrt(abs(beta(i)*beta(i-1)) + ...
      abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1)));
  if isfinite(anorm1)
    anorm = max(anorm,anorm1);
  end
end


load('.\faust_synthetic\distance_matrix\tr_reg_095.mat')
load('.\faust_synthetic\shapes\tr_reg_095.mat')
trisurf(TRIV,VERT(:,1),VERT(:,2),VERT(:,3),D(1,:)); colorbar; colormap flag; axis equal; shading interp; axis off
hold
scatter3(VERT(1,1),VERT(1,2),VERT(1,3),30,'g','filled');
title('Geodesic Distance from Source Point','FontSize',28)


function A = slkfd(K, nums, varargin)
%SLKFD Perform Kernelized Fisher Discriminant Analysis
%
% $ Syntax $
%   - A = slkfd(K, nums, ...)
%
% $ Arguments $
%   - K:        the kernel gram matrix of the training samples
%   - nums:     the numbers of samples in all classes
%   - A:        the projection coefficient matrix
%
% $ Description $
%   - A = slkfd(K, nums, ...) performs Kernerlized Fisher discriminant
%     analysis on the samples X according to the specified properties.
%     \*
%     \t   Table 1.  The properties of Fisher Discriminant Analysis   \\
%     \h     name     &     description                                \\
%           'sol'     &  The cell containing the arguments for solving
%                        the generalized eigen-problem by slsymgeig.
%                        default = {}.                                 \\
%           'dimset'  &  The cell containing the arguments for determining
%                        the output feature dimension. default = {}.
%                        (refer to sldim_by_eigval).                   \\
%           'Sb'      &  The pre-computed kernelized between-class
%                        scattering matrix or the cell containing
%                        the arguments for computing the kernelized
%                        scatter matrix in the form {type, ...},
%                        which is input to slkernelscatter.     \\
%           'Sw'      &  The pre-computed kernelized within-class
%                        scattering matrix or the cell containing
%                        the arguments for computing the scatter
%                        matrix in the form {type, ...}, which is
%                        input to slkernelscatter.     \\
%         'weights'   &  The sample weights. default = [].   \\
%     \*
%
% $ History $
%   - Created by Dahua Lin on May 03, 2006
%

%% parse and verify input arguments

if nargin < 2
    raise_lackinput('slkfd', 2);
end

% for K
n = size(K, 1);
if ~isequal(size(K), [n, n])
    error('sltoolbox:invaliddims', ...
        'K should be a square matrix');
end

% for nums
nc = length(nums);
if ~isequal(size(nums), [1 nc])
    error('sltoolbox:sizmismatch', ...
        'nums should be a 1 x nc row vector');
end

% for options

opts.sol = {};
opts.dimset = {};
opts.Sb = {'Sb'};
opts.Sw = {'Sw'};
opts.weights = [];
opts = slparseprops(opts, varargin{:});

has_Sb = ~isempty(opts.Sb) && isnumeric(opts.Sb);
has_Sw = ~isempty(opts.Sw) && isnumeric(opts.Sw);
if has_Sb
    Sb = opts.Sb;
    if ~isequal(size(Sb), [n n])
        error('sltoolbox:sizmismatch', ...
            'Sb should be a n x n matrix');
    end
end
if has_Sw
    Sw = opts.Sw;
    if ~isequal(size(Sw), [n n])
        error('sltoolbox:sizmismatch', ...
            'Sw should be a n x n matrix');
    end
end

if ~isempty(opts.weights)
    w = opts.weights;
    if ~isequal(size(w), [1 n])
        error('sltoolbox:sizmismatch', ...
            'The weights should be a 1 x n row vector');
    end
else
    w = [];
end

%% Compute

%% Step 1: Construct the eigen-problem

if ~has_Sb
    Sb = slscatter(K, opts.Sb{:}, 'sweights', w, 'nums', nums);
end

if ~has_Sw
    Sw = slscatter(K, opts.Sw{:}, 'sweights', w, 'nums', nums);
end

%% Step 2: Resolve the eigen-problem

[evs, A] = slsymgeig(Sb, Sw, opts.sol{:});

rk = sldim_by_eigval(evs, opts.dimset{:});
A = A(:, 1:rk);


function [C,S,a,c]=getEMMCoeffs(M,year,fullyNormalize)
%%GETEMMCOEFFS Obtain spherical harmonic coefficients for the 2017
%              version of the National Oceanic and Atmospheric
%              Administration's (NOAA's) Enchaned Magnetic Model (EMM) at a
%              particular time or at the reference epoch (2017). The model
%              is considered valid down to 10km underground.
%
%INPUTS: M The integer maximum order of the spherical harmonic coefficients
%          obtained. This is a value between 1 and 740. If this parameter
%          is omitted, the default value is 740. If one wishes to load all
%          coefficients, one can also just pass Inf.
%     year A decimal number indicating a year in the Gregorian calendar as
%          specified by universal coordinated time (UTC). For example,
%          halfway through the year 2017 would be represented as 2017.5.
%          The precision of the model is not sufficiently fine that leap
%          seconds matter. If this parameter is omitted, then the
%          reference year of the EMM model is used. In this instance,
%          2017.0.
% fullyNormalize Geomagnetic models are normally given in terms of Schmidt
%          semi-normalized Legendre functions. If fullyNormalize=true, then
%          the coefficients are converted for use with fully normalized
%          associated Legendre functions, as are commonly used with
%          spherical harmonic algorithms for gravitational models. If this
%          parameter is omitted, the default value is true.
%
%OUTPUTS: C An array holding the coefficient terms that are multiplied by
%           cosines in the harmonic expansion. This can be given to a
%           CountingClusterSet so that C(n+1,m+1) is the coefficient of
%           degree n and order m. The coefficients have units of Tesla. The
%           coefficients are normalized according to the fullyNormalize
%           term.
%         S An array holding the coefficient terms that are multiplied by
%           sines in the harmonic expansion. The format of S is the same as
%           that of C.
%         a The numerator in the (a/r)^n term in the spherical harmonic sum
%           having units of meters.
%         c The constant value by which the spherical harmonic series is
%           multiplied, having units of squared meters.
%
%Details on the normalization of the coefficients is given in the comments
%to the function spherHarmonicEval.
%
%This function first checks for a .mat file with the coefficients in it.
%The .mat file is small and can be read quite quickly. However, if one does
%not exist, then it tries to read the EMM2015.COF and EMM2015SV.COF text
%files that one can obtain directly from the NOAA. Reading from the text
%files is very slow.
%
%Documentation is in [1] and [2]. Documentation as well as some code
%containing containing the coefficients is available at
%http://www.ngdc.noaa.gov/geomag/EMM/
%Being created by the U.S. government, the model is not subject to
%copyright.
%
%The data is kept in zipped files in the ./data folder. If all of the data
%is being loaded for a particular year (M=Inf), then a .mat file will be
%created in the ./data folder with the data for that year so that it can be
%loaded more quickly in the future.
%
%REFERENCES:
%[1] S. Maus, "An ellipsoidal harmonic representation of Earth's
%    lithospheric magnetic field to degree and order 720," Geochemistry,
%    Geophysics, Geosystems, vol. 11, no. 6, Jun. 2010.
%[2] Information on Schmidt semi-normalized Legendre functions is given in
%    D. E. Winch, D. J. Ivers, J. P. R. Turner, and R. J. Stening,
%    "Geomagnetism and Schmidt quasi-normalization," Geophysical Journal
%    International, vol. 160, no. 2, pp. 487-504, Feb. 2005.
%
%June 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<1||isempty(M))
    M=Inf;%Load all of the coefficients.
end

yearRef=2015.0;
if(nargin<2||isempty(year))
    year=yearRef;
end

if(nargin<3||isempty(fullyNormalize))
    fullyNormalize=true;
end

saveMatFile=(M==Inf);

%The EMM2015 magnetic coefficient data file, should be located in a data
%folder that is in the same folder as this file. This find the path to this
%file.
ScriptPath=mfilename('fullpath');
ScriptFolder=fileparts(ScriptPath);

%We have to load the version of the EMM that predicts for the year
%given. To know which one to load, we will see which years are in the
%zip archive. The years are given in the filenames.
filePathAndName=fileNamesInZipArchive([ScriptFolder,'/data/EMM_Coefficients.zip']);
secVarFilePathAndName=fileNamesInZipArchive([ScriptFolder,'/data/EMM_Secular_Variations.zip']);

numFiles=length(filePathAndName);
EMMYears=zeros(numFiles,1);
for curFile=1:numFiles
    [~,fileName] = fileparts(filePathAndName{curFile});
    %The filenames all begin with EMM, so skip the first three
    %characters to get the years.
    EMMYears(curFile)=sscanf(fileName(4:end),'%f',1);
end

%We have to find the EMM model year that is equal to the integer part
%of year. If there is none, then we have to find the closest year.
yearIdx2Load=find(EMMYears==fix(year),1);
if(isempty(yearIdx2Load)||yearIdx2Load~=year)
    [~,yearIdx2Load]=min(abs(EMMYears-fix(year)));
end
yearRef=EMMYears(yearIdx2Load);

%First, see if a .mat file with all of the data for the appropriate year
%exists. If so, then use that to load the coefficients from which
%interpolation must be performed.
matFile=[ScriptFolder,'/data/EMM',int2str(yearRef),'.mat'];

if(exist(matFile,'file'))
    load(matFile,'CCoeffs','SCoeffs','C1Coeffs','S1Coeffs');

    %Keep only as many coefficients as the maximum order provided.
    totalNumCoeffs=(M+1)*(M+2)/2;
    if(length(CCoeffs)<totalNumCoeffs)
        totalNumCoeffs=length(CCoeffs);
    end
    C=CountingClusterSet(CCoeffs(1:totalNumCoeffs));
    S=CountingClusterSet(SCoeffs(1:totalNumCoeffs));
    M=C.numClust-1;

    %If the drift coefficients need to be reduced in size.
    totalNumDriftCoeffs=min(length(C1Coeffs),totalNumCoeffs);
    if(length(C1Coeffs)<totalNumCoeffs)
        totalNumDriftCoeffs=length(C1Coeffs);
    end

    C1=CountingClusterSet(C1Coeffs(1:totalNumDriftCoeffs));
    S1=CountingClusterSet(S1Coeffs(1:totalNumDriftCoeffs));
else
    %Otherwise, just read the data from the text files.

    %We now know which coefficients and secular variation terms to load.
    temp=readZipArchive([ScriptFolder,'/data/EMM_Coefficients.zip'],filePathAndName{yearIdx2Load});
    coefficientFile=char(temp{2});
    temp=readZipArchive([ScriptFolder,'/data/EMM_Secular_Variations.zip'],secVarFilePathAndName{yearIdx2Load});
    secVarTerms=char(temp{2});

    %The first line of the coefficient file is just a description, so find
    %the first occurence of a return character and only take the file after
    %that character
    startIdx=1;
    %char(10) is the newline character used in the file.
    while(strcmp(coefficientFile(startIdx),newline)~=1)
        startIdx=startIdx+1;
    end
    startIdx=startIdx+1;
    %Load all of the coefficients.
    dataCoeffs=sscanf(coefficientFile(startIdx:end),'%d %d %f %f',Inf);
    numCoeffEntries=length(dataCoeffs)/4;
    dataCoeffs=reshape(dataCoeffs,4,numCoeffEntries);

    %The number of coefficients.
    M=min(M,dataCoeffs(1,end));
    totalNumCoeffs=(M+1)*(M+2)/2;

    %Load all of the drift coefficients.
    driftCoeffs=sscanf(secVarTerms,'%d %d %f %f',Inf);
    numDriftEntries=length(driftCoeffs)/4;
    driftCoeffs=reshape(driftCoeffs,4,numDriftEntries);
    MDrift=driftCoeffs(1,end);
    totalNumDriftCoeffs=(MDrift+1)*(MDrift+2)/2;

    %Allocate space for the coefficients.
    emptyData=zeros(totalNumCoeffs,1);
    C=CountingClusterSet(emptyData);
    S=CountingClusterSet(emptyData);
    %These will hold the time-varying terms.
    emptyData=zeros(totalNumDriftCoeffs,1);
    C1=CountingClusterSet(emptyData);
    S1=CountingClusterSet(emptyData);

    %Put the data into the CountingClusterSet class instances.
    for curRow=1:numCoeffEntries
        n=dataCoeffs(1,curRow);%The degree of the coefficient.
        m=dataCoeffs(2,curRow);%The order of the coefficient.
        C(n+1,m+1)=dataCoeffs(3,curRow);
        S(n+1,m+1)=dataCoeffs(4,curRow);
    end

    for curRow=1:numDriftEntries
        n=driftCoeffs(1,curRow);%The degree of the coefficient.
        m=driftCoeffs(2,curRow);%The order of the coefficient.
        C1(n+1,m+1)=driftCoeffs(3,curRow);
        S1(n+1,m+1)=driftCoeffs(4,curRow);
    end

    %Change the units fron Nanotesla to Tesla.
    C(:)=10^(-9)*C(:);
    S(:)=10^(-9)*S(:);
    C1(:)=10^(-9)*C1(:);
    S1(:)=10^(-9)*S1(:);

    %If the coefficients should be saved into a .mat file for more
    %efficient loading in the future.
    if(saveMatFile)
        CCoeffs=C.clusterEls;
        SCoeffs=S.clusterEls;

        C1Coeffs=C1.clusterEls;
        S1Coeffs=S1.clusterEls;

        save(matFile,'CCoeffs','SCoeffs','C1Coeffs','S1Coeffs');
    end
end

%If interpolation to other dates must be performed.
if(year~=yearRef)
    yearDiff=year-yearRef;
    %Perform linear interpolation.
    idx=1:totalNumDriftCoeffs;
    C.clusterEls(idx)=C.clusterEls(idx)+yearDiff*C1.clusterEls(idx);
    S.clusterEls(idx)=S.clusterEls(idx)+yearDiff*S1.clusterEls(idx);
end

%If the coefficients should be fully normalized.
if(fullyNormalize~=false)
     for n=0:M
        k=1/sqrt(1+2*n);
        C(n+1,:)=k*C(n+1,:);
        S(n+1,:)=k*S(n+1,:);
     end
end

%Return S and C as arrays, not as a CountingClusterSet classes.
C=C.clusterEls;
S=S.clusterEls;

%The EMM2015 model uses the same reference ellipse as the WMM2010.
a=Constants.WMM2010SphereRad;%meters
c=a^2;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function row_sum = r8row_sum ( m, n, a )

%*****************************************************************************80
%
%% R8ROW_SUM returns the sums of an R8ROW.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), the R8ROW
%
%    Output, real ROW_SUM(M), the sum of the entries of
%    each row.
%
  for i = 1 : m
    row_sum(i) = sum ( a(i,1:n) );
  end

  return
end


function x = cheby_diff1_null_right ( m, n )

%*****************************************************************************80
%
%% CHEBY_DIFF1_NULL_RIGHT returns a right null vector of the CHEBY_DIFF1 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the order of A.
%
%    Output, real X(N,1), the null vector.
%
  x = zeros ( n, 1 );

  if ( mod ( n, 2 ) == 1 )
    x(1:n,1) = 1.0;
  end

  return
end


function out = SB_BinaryStats(y,binaryMethod)
% SB_BinaryStats    Statistics on a binary symbolization of the time series
%
% Binary symbolization of the time series is a symbolic string of 0s and 1s.
%
% Provides information about the coarse-grained behavior of the time series
%
%---INPUTS:
% y, the input time series
%
% binaryMethod, the symbolization rule:
%         (i) 'diff': by whether incremental differences of the time series are
%                      positive (1), or negative (0),
%         (ii) 'mean': by whether each point is above (1) or below the mean (0)
%         (iii) 'iqr': by whether the time series is within the interquartile range
%                      (1), or not (0).
%
%---OUTPUTS:
% Include the Shannon entropy of the string, the longest stretches of 0s
% or 1s, the mean length of consecutive 0s or 1s, and the spread of consecutive
% strings of 0s or 1s.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

%-------------------------------------------------------------------------------
% Check inputs, set defaults:
%-------------------------------------------------------------------------------
if nargin < 2 || isempty(binaryMethod)
    binaryMethod = 'diff';
end

%-------------------------------------------------------------------------------
% Binarize the time series:
%-------------------------------------------------------------------------------
yBin = BF_Binarize(y,binaryMethod);

N = length(yBin); % length of signal - 1 (difference operation)

%-------------------------------------------------------------------------------
% Stationarity of binarized time series:
%-------------------------------------------------------------------------------
% (cf. SB_MotifTwo for basic stats on binarized time series)

% Stationarity:
out.pupstat2 = sum(yBin(floor(end/2)+1:end) == 1)/sum(yBin(1:floor(end/2)) == 1);

%-------------------------------------------------------------------------------
% Consecutive string of ones / zeros (normalized by length)
%-------------------------------------------------------------------------------
difffy = diff(find([1;yBin;1]));
stretch0 = difffy(difffy ~= 1) - 1;

difffy = diff(find([0;yBin;0] == 0));
stretch1 = difffy(difffy ~= 1) - 1;

%-------------------------------------------------------------------------------
% pstretches
%-------------------------------------------------------------------------------
% Number of different stretches as proportion of the time-series length
out.pstretch1 = length(stretch1)/N;
% The following are trivially dependent on pstretch1:
% out.pstretch0 = length(stretch0)/N;
% out.pstretches = (length(stretch0)+length(stretch1))/N;

if isempty(stretch0) % all 1s (almost impossible to actually occur)
    out.longstretch0 = 0;
    out.longstretch0norm = 0;
    out.meanstretch0 = 0;
    out.meanstretch0norm = 0;
    out.stdstretch0 = NaN;
    out.stdstretch0norm = NaN;
else
    out.longstretch0 = max(stretch0); % longest consecutive stretch of zeros
    out.longstretch0norm = max(stretch0)/N; % longest consecutive stretch of zeros as proportion of time-series length
    out.meanstretch0 = mean(stretch0); % mean stretch of zeros
    out.meanstretch0norm = mean(stretch0)/N; % mean stretch of zeros
    out.stdstretch0 = std(stretch0); % standard deviation of stretch lengths of consecutive zeros
    out.stdstretch0norm = std(stretch0)/N; % standard deviation of stretch lengths of consecutive zeros
end

if isempty(stretch1) % all 0s (almost impossible to actually occur)
    out.longstretch1 = 0;
    out.longstretch1norm = 0;
    out.meanstretch1 = 0;
    out.meanstretch1norm = 0;
    out.stdstretch1 = NaN;
else
    out.longstretch1 = max(stretch1); % longest consecutive stretch of ones
    out.longstretch1norm = max(stretch1)/N; % longest consecutive stretch of ones as proportion of the time-series length
    out.meanstretch1 = mean(stretch1);
    out.meanstretch1norm = mean(stretch1)/N;
    out.stdstretch1 = std(stretch1);
    out.stdstretch1norm = std(stretch1)/N;
end

out.meanstretchdiff = (out.meanstretch1 - out.meanstretch0)/N;
out.stdstretchdiff = (out.stdstretch1 - out.stdstretch0)/N;

out.diff21stretch1 = mean(stretch1 == 2) - mean(stretch1 == 1);
out.diff21stretch0 = mean(stretch0 == 2) - mean(stretch0 == 1);

end


%CONFMAT Construct confusion matrix
%
%  [C,NE,LABLIST1,LABLIST2] = CONFMAT(LAB1,LAB2,METHOD,FID)
%
% INPUT
%  LAB1        Set of labels
%  LAB2        Set of labels
%  METHOD      'count' (default) to count number of co-occurences in
%	             LAB1 and LAB2, 'disagreement' to count relative
%		           non-co-occurrence.
%  FID         Write text result to file
%
% OUTPUT
%  C           Confusion matrix
%  NE          Total number of errors (empty labels are neglected)
%  LABLIST1    Label list for LAB1
%  LABLIST2    Label list for LAB2
%
% DESCRIPTION
% Constructs a confusion matrix C between two sets of labels LAB1
% (corresponding to the rows in C) and LAB2 (the columns in C). The order of
% the rows and columns is returned in LABLIST. NE is the total number of
% errors (sum of non-diagonal elements in C).
%
% If METHOD = 'count' (default), co-occurences in LAB1 and LAB2 are counted
% and returned in C.
% For METHOD = 'disagreement', the relative disagreement is returned in NE,
% and is split over all combinations of labels in C (such that the rows sum
% to 1). (The total disagreement for a class equals one minus the
% sensitivity for that class as computed by TESTC).
% For METHOD = 'ids' a cell array C is returned in which C(i,j) contains
% the indices of the objects for which LAB1 equals LABLIST1(i,:) and LAB2
% equals LABLIST2(j,:).
%
%   [C,NE,LABLIST] = CONFMAT(D,METHOD)
%
% If D is a classification result D = A*W, the labels LAB1 and LAB2 are
% internally retrieved by CONFMAT before computing the confusion matrix.
%
%   C = CONFMAT(D)
%
% This call also applies in case in D = A*W the dataset A has soft labels
% W is trained by a soft labeld classifier.
%
% When no output argument is specified, or when FID is given, the
% confusion matrix is displayed or written a to a text file. It is assumed
% that LAB1 contains true labels and LAB2 stores estimated labels.
%
% EXAMPLE
% Typical use of CONFMAT is the comparison of true and and estimated labels
% of a testset A by application to a trained classifier W:
% LAB1 = GETLABELS(A); LAB2 = A*W*LABELD.
% More examples can be found in PREX_CONFMAT, PREX_MATCHLAB.
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, GETLABELS, LABELD

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: confmat.m,v 1.8 2010/06/01 08:45:31 duin Exp $

function [CC,ne,lablist1,lablist2] = confmat (arg1,arg2,arg3,fid)

		% Check arguments.
  if nargin < 4, fid = 1; end
	if nargin < 3 | isempty(arg3)
		if isdataset(arg1)
			if islabtype(arg1,'crisp')
				lablist1 = getlablist(arg1);
				nlab1 = getnlab(arg1);
				lab2 = arg1*labeld;
				nlab2 = renumlab(lab2,lablist1); % try to fit on original lablist
				if any(nlab2==0)                 % doesn't fit:  contruct its own one
					[nlab2,lablist2] = renumlab(lab2);
				else
					lablist2 = lablist1;
				end
				if nargin < 2| isempty(arg2)
					method = 'count';
					prwarning(4,'no method supplied, assuming count');
				else
					method = arg2;
				end
			elseif islabtype(arg1,'soft')
				true_labs = gettargets(arg1);
				if any(true_labs > 1 | true_labs < 0)
					error('True soft labels should be between 0 and 1')
				end
				est_labs = +arg1;
				if any(est_labs > 1 | est_labs < 0)
					error('Estimated soft labels should be between 0 and 1')
				end
				C = true_labs'*est_labs;
				output(C,getlablist(arg1),getfeatlab(arg1),'soft',fid);
				return
			else
				error('Confusion matrix can only be computed for crisp and soft labeld datasets')
			end
		else
			method = 'count';
			prwarning(4,'no method supplied, assuming count');
			if (nargin < 2 | isempty(arg2))
				error('Second label list not supplied')
			end
			[nlab1,lablist1] = renumlab(arg1);
			[nlab2,lablist2] = renumlab(arg2);
		end
	else
		[nlab1,lablist1] = renumlab(arg1);
		[nlab2,lablist2] = renumlab(arg2);
		%lab1 = arg1;
		%lab2 = arg2;
		method = arg3;
	end
	if nargin < 2
		if ~isdataset(arg1)
			error('two labellists or one dataset should be supplied')
		end
	end

	% Renumber LAB1 and LAB2 and find number of unique labels.

	m = size(nlab1,1);
	if (m~=size(nlab2,1))
		error('LAB1 and LAB2 have to have the same lengths.');
	end
	n1 = size(lablist1,1);
	n2 = size(lablist2,1);
	n = max(n1,n2);

	% Construct matrix of co-occurences (confusion matrix).

  if strcmp(method,'ids')
    C = cell(n1+1,n2+1); % we need to store object id's
  else
    C = zeros(n1+1,n2+1);
  end
	for i = 0:n1
		K = find(nlab1==i);
		if (isempty(K))
      if strcmp(method,'ids')
        C(i+1,:) = repmat({[]},1,n2+1);
      else
        C(i+1,:) = zeros(1,n2+1);
      end
		else
			for j = 0:n2
        if strcmp(method,'ids')
          C(i+1,j+1) = {K(nlab2(K)==j)};
        else
          C(i+1,j+1) = sum(nlab2(K)==j);
        end
			end
		end
	end

	% position rejects and unlabeled object at the end of the matrix

	D = C;
	D(1:end-1,1:end-1) = C(2:end,2:end);
	D(end,:) = [C(1,2:end) C(1,1)];
	D(1:end-1,end) = C(2:end,1);
	C = D;
	D = D(1:end-1,1:end-1);
	DD = D(1:min(n1,n2),1:min(n1,n2));

	% Calculate number of errors ('count') or disagreement ('disagreement').
	% Neglect rejects

	switch (method)
		case {'count','ids'}
			J = find(nlab1~=0 & nlab2~=0);
			ne = nlabcmp(lablist1(nlab1(J),:),lablist2(nlab2(J),:));
		case 'disagreement'
			ne = (sum(sum(D)) - sum(diag(DD)))/m;  % Relative sum of off-diagonal
			ne = ne/m;														 % entries.
			E = repmat(sum(D,2),1,n2);             % Disagreement = 1 -
			D = ones(n1,n2)-D./E;                  % relative co-occurence.
			D = D / (n-1);
		otherwise
			error('unknown method');
  end

  if ~strcmp(method,'ids')
    %Distinguish 'rejects / no_labels' from 'non_rejects / fully labeled'
    if (any(C(:,end) ~= 0) | any(C(end,:)~=0)) & strcmp(method,'count')
      n1 = n1+1; n2 = n2+1;
      labch = char(strlab(lablist2),'reject');
      labcv = char(strlab(lablist1),'No');
    else
      labch = strlab(lablist2);
      labcv = strlab(lablist1);
      %labcv = labch;
      C = D;
    end
  end

    % If no output argument is specified, pretty-print C.

	if ((nargout == 0) || nargin == 4) && (~strcmp(method,'ids'))

		output(C,labcv,labch,method,fid)
	end

	if nargout > 0 || strcmp(method,'ids')
		CC = C;
	end

return

function output(C,labcv,labch,method,fid)

	n1 = size(labcv,1);
	n2 = size(labch,1);

		% Make sure labels are stored in LABC as matrix of characters,
    % max. 6 per label.

		if (size(labch,2) > 6)
			labch = labch(:,1:6);
			%labcv = labcv(:,1:6);
		end
		if (size(labch,2) < 5)
			labch = [labch repmat(' ',n2,ceil((5-size(labch,2))/2))];
			labcv = [labcv repmat(' ',n1,ceil((5-size(labcv,2))/2))];
		end

		%C = round(1000*C./repmat(sum(C,2),1,size(C,2)));

		nspace = max(size(labcv,2)-7,0);
		cspace = repmat(' ',1,nspace);
		%fprintf(fid,['\n' cspace '        | Estimated Labels']);
		fprintf(fid,['\n  True   ' cspace '| Estimated Labels']);
		fprintf(fid,['\n  Labels ' cspace '|']);
		for j = 1:n2, fprintf(fid,'%7s',labch(j,:)); end
		fprintf(fid,'|');
		fprintf(fid,' Totals');
		fprintf(fid,'\n ');
		fprintf(fid,repmat('-',1,8+nspace));
		fprintf(fid,'|%s',repmat('-',1,7*n2));
		fprintf(fid,'|-------');
		fprintf(fid,'\n ');

		for j = 1:n1
			fprintf(fid,' %-7s|',labcv(j,:));
			switch (method)
				case 'count'
					fprintf(fid,'%5i  ',C(j,:)');
					fprintf(fid,'|');
					fprintf(fid,'%5i',sum(C(j,:)));
				case 'disagreement'
					fprintf(fid,' %5.3f ',C(j,:)');
					fprintf(fid,'|');
					fprintf(fid,' %5.3f ',sum(C(j,:)));
				case 'soft'
					fprintf(fid,' %5.2f ',C(j,:)');
					fprintf(fid,'|');
					fprintf(fid,' %5.2f ',sum(C(j,:)));
			end
			fprintf(fid,'\n ');
		end

		fprintf(fid,repmat('-',1,8+nspace));
		fprintf(fid,'|%s',repmat('-',1,7*n2));
		fprintf(fid,'|-------');
		fprintf(fid,['\n  Totals ' cspace '|']);

		switch (method)
			case 'count'
				fprintf(fid,'%5i  ',sum(C));
				fprintf(fid,'|');
				fprintf(fid,'%5i',sum(C(:)));
			case 'disagreement'
				fprintf(fid,' %5.3f ',sum(C));
				fprintf(fid,'|');
			%	fprintf(fid,' %5.3f ',sum(C(:)));
			case 'soft'
				fprintf(fid,' %5.2f ',C(j,:)');
				fprintf(fid,'|');
				fprintf(fid,' %5.2f ',sum(C(:)));
		end
		fprintf(fid,'\n\n');
	return


function [Rlatt,Rrp,ind_rp,eff] = latmio_und(R,ITER,D)
%LATMIO_UND     Lattice with preserved degree distribution
%
%   [Rlatt,Rrp,ind_rp,eff] = latmio_und(R,ITER,D);
%
%   This function "latticizes" an undirected network, while preserving the
%   degree distribution. The function does not preserve the strength
%   distribution in weighted networks.
%
%   Input:      R,      undirected (binary/weighted) connection matrix
%               ITER,   rewiring parameter
%                       (each edge is rewired approximately ITER times)
%               D,      distance-to-diagonal matrix
%
%   Output:     Rlatt,  latticized network in original node ordering
%               Rrp,    latticized network in node ordering used for
%                       latticization
%               ind_rp, node ordering used for latticization
%               eff,    number of actual rewirings carried out
%
%   References: Maslov and Sneppen (2002) Science 296:910
%               Sporns and Zwi (2004) Neuroinformatics 2:145
%
%   2007-2012
%   Mika Rubinov, UNSW
%   Jonathan Power, WUSTL
%   Olaf Sporns, IU

%   Modification History:
%   Jun 2007: Original (Mika Rubinov)
%   Apr 2008: Edge c-d is flipped with 50% probability, allowing to explore
%             all potential rewirings (Jonathan Power)
%   Feb 2012: limit on number of attempts, distance-to-diagonal as input,
%             count number of successful rewirings (Olaf Sporns)
%   Feb 2012: permute node ordering on each run, to ensure lattices are
%             shuffled across mutliple runs (Olaf Sporns)

n=size(R,1);

% randomly reorder matrix
ind_rp = randperm(n);
R = R(ind_rp,ind_rp);

% create 'distance to diagonal' matrix
if nargin<3 %if D is not specified by user
    D=zeros(n);
    u=[0 min([mod(1:n-1,n);mod(n-1:-1:1,n)])];
    for v=1:ceil(n/2)
        D(n-v+1,:)=u([v+1:n 1:v]);
        D(v,:)=D(n-v+1,n:-1:1);
    end
end
%end create

[i,j]=find(tril(R));
K=length(i);
ITER=K*ITER;

% maximal number of rewiring attempts per 'iter'
maxAttempts= round(n*K/(n*(n-1)/2));
% actual number of successful rewirings
eff = 0;

for iter=1:ITER
    att=0;
    while (att<=maxAttempts)                                     %while not rewired
        while 1
            e1=ceil(K*rand);
            e2=ceil(K*rand);
            while (e2==e1),
                e2=ceil(K*rand);
            end
            a=i(e1); b=j(e1);
            c=i(e2); d=j(e2);

            if all(a~=[c d]) && all(b~=[c d]);
                break           %all four vertices must be different
            end
        end

        if rand>0.5
            i(e2)=d; j(e2)=c; 	%flip edge c-d with 50% probability
            c=i(e2); d=j(e2); 	%to explore all potential rewirings
        end

        %rewiring condition
        if ~(R(a,d) || R(c,b))
            %lattice condition
            if (D(a,b)*R(a,b)+D(c,d)*R(c,d))>=(D(a,d)*R(a,b)+D(c,b)*R(c,d))
                R(a,d)=R(a,b); R(a,b)=0;
                R(d,a)=R(b,a); R(b,a)=0;
                R(c,b)=R(c,d); R(c,d)=0;
                R(b,c)=R(d,c); R(d,c)=0;

                j(e1) = d;          %reassign edge indices
                j(e2) = b;
                eff = eff+1;
                break;
            end %lattice condition
        end %rewiring condition
        att=att+1;
    end %while not rewired
end %iterations

% lattice in node order used for latticization
Rrp = R;
% reverse random permutation of nodes
[~,ind_rp_reverse] = sort(ind_rp);
Rlatt = Rrp(ind_rp_reverse,ind_rp_reverse);


function h = drawCameras(R, C, scale)
    h = hggroup;
    for i = 1:size(C,2)
        Ci = C(:,i);
        Ri = R(:,:,i);
        if ~isequal(Ri, zeros(3))
            drawCamera(Ci, Ri, scale, h);
            text(Ci(1), Ci(2), Ci(3), num2str(i-1), 'Parent', h);
        end
    end
end

function h = drawCamera(C, R, windowScale, handle)
h = hggroup;
% windowScale - scalar, specify the distance between optical center to
% camera frontal face; equal to half of frontal face side length

assert(isequal(size(C), [3,1]), 'size of C must be 3x1!');

window11 = windowScale*[1; 1; 1];
window12 = windowScale*[-1; 1; 1];
window21 = windowScale*[-1;-1;1];
window22 = windowScale*[1; -1; 1];
windowPrime11 = R'*window11+C;
windowPrime12 = R'*window12+C;
windowPrime21 = R'*window21+C;
windowPrime22 = R'*window22+C;
AxisEndPoints = C + windowScale * R'; % 3x3, each column is an endpoint coordinates of each axis
windowVertices = [windowPrime11 windowPrime12 windowPrime22 windowPrime21]; % 3x4, each column is a vertex coornidates of the camera window

% draw three camera axis
line([C(1) AxisEndPoints(1,1)], [C(2) AxisEndPoints(2,1)], [C(3) AxisEndPoints(3,1)], 'Color', 'r', 'LineWidth', 1, 'Parent', h);
line([C(1) AxisEndPoints(1,2)], [C(2) AxisEndPoints(2,2)], [C(3) AxisEndPoints(3,2)], 'Color', 'g', 'LineWidth', 1, 'Parent', h);
line([C(1) AxisEndPoints(1,3)], [C(2) AxisEndPoints(2,3)], [C(3) AxisEndPoints(3,3)], 'Color', 'b', 'LineWidth', 1, 'Parent', h);

% draw square window
line([windowPrime11(1), windowPrime12(1), windowPrime21(1), windowPrime22(1), windowPrime11(1)],...
    [windowPrime11(2), windowPrime12(2), windowPrime21(2), windowPrime22(2), windowPrime11(2)], ...
    [windowPrime11(3), windowPrime12(3), windowPrime21(3), windowPrime22(3), windowPrime11(3)], 'Color', [0.5 0.5 0.5], 'Parent', h);
% draw lines connecting camera center to four vertices of the square window
line([windowPrime11(1) C(1)], [windowPrime11(2) C(2)], [windowPrime11(3) C(3)], 'Color', [0.5 0.5 0.5], 'Parent', h);
line([windowPrime12(1) C(1)], [windowPrime12(2) C(2)], [windowPrime12(3) C(3)], 'Color', [0.5 0.5 0.5], 'Parent', h);
line([windowPrime22(1) C(1)], [windowPrime22(2) C(2)], [windowPrime22(3) C(3)], 'Color', [0.5 0.5 0.5], 'Parent', h);
line([windowPrime21(1) C(1)], [windowPrime21(2) C(2)], [windowPrime21(3) C(3)], 'Color', [0.5 0.5 0.5], 'Parent', h);

if exist('handle', 'var')
    h.Parent = handle;
end

end


function linefit
zoom off
k = waitforbuttonpress;
point1 = get(gca,'CurrentPoint');    % button down detected
finalRect = rbbox;                   % return Figure units
point2 = get(gca,'CurrentPoint');    % button up detected
point1 = point1(1,1:2);              % extract x and y
point2 = point2(1,1:2);
p1 = min(point1,point2);             % calculate locations
offset = abs(point1-point2);         % and dimensions
x1 = p1(1);
x2 = p1(1)+offset(1);

zoom on
linehandle = findobj(gca, 'Type', 'line');
if ~isempty(linehandle)
    xdata = get(linehandle(1), 'XData');
    ydata = get(linehandle(1), 'YData');
    ind = find(xdata >=x1  & xdata <= x2);
    [p,s] = polyfit(xdata(ind), ydata(ind), 1);
    if p(2) >= 0
        msgbox(['y = ' num2str(p(1)) ' * x + ' num2str(p(2))], 'Line fit');
    else
        msgbox(['y = ' num2str(p(1)) ' * x - ' num2str(abs(p(2)))], 'Line fit');
    end
end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2019.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtFfmBuilderFem.m                            |
%|    #    |   VERSION    : 0.61                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2019                                    |
%|  / 0 \  |   LAST MODIF : 05.09.2019                                    |
%| ( === ) |   SYNOPSIS   : Non regression test for convolution using     |
%|  `---'  |                arbitrary kenel integral galerkin formulation |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('~~~~~~~~~~~~~ DEFINITIONS ~~~~~~~~~~~~~')

% Dimensions
Nx = 1e3
Ny = 2e3

% Accuracy
tol = 1e-3

% Meshes
Xmsh = mshSphere(Nx,1);
Ymsh = mshCube(Ny,2*[1 1 1]);
Ymsh = Ymsh.bnd;

% Domain
Xdom = dom(Xmsh,3);
Ydom = dom(Ymsh,3);

% Finite element
Xfem = fem(Xmsh,'P1');
Yfem = fem(Ymsh,'P1');

% Wave number or frequency (Hz)
stp = Xmsh.stp;
k   = 5

% Green kernel
green = '[exp(ikr)/r]'
Gxy   = @(X,Y) femGreenKernel(X,Y,green,k);

% Charges
V = (-1+2*rand(length(Yfem),1)) + (-1+2i*rand(length(Yfem),1));

% Spatial representation of particles
figure
plot(Xmsh,'b')
hold on
plot(Ymsh,'r')
alpha(0.5)
axis equal

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FULL PRODUCT %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('~~~~~~~~~~~~~ FULL PRODUCT ~~~~~~~~~~~~~')

% Full Matrix
tic
M = integral(Xdom,Ydom,Xfem,Gxy,Yfem);
toc

% Matrix-vector product
tic
ref = M * V;
toc

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FFM PRODUCT %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('~~~~~~~~~~~~~ FFM PRODUCT ~~~~~~~~~~~~~')

% FFM
tic
Mv = integral(Xdom,Ydom,Xfem,green,k,Yfem,tol);
toc

% FFM Matrix-vector product
tic
sol = Mv * V;
toc

% Error
norm(ref-sol)/norm(ref)

disp('~~> Michto gypsilab !')


function SNR = VBA_spm_getSNR(y,u,hrf)

% Computes fMRI SNR from controlled time windows
% function SNR = spm_getSNR(y,uu,hrf)
% IN:
%   - y: the nXt1 ROI time series
%   - u: the nuXt2 input (experimental control)
%   - hrf: the hrf (should be same sampling rate than y)
% OUT:
%   - SNR: the SNR (power ratio: signal vs noise)

ru = VBA_spm_resample(full(u),size(y,2)./size(u,2));
nu = size(u,1);
for i=1:nu
    tmp = conv(ru(i,:),hrf);
    tmp = tmp./std(tmp);
    X(:,i) = tmp(2:size(y,2)+1)';
end
iX = pinv(X'*X)*X';
ind = find(abs(mean(ru,1))>1e-1);
nreg = size(y,1);
for i=1:nreg
    beta = iX*y(i,:)';
    yc = X*beta;
    yp(:,i) = yc(ind);
    np(:,i) = y(i,ind)'-yc(ind);
end
SNR = sum(yp(:).^2)./sum(np(:).^2);


function I_s = segImage(I,S)

    S = single(S);
    [cx,cy] = gradient(S);
    ccc = (abs(cx)+abs(cy))~=0;
    ccc = uint8(ccc)*255;
    I_s = I;
    I_s(:,:,1) = max(I_s(:,:,1),ccc);
    I_s(:,:,2) = min(I_s(:,:,2),255-ccc);
    I_s(:,:,3) = min(I_s(:,:,3),255-ccc);


function [y,Jyl,Jyr,ilocl,ilocr,evalcnt]=greedy2_cross(n, fun, tol, varargin)
% Two-site greedy cross interpolation scheme.
%   [y,Jyl,Jyr,ilocl,ilocr]=greedy2_cross(n, fun, tol, varargin)
% Tries to interpolate the tensor with mode sizes n specified by the
% function fun up to the accuracy tol using the "basic" greedy restricted
% cross interpolation algorithm in the TT format.
% The method sequentially adds one pivot to each superblock in a forward
% DMRG-type half-sweep (k=1,...,d).
%
% The input n should be a vector of mode sizes of length d,
% fun = @(ind)fun(ind) is a sought function of index ind.
% By default, ind is an array of d indices, and the function fun should
% return one value of the corresponding tensor entry.
% To speed up the computations, set the optional parameter 'vec' to true,
% and provide the function which takes ind as an array of sizes M x d, and
% returns an array of M values.
%
% In addition to the indexwise function, one may provide the value-wise
% function of another tt_tensor (funcrs style) via optional parameters
% 'aux', 'auxfun' (see below).
% The resulting tensor is the sum    y(ind)=fun(ind) + auxfun(aux(ind)).
%
% The first output parameter is the computed tt_tensor, the rest four
% parameters return the pivot indices (left-global, right-global, left-local, right-local) for
% testing purposes.
%
% Optional arguments are provided in the form
% 'PropertyName1',PropertyValue1,'PropertyName2',PropertyValue2 and so on.
% The list of option names and default values:
%       o nswp - maximal number of sweeps [20]
%       o tol_exit - stopping difference between consecutive iterations [tol]
%       o verb - verbosity level, 0-silent, 1-sweep info, 2-block info [1]
%       o vec - whether fun can accept and return vectorized values [false]
%       o aux - set of tt_tensors for auxiliary funcrs contribution []
%       o auxfun - an auxiliary function defined pointwise at the elements
%           of aux []
%       o 'locsearch' - an algorithm for the error pivoting in superblocks:
%           try a lottery of n*r random elements ('lot'), or
%           conduct two-dimensional maxvol ALS iteration ('als').
%           The first one uses less evaluations, but may be costly due to a
%           nonvectorized MATLAB loop ['lot']
%
%
% This procedure implements Algorithm 2 from
%   D. Savostyanov, Quasioptimality of maximum-volume cross interpolation of tensors, Linear Algebra Applications 458, pp 217-244, 2014.
%    http://dx.doi.org/10.1016/j.laa.2014.06.006
% Please cite this paper if your research benefits from the use of this code.
% Development of the MATLAB version: S. Dolgov.
% Please send feedback to: {dmitry.savostyanov,sergey.v.dolgov}@gmail.com
%
%---------------------------

if (~isempty(varargin))
    v1 = varargin{1};
    if (isa(v1, 'cell'))
        varargin=v1;
    end;
end;
vars = varargin;

nswp = 20;
tol_exit = tol;
verb = 1;
vec = false;
aux = []; % Extra tt_tensors to pass into this cross
auxfun = []; % the total function equals fun(ind)+auxfun(aux(ind)), since simple aux(ind) via tt_tensor/subsref suxx.
% locsearch = 'als';
locsearch = 'lot';
y0 = [];

i = 1;
while (i<length(vars))
    switch lower(vars{i})
        case 'nswp'
            nswp=vars{i+1};
        case 'tol_exit'
            tol_exit=vars{i+1};
        case 'verb'
            verb = vars{i+1};
        case 'vec'
            vec = vars{i+1};
        case 'aux'
            aux = vars{i+1};
        case 'auxfun'
            auxfun = vars{i+1};
        case 'locsearch'
            locsearch = vars{i+1};
        case 'xtru'
            xtru = vars{i+1};
        case 'y0'
            y0 = vars{i+1};
        otherwise
            warning('Option %s was not recognized', vars{i});
    end;
    i=i+2;
end;

d = numel(n);
ry = ones(d+1,1);
y = cell(d,1);

if (~isempty(aux))
    if (isa(aux, 'tt_tensor'))
        raux = aux.r;
        aux = core2cell(aux);
        phiauxl = cell(d+1,1); phiauxl{1}=1; phiauxl{d+1}=1;
        phiauxr = cell(d+1,1); phiauxr{1}=1; phiauxr{d+1}=1;
%         for i=2:d
%             phiauxl{i} = ones(1,raux(i));
%             phiauxr{i} = ones(raux(i),1);
%         end;
        Raux = 1;
    else
        Raux = size(aux, 2);
        raux = ones(d+1,Raux);
        aux = [aux; cell(d-1, Raux)];
        phiauxl = cell(d+1,Raux);
        phiauxr = cell(d+1,Raux);
        for j=1:Raux
            raux(:,j) = aux{1,j}.r;
            aux(:,j) = core2cell(aux{1,j});
            phiauxl{1,j}=1; phiauxl{d+1,j}=1;
            phiauxr{1,j}=1; phiauxr{d+1,j}=1;
%             for i=2:d
%                 phiauxl{i,j} = ones(1,raux(i,j));
%                 phiauxr{i,j} = ones(raux(i,j),1);
%             end;
        end;
    end;
end;

% Factorized inverse interpolation matrix -- in the form U^{-1}, L^{-1}
mid_inv = cell(d+1,2); mid_inv{1,1}=1; mid_inv{1,2}=1; mid_inv{d+1,1}=1; mid_inv{d+1,2}=1;

Jyr = cell(d+1,1);
Jyl = cell(d+1,1);
ilocr = cell(d+1,1);
ilocl = cell(d+1,1);

evalcnt = 0;

% Start with some rand indices
for i=1:d-1
    if (isempty(y0))
        ilocl{i+1} = rand(1,1)*ry(i)*n(i);
        ilocl{i+1} = round(ilocl{i+1});
        if (ilocl{i+1}==0)
            ilocl{i+1} = 1;
        end;
    else
        ilocl{i+1} = y0(i);
    end
    Jyl{i+1} = indexmerge(Jyl{i}, (1:n(i))');
    Jyl{i+1} = Jyl{i+1}(ilocl{i+1},:);

    if (~isempty(aux))
        for j=1:Raux
            craux1 = reshape(aux{i,j}, raux(i,j), n(i)*raux(i+1,j));
            craux1 = phiauxl{i,j}*craux1;
            craux1 = reshape(craux1, ry(i)*n(i), raux(i+1,j));
            phiauxl{i+1,j} = craux1(ilocl{i+1}, :);
        end;
    end;
end;
% Perform one sweep back-forth to fix the initial index
for i=d:-1:1
    J = indexmerge(Jyl{i}, (1:n(i))', Jyr{i+1});
    evalcnt = evalcnt + size(J,1);
    cry1 = autovecfun(fun, J, vec);
    if (~isempty(aux))
        craux = zeros(ry(i)*n(i)*ry(i+1), Raux);
        for j=1:Raux
            craux1 = reshape(aux{i,j}, raux(i,j), n(i)*raux(i+1,j));
            craux1 = phiauxl{i,j}*craux1;
            craux1 = reshape(craux1, ry(i)*n(i), raux(i+1,j));
            craux1 = craux1*phiauxr{i+1,j};
            craux(:,j) = reshape(craux1, ry(i)*n(i)*ry(i+1), 1);
        end;
        cry1 = cry1+auxfun(craux);
    end;

    if (i>1)
        cry1 = reshape(cry1, ry(i), n(i)*ry(i+1));
        [v,ilocr{i}] = max(abs(cry1.'));
        ilocr{i} = ilocr{i}(:);
        mid_inv{i,1} = 1/cry1(:,ilocr{i});
        mid_inv{i,2} = 1;
        Jyr{i} = indexmerge((1:n(i))', Jyr{i+1});
        Jyr{i} = Jyr{i}(ilocr{i},:);

        if (~isempty(aux))
            for j=1:Raux
                phiauxr{i,j} = reshape(aux{i,j}, raux(i,j)*n(i), raux(i+1,j));
                phiauxr{i,j} = phiauxr{i,j}*phiauxr{i+1,j};
                phiauxr{i,j} = reshape(phiauxr{i,j}, raux(i,j), n(i)*ry(i+1));
                phiauxr{i,j} = phiauxr{i,j}(:, ilocr{i});
            end;
        end;
    end;

    y{i} = reshape(cry1, ry(i), n(i), ry(i+1));
end;
for i=1:d
    J = indexmerge(Jyl{i}, (1:n(i))', Jyr{i+1});
    evalcnt = evalcnt + size(J,1);
    cry1 = autovecfun(fun, J, vec);
    if (~isempty(aux))
        craux = zeros(ry(i)*n(i)*ry(i+1), Raux);
        for j=1:Raux
            craux1 = reshape(aux{i,j}, raux(i,j), n(i)*raux(i+1,j));
            craux1 = phiauxl{i,j}*craux1;
            craux1 = reshape(craux1, ry(i)*n(i), raux(i+1,j));
            craux1 = craux1*phiauxr{i+1,j};
            craux(:,j) = reshape(craux1, ry(i)*n(i)*ry(i+1), 1);
        end;
        cry1 = cry1+auxfun(craux);
    end;

    if (i<d)
        cry1 = reshape(cry1, ry(i)*n(i), ry(i+1));
        [v,ilocl{i+1}] = max(abs(cry1));
        mid_inv{i+1,1} = 1/cry1(ilocl{i+1});
        mid_inv{i+1,2} = 1;
        Jyl{i+1} = indexmerge(Jyl{i}, (1:n(i))');
        Jyl{i+1} = Jyl{i+1}(ilocl{i+1},:);
        if (~isempty(aux))
            for j=1:Raux
                phiauxl{i+1,j} = reshape(aux{i,j}, raux(i,j), n(i)*raux(i+1,j));
                phiauxl{i+1,j} = phiauxl{i,j}*phiauxl{i+1,j};
                phiauxl{i+1,j} = reshape(phiauxl{i+1,j}, ry(i)*n(i), raux(i+1,j));
                phiauxl{i+1,j} = phiauxl{i+1,j}(ilocl{i+1}, :);
            end;
        end;
    end;

    y{i} = reshape(cry1, ry(i), n(i), ry(i+1));
end;

last_sweep = false;
maxy = 0;
max_dx = 0;
swp = 1;
dir = 1;
i = 1;
while (swp<=nswp)
    % Generate a random test set for new indices
    % Prepare candidate index sets--the ones with the current crosses excluded
    cind1 = (1:ry(i)*n(i))';
    cind2 = (1:n(i+1)*ry(i+2))';
    cind1(ilocl{i+1})=[];
    cind2(ilocr{i+1})=[];

    if (strcmp(locsearch, 'als'))
        testsz = min(numel(cind1), numel(cind2));
    else
        %%% % rn Lottery
        % Now draw random entries
        tind = rand(min(numel(cind1), numel(cind2)), 1)*numel(cind1)*numel(cind2);
        tind = round(tind);
        tind(tind>numel(cind1)*numel(cind2))=numel(cind1)*numel(cind2);
        tind(tind<1)=1;
        tind = unique(tind);
        testsz = size(tind,1);
    end;

    if (~isempty(aux))
        craux1 = cell(1,Raux);
        craux2 = cell(1,Raux);
        for j=1:Raux
            craux = reshape(aux{i,j}, raux(i,j), n(i)*raux(i+1,j));
            craux = phiauxl{i,j}*craux;
            craux1{j} = reshape(craux, ry(i)*n(i), raux(i+1,j));
            craux = reshape(aux{i+1,j}, raux(i+1,j)*n(i+1), raux(i+2,j));
            craux = craux*phiauxr{i+2,j};
            craux2{j} = reshape(craux, raux(i+1,j), n(i+1)*ry(i+2));
        end;
    end;

    % Check that we are not in the full rank case
    if (testsz>0)
        if (strcmp(locsearch, 'als'))
            % 2D ALS cross
            % Evaluate y at tind
            cry1 = reshape(y{i}, ry(i)*n(i), ry(i+1));
            cry2 = reshape(y{i+1}, ry(i+1), n(i+1)*ry(i+2));
            ys1 = cry1*mid_inv{i+1,1};
            ys1 = ys1(cind1,:);
            ys2 = mid_inv{i+1,2}*cry2;
            ys2 = ys2(:,cind2);
            % Full indices
            J1 = indexmerge(Jyl{i}, (1:n(i))');
            J1c = J1(cind1,:);
            J2 = indexmerge((1:n(i+1))', Jyr{i+2});
            J2c = J2(cind2,:);
            rz = 2;
            cre2 = randn(numel(cind2), rz);
            [cre2,rv]=qr(cre2,0);
            indr = maxvol2(cre2);
            Jr = J2c(indr,:);
            Ye2 = ys2(:,indr);

            J = indexmerge(J1c,Jr);
            evalcnt = evalcnt + size(J,1);
            cre1 = autovecfun(fun, J, vec);
            if (~isempty(aux))
                craux = zeros(numel(cind1)*numel(indr), Raux);
                for j=1:Raux
                    craux(:,j) = reshape(craux1{j}(cind1,:)*craux2{j}(:,cind2(indr)), [], 1);
                end;
                craux = auxfun(craux);
                cre1 = cre1+craux;
            end;
            maxy = max(maxy, max(abs(cre1)));
            cre1 = reshape(cre1, numel(cind1), rz);
            cre1 = cre1-ys1*Ye2;
            [zmax1,imaxnew]=max(abs(cre1(:)));
            imaxnew = tt_ind2sub([numel(cind1), rz], imaxnew);
            imax1 = imaxnew(1);
            [cre1,rv]=qr(cre1,0);
            indl = maxvol2(cre1);
            Jl = J1c(indl,:);
            Ye1 = ys1(indl,:);

            J = indexmerge(Jl,J2c);
            evalcnt = evalcnt + size(J,1);
            cre2 = autovecfun(fun, J, vec);
            if (~isempty(aux))
                craux = zeros(numel(indl)*numel(cind2), Raux);
                for j=1:Raux
                    craux(:,j) = reshape(craux1{j}(cind1(indl),:)*craux2{j}(:,cind2), [], 1);
                end;
                craux = auxfun(craux);
                cre2 = cre2+craux;
            end;
            maxy = max(maxy, max(abs(cre2)));
            cre2 = reshape(cre2, rz, numel(cind2));
            cre2 = cre2-Ye1*ys2;
            [zmax2,imaxnew]=max(abs(cre2(:)));
            imaxnew = tt_ind2sub([rz, numel(cind2)], imaxnew);
            imax2 = imaxnew(2);
            imax1 = cind1(imax1);
            imax2 = cind2(imax2);
            emax = max(zmax1,zmax2);
        else
            %%% % rn Lottery
            tind = tt_ind2sub([numel(cind1), numel(cind2)], tind);
            tind = [cind1(tind(:,1)), cind2(tind(:,2))];
            % Full indices
            J1 = indexmerge(Jyl{i}, (1:n(i))');
            J1c = J1(tind(:,1),:);
            J2 = indexmerge((1:n(i+1))', Jyr{i+2});
            J2c = J2(tind(:,2),:);
            J = [J1c,J2c];
            % Evaluate the function
            evalcnt = evalcnt + testsz;
            crt = autovecfun(fun, J, vec);
            if (~isempty(aux))
                craux = zeros(testsz, Raux);
                for k=1:Raux
                    for j=1:testsz
                        craux(j,k) = craux1{k}(tind(j,1),:)*craux2{k}(:,tind(j,2));
                    end;
                end;
                crt = crt+auxfun(craux);
            end;
            maxy = max(maxy, max(abs(crt)));
            % Evaluate y at tind
            cry1 = reshape(y{i}, ry(i)*n(i), ry(i+1));
            cry2 = reshape(y{i+1}, ry(i+1), n(i+1)*ry(i+2));
            % Subtract the current approx.
            cry = zeros(testsz, 1);
            % Apply the inverse interp at the middle
            cre1 = cry1*mid_inv{i+1,1};
            cre2 = mid_inv{i+1,2}*cry2;
            for j=1:testsz
                cry(j) = cre1(tind(j,1),:)*cre2(:,tind(j,2));
            end;
            cre = crt-cry;
            % Take the max-err index to enrich
            [emax,imax2] = max(abs(cre));

            % All again: at (:,tind(imax,2)), run the error maximization
            J1c = J1(cind1,:);
            J = indexmerge(J1c, J2c(imax2,:));
            evalcnt = evalcnt + size(J,1);
            crt = autovecfun(fun, J, vec);
            if (~isempty(aux))
                craux = zeros(numel(cind1), Raux);
                for j=1:Raux
                    craux(:,j) = craux1{j}(cind1,:)*craux2{j}(:,tind(imax2,2));
                end;
                crt = crt+auxfun(craux);
            end;
            maxy = max(maxy, max(abs(crt)));
            % Subtract the current approx.
            cry = cre1(cind1,:)*cre2(:,tind(imax2,2));
            cre = crt-cry;
            % Take the max-err index to enrich
            [emax,imax1] = max(abs(cre));
            imax1 = cind1(imax1);
            imax2 = tind(imax2,2);
            % imax1 samples from ry(i)*n(i)
            % imax2 samples from n(i+1)*ry(i+2)
        end;

        dx = emax/maxy;
        max_dx = max(max_dx,dx);
        if (verb>1)
            fprintf('=greedy_cross= i=%d, swp=%d, testsz=%d, emax=%3.3e, dx=%3.3e, cond=[%3.3e,%3.3e]\n', i, swp, testsz, emax, dx, cond(mid_inv{i+1,1}), cond(mid_inv{i+1,2}));
        end;

        if (dx>tol)
            % Now evaluate new factors
            J1m = J1(imax1,:);
            J2m = J2(imax2,:);
            % Generate a full index = [Jl, (1:n)', Jr]
            % Jyl to lowest index           n(i) to the middle one,                      Jyr to the senior index
            Jl = indexmerge(J1, J2m);
            Jr = indexmerge(J1m, J2);

            evalcnt = evalcnt + size(Jl,1);
            cre1 = autovecfun(fun, Jl, vec);
            if (~isempty(aux))
                craux = zeros(ry(i)*n(i), Raux);
                for j=1:Raux
                    craux(:,j) = craux1{j}*craux2{j}(:,imax2);
                end;
                cre1 = cre1+auxfun(craux);
            end;

            evalcnt = evalcnt + size(Jr,1);
            cre2 = autovecfun(fun, Jr, vec);
            if (~isempty(aux))
                craux = zeros(n(i+1)*ry(i+2), Raux);
                for j=1:Raux
                    craux(:,j) = craux1{j}(imax1,:)*craux2{j};
                end;
                cre2 = cre2+auxfun(craux);
            end;

            cre1 = reshape(cre1, ry(i)*n(i), 1);
            cre2 = reshape(cre2, 1, n(i+1)*ry(i+2));

            % Expand blocks
            y{i} = [cry1, cre1];
            y{i+1} = [cry2; cre2];
            ry(i+1)=ry(i+1)+1;
            % ilocl{i+2} is smashed now, since ry(i+1) changed. Recover...
            if (i<d-1)
                ii = floor((ilocl{i+2}-1)/(ry(i+1)-1));
                ilocl{i+2} = ilocl{i+2}+ii;
            end;
            % Expand indices
            Jyl{i+1} = [Jyl{i+1}; J1m];
            Jyr{i+1} = [Jyr{i+1}; J2m];
            ilocl{i+1} = [ilocl{i+1}; imax1];
            ilocr{i+1} = [ilocr{i+1}; imax2];
            % We know that the inverse LU may be written analytically
            uold = mid_inv{i+1,1};
            lold = mid_inv{i+1,2};
            % Expanding vectors
            erow = cry1(imax1, :);
            ecol = cre1(ilocl{i+1}(1:ry(i+1)-1), 1);
            % ..and the scalar
            eel = cre1(imax1,1);
            ecol = lold*ecol;
            erow = erow*uold;
            eel = eel - erow*ecol; % alpha-dA^{-1}c
            ecol = uold*ecol; % A^{-1} c
            erow = erow*lold; % d A^{-1}
            % New inv(U)
            mid_inv{i+1,1} = zeros(ry(i+1), ry(i+1));
            mid_inv{i+1,1}(1:ry(i+1)-1, 1:ry(i+1)-1)=uold;
            mid_inv{i+1,1}(1:ry(i+1)-1, ry(i+1)) = -ecol/eel;
            mid_inv{i+1,1}(ry(i+1), ry(i+1))=1/eel;
            % New inv(L)
            mid_inv{i+1,2} = zeros(ry(i+1), ry(i+1));
            mid_inv{i+1,2}(1:ry(i+1)-1, 1:ry(i+1)-1)=lold;
            mid_inv{i+1,2}(ry(i+1), 1:ry(i+1)-1) = -erow;
            mid_inv{i+1,2}(ry(i+1), ry(i+1))=1;

            y{i} = reshape(y{i}, ry(i), n(i), ry(i+1));
            y{i+1} = reshape(y{i+1}, ry(i+1), n(i+1), ry(i+2));
        end;
    end;

    if (~isempty(aux))
        for j=1:Raux
            phiauxl{i+1,j} = craux1{j}(ilocl{i+1}, :);
            phiauxr{i+1,j} = craux2{j}(:, ilocr{i+1});
        end;
    end;

    i = i+dir;

    % Check the convergence, restart
    if ((i==d)||(i==0))
        if (verb>0)
            if (exist('xtru'))
                ytest=formtensor(y,mid_inv,d,ry,n);
                fprintf('=greedy_cross= swp=%d, max_dx=%3.3e, max_rank=%d, cum#evals=%d, err_tru=%3.3e\n', swp, max_dx, max(ry), evalcnt, norm(ytest-xtru)/norm(xtru));
            else
                fprintf('=greedy_cross= swp=%d, max_dx=%3.3e, max_rank=%d, cum#evals=%d\n', swp, max_dx, max(ry), evalcnt);
            end;
        end;

        if (max_dx<tol_exit)
            break;
        end;

        max_dx = 0;
        i=1;
        swp = swp+1;
    end;
end;

y=formtensor(y,mid_inv,d,ry,n);
end

function [y]=formtensor(y,mid_inv,d,ry,n)
% Merge mid_inv: y:=inv(L)*y*inv(U)
for i=1:d
    y{i} = reshape(y{i}, ry(i), n(i)*ry(i+1));
    y{i} = mid_inv{i,2}*y{i};
    y{i} = reshape(y{i}, ry(i)*n(i), ry(i+1));
    y{i} = y{i}*mid_inv{i+1,1};
    y{i} = reshape(y{i}, ry(i), n(i), ry(i+1));
end;
y = cell2core(tt_tensor,y);
end

function [y]=autovecfun(fun, J, vec)
if (vec)
    y = fun(J); % User can cast M x d -> M x 1
else
    % They can't -- run a loop manually
    sz = size(J,1);
    y = zeros(sz, 1);
    for j=1:sz
        y(j) = fun(J(j,:));
    end;
end;
end

function [J]=indexmerge(varargin)
% Merges two or three indices in the little-endian manner
sz1 = max(size(varargin{1},1),1);
sz2 = max(size(varargin{2},1),1);
sz3 = 1;
if (nargin>2) % Currently allows only 3
    sz3 = max(size(varargin{3}, 1), 1);
end;
% J1 goes to the fastest index, just copy it
J1 = repmat(varargin{1}, sz2*sz3, 1);
% J2 goes to the middle
J2 = reshape(varargin{2}, 1, []);
J2 = repmat(J2, sz1, 1); % now sz1 ones will be the fastest
J2 = reshape(J2, sz1*sz2, []);
J2 = repmat(J2, sz3, 1);
J = [J1,J2];
if (nargin>2)
    % J3 goes to the slowest
    J3 = reshape(varargin{3}, 1, []);
    J3 = repmat(J3, sz1*sz2, 1); % now sz1 ones will be the fastest
    J3 = reshape(J3, sz1*sz2*sz3, []);
    J = [J,J3];
end;
end


function [i,j] = argmin2(x)
%ARGMIN2  Index of minimum element of matrix.
% [i,j] = ARGMIN2(x) returns indices (i,j) such that x(i,j) == min(x(:)).
%
% See also ARGMIN, ARGMAX2.

[colmin,i] = min(x);
[ignore,j] = min(colmin);
i = i(j);


%% [B1_] = tensor_fibers_column(A)
%
function [B1_] = tensor_fibers_column(A)
  [n1, n2, n3] = size(A);
  for j = 1:n2
    for k = 1:n3
      B1_{j,k} = A(:,j,k);
    end
  end
end


function [rep_motion motion_vector] = fastMV(im1, im2, w)
% Author:    Arash Jalalian
% E-mail:    arash.jalalian@gmail.com
% Arguments: [rep_motion motion_vector] = fastMV(im1, im2, w)
% This is the simplified block matching algorithm(BMA) which is proposed by
% L. Hao. I've simplified this algorithm for use it in a real-time problem.
% We have a dynamic search pattern during finding the motion vector.
% 1. check with big diamond.
% 2. check with one of the hexagon subject to previous results.
% 3. check with small diamond.
% 'im1' is base frame of a video and 'im2' is the second frame. and 'w' is
% the window size. 'rep_motion' is the representative motion vector and
% 'motion_vector' declare motion vectors for each block of image.
% Example:
% im1 = imread('frame001.jpg');
% im2 = imread('frame002.jpg');
% w = 16;
% [rep_m m_vector] = fastMV(im1, im2, w);

%   clear all
%   close all
%   clc
%   im1 = imread('img_1057.pgm');
%   im2 = imread('img_1058.pgm');
%  % dis = 2;
%   w = 8;

% initialization
[r1 c1] = size(im1);
[r2 c2] = size(im2);
if r1 ~= r2 || c1 ~= c2
    error('The images must be in a same size')
end

pat.org    = [0; 0];
pat.diam1  = [2 0 -2 0; 0 2 0 -2];%big diamond
pat.diam2  = [1 0 -1 0; 0 1 0 -1];%small diamond
pat.hexver = [2 1  -1 -2 -1 1; 0 2 2 0 -2 -2];
pat.hexhor = [2 0 -2 -2 0 2 ;1 2 1 -1 -2 -1];

r_time = int16(floor(r1 ./ w));
c_time = int16(floor(c1 ./ w));

% for preventing index exceeding from end of image. 4 is the maximum
% Magnitude of the motion vector
if mod(r1, w) < 4
    r_time = r_time -1;
end
if mod(c1, w) < 4
    c_time = c_time -1;
end
slice1 = cell(r_time, c_time);
slice2 = cell(r_time, c_time);
motion_vector = cell(r_time, c_time);

% creating slices with cell array of slices
for i = 1:r_time
    for j = 1:c_time
        slice1{i, j} = im1(((i-1) * w) + 1:i * w, ((j-1) * w) + 1:j * w);
        slice2{i, j} = im2(((i-1) * w) + 1:i * w, ((j-1) * w) + 1:j * w);
    end
end

% first step checking with 4 points of big diamond and the origin
mad_org = zeros(r_time, c_time);
min_mad = zeros(r_time, c_time);
idx_min_mad = zeros(r_time, c_time);
for i = 1:r_time
    for j = 1:c_time
        s_h_r = ((i-1) * w) + 1;%slice head row
        s_h_c = ((j-1) * w) + 1;%slice head column
        diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});
        [r_diam c_diam] = size(pat.diam1);
        for k = 1:c_diam
            if s_h_r + pat.diam1(1, k) > 0 && s_h_c + pat.diam1(2, k) > 0
                diff.diam1{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.diam1(1, k):...
                    i *w + pat.diam1(1, k),...
                    s_h_c + pat.diam1(2, k):...
                    j * w + pat.diam1(2, k)));
                % calculating MAD = Mean Absolute Difference
                diff.diam1{2, k} = sum(sum(diff.diam1{1,k})) / (w ^ 2);% insert each MAD below each Pat
                mad(k) = sum(sum(diff.diam1{1,k})) / (w ^ 2);
            else
                diff.diam1{1, k} = 9999999999999;
                diff.diam1{2, k} = 9999999999999;
                mad(k) = sum(sum(diff.diam1{1,k})) / (w ^ 2);
            end
        end
        % calculating minimum of mad in 4 pat and the origin and inserting
        % them into a min_mad(i, j). where i, j decler the position of the
        % slice in the image
        mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);
        mad = [mad mad_org(i, j)];
        [min_mad(i, j) idx_min_mad(i, j)] = min(mad);
        clear mad

    end
end
% if the idx_min_mad == 5 it means that the origin has the minimum value of
% mad (between 4 big diamond points and the origin the orogin has the
% minimum value) otherwise the index in idx_min_mad shows the pat index that
% has the min mad value.
idx_min_mad2 = zeros(r_time, c_time);
min_mad2 = zeros(r_time, c_time);
for i = 1:r_time
    for j = 1:c_time
        clear mad
        if idx_min_mad(i, j) == 5
            % it means this point didn't moved and it's better to do not use
            % any motion vectors so we must put [0; 0] for these points as a
            % motion vector.accourding to the paper for any slices that
            % idx_min_mad(i, j) ==5 we must use anothe four points as new
            % pat and check the slices with these i and j for mad. and the
            % final solution for these slices is these mad. here we must
            % use pat.diam2 as new patterns and repeat thise process again.
            s_h_r = ((i-1) * w) + 1;%slice head row
            s_h_c = ((j-1) * w) + 1;%slice head column
%           diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});
            [r_diam c_diam] = size(pat.diam1);
            for k = 1:c_diam
                if s_h_r + pat.diam2(1, k) > 0 && s_h_c + pat.diam2(2, k) > 0
                    diff.diam2{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.diam2(1, k):...
                        i *w + pat.diam2(1, k),...
                        s_h_c + pat.diam2(2, k):...
                        j * w + pat.diam2(2, k)));
                    % calculating MAD = Mean Absolute Diffrence
                    diff.diam2{2, k} = sum(sum(diff.diam2{1,k})) / (w ^ 2);% insert each MAD below each Pat
                    mad(k) = sum(sum(diff.diam2{1,k})) / (w ^ 2);
                else
                    diff.diam2{1, k} = 9999999999999;
                    diff.diam2{2, k} = 9999999999999;
                    mad(k) = sum(sum(diff.diam2{1,k})) / (w ^ 2);
                end
            end
            % calculating minimum of mad in 4 pat and the origin and inserting
            % them into a min_mad(i, j). where i, j decler the position of the
            % slice in the image
%            mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);
%            mad = [mad mad_org(i, j)];
            [min_mad2(i, j) idx_min_mad2(i, j)] = min(mad);

        end
    end
end
for i = 1:r_time
    for j = 1:c_time
        if idx_min_mad2(i, j) ~= 0
            motion_vector{i , j} = pat.diam2(:, idx_min_mad2(i, j));
        end
    end
end

% other points that their minimum mads idx ~= 5. it means that these slices
% are closer to farder slices.
idx_min_mad3 = zeros(r_time, c_time);
min_mad3 = zeros(r_time, c_time);
for i = 1:r_time
    for j = 1:c_time
        clear mad
        if idx_min_mad(i, j) ~= 5
            s_h_r = ((i-1) * w) + 1;%slice head row
            s_h_c = ((j-1) * w) + 1;%slice head column
%           diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});
            [r_hex c_hex] = size(pat.hexhor);
            for k = 1:c_hex
                if s_h_r + pat.hexhor(1, k) > 0 && s_h_c + pat.hexhor(2, k) > 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)
                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexhor(1, k):...
                        i *w + pat.hexhor(1, k),...
                        s_h_c + pat.hexhor(2, k):...
                        j * w + pat.hexhor(2, k)));
                    % calculating MAD = Mean Absolute Diffrence
                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                elseif s_h_r + pat.hexver(1, k) > 0 && s_h_c + pat.hexver(2, k) > 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)
                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexver(1, k):...
                        i *w + pat.hexver(1, k),...
                        s_h_c + pat.hexver(2, k):...
                        j * w + pat.hexver(2, k)));
                    % calculating MAD = Mean Absolute Diffrence
                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                else
                    diff.hex{1, k} = 9999999999999;
                    diff.hex{2, k} = 9999999999999;
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                end
            end
            % calculating minimum of mad in 4 pat and inserting
            % them into a min_mad(i, j). where i, j decler the position of the
            % slice in the image
%            mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);
%            mad = [mad mad_org(i, j)];
            [min_mad3(i, j) idx_min_mad3(i, j)] = min(mad);

        end
    end
end
% org_mov declare the movement vector for new pattern origins.
org_mov = cell(r_time, c_time);
for i = 1:r_time
    for j = 1:c_time
        if idx_min_mad3(i, j) ~= 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)
            org_mov{i , j} = pat.hexhor(:, idx_min_mad3(i, j));
        elseif idx_min_mad3(i, j) ~= 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)
            org_mov{i , j} = pat.hexver(:, idx_min_mad3(i, j));
        end
    end
end
% now we must go the theird step. the minimum mad point found in the
% previous search step is repositioned as the center point to form a new
% hexagon nad only three new non overlaped points will be checked as
% candidates each time. so we must define new pattern. this time our
% patterns must have a dynamic behavior. and also these pattern must be
% created based on the old pat.hexver and pat.hexhor
[r_mov c_mov] = size(org_mov);
idx_min_mad4 = zeros(r_mov, c_mov);
min_mad4 = zeros(r_mov, c_mov);
for i = 1:r_mov
    for j = 1:c_mov
        clear mad
        nu = sparse(org_mov{i, j});
        [r_nu c_nu] = size(nu);
        if r_nu ~= 0 || c_nu ~= 0
            for z = 1:c_hex
                pat.hexver_new(:, z) = pat.hexver(:, z) + org_mov{i, j};
                pat.hexhor_new(:, z) = pat.hexhor(:, z) + org_mov{i, j};
            end
            s_h_r = ((i-1) * w) + 1;%slice head row
            s_h_c = ((j-1) * w) + 1;%slice head column
%           diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});
            [r_hex c_hex] = size(pat.hexhor_new);
            for k = 1:c_hex
%                clear mad
                if s_h_r + pat.hexhor_new(1, k) > 0 && s_h_c + pat.hexhor_new(2, k) > 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)
                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexhor_new(1, k):...
                        i *w + pat.hexhor_new(1, k),...
                        s_h_c + pat.hexhor_new(2, k):...
                        j * w + pat.hexhor_new(2, k)));
                    % calculating MAD = Mean Absolute Diffrence
                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                elseif s_h_r + pat.hexver_new(1, k) > 0 && s_h_c + pat.hexver_new(2, k) > 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)
                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexver_new(1, k):...
                        i *w + pat.hexver_new(1, k),...
                        s_h_c + pat.hexver_new(2, k):...
                        j * w + pat.hexver_new(2, k)));
                    % calculating MAD = Mean Absolute Diffrence
                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                else
                    diff.hex{1, k} = 9999999999999;
                    diff.hex{2, k} = 9999999999999;
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                end
            end
            % calculating minimum of mad in 4 pat and inserting
            % them into a min_mad(i, j). where i, j declare the position of the
            % slice in the image
%            mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);
            mad = [mad mad_org(i, j)];
            [min_mad4(i, j) idx_min_mad4(i, j)] = min(mad);
%              clear mad
        end
    end
end
% org_mov declare the movement vector for new pattern origins.
% final motion vector calculation
for i = 1:r_mov
    for j = 1:c_mov
        if idx_min_mad4(i, j) == 7
            motion_vector{i, j} = pat.org;
        elseif idx_min_mad4(i, j) ~= 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)
            motion_vector{i , j} = pat.hexhor_new(:, idx_min_mad4(i, j));
        elseif idx_min_mad4(i, j) ~= 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)
            motion_vector{i , j} = pat.hexver_new(:, idx_min_mad3(i, j));
        end
    end
end

% now we want to find rep_motion vector
motion_vectors = cat(2, motion_vector{:});
most_motion_vector = zeros(1, r_time .* c_time);
clear temp
temp = motion_vectors;
for i=1:r_time .* c_time
    my_count = 0;
    element(:, 1) = temp(:, i);
    for k = i+1:r_time .* c_time
        if temp(:, k) == element(:, 1)
            my_count = my_count+1;
%             temp(:, k) = [];
        end
    end
    most_motion_vector(i) = my_count;
end
[value, idx] = max(most_motion_vector);
rep_motion= motion_vectors(:, idx);


function expanded_array = expand_complex(complex_array)

% Note that if "complex_array" is a column vector, then its real and imaginary
% parts are the row vectors of "expanded_array", because the fastest-varying
% index (i.e., index in the column direction) is assigned for alternating the
% real and imaginary parts.
chkarg(istypeof(complex_array, 'complex'), '"complex_array" should be array with complex elements.');
nD = ndims(complex_array);
expanded_array = cat(nD+1, real(complex_array), imag(complex_array));
expanded_array = permute(expanded_array, [nD+1, 1:nD]);