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


%+========================================================================+
%|                                                                        |
%|            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 [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 [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 y = vl_nnloss(x,c,varargin)
%VL_NNLOSS CNN categorical or attribute loss.
%   Y = VL_NNLOSS(X, C) computes the loss incurred by the prediction
%   scores X given the categorical labels C.
%
%   The prediction scores X are organised as a field of prediction
%   vectors, represented by a H x W x D x N array. The first two
%   dimensions, H and W, are spatial and correspond to the height and
%   width of the field; the third dimension D is the number of
%   categories or classes; finally, the dimension N is the number of
%   data items (images) packed in the array.
%
%   While often one has H = W = 1, the case W, H > 1 is useful in
%   dense labelling problems such as image segmentation. In the latter
%   case, the loss is summed across pixels (contributions can be
%   weighed using the `InstanceWeights` option described below).
%
%   The array C contains the categorical labels. In the simplest case,
%   C is an array of integers in the range [1, D] with N elements
%   specifying one label for each of the N images. If H, W > 1, the
%   same label is implicitly applied to all spatial locations.
%
%   In the second form, C has dimension H x W x 1 x N and specifies a
%   categorical label for each spatial location.
%
%   In the third form, C has dimension H x W x D x N and specifies
%   attributes rather than categories. Here elements in C are either
%   +1 or -1 and C, where +1 denotes that an attribute is present and
%   -1 that it is not. The key difference is that multiple attributes
%   can be active at the same time, while categories are mutually
%   exclusive. By default, the loss is *summed* across attributes
%   (unless otherwise specified using the `InstanceWeights` option
%   described below).
%
%   DZDX = VL_NNLOSS(X, C, DZDY) computes the derivative of the block
%   projected onto the output derivative DZDY. DZDX and DZDY have the
%   same dimensions as X and Y respectively.
%
%   VL_NNLOSS() supports several loss functions, which can be selected
%   by using the option `type` described below. When each scalar c in
%   C is interpreted as a categorical label (first two forms above),
%   the following losses can be used:
%
%   Classification error:: `classerror`
%     L(X,c) = (argmax_q X(q) ~= c). Note that the classification
%     error derivative is flat; therefore this loss is useful for
%     assessment, but not for training a model.
%
%   Top-K classification error:: `topkerror`
%     L(X,c) = (rank X(c) in X <= K). The top rank is the one with
%     highest score. For K=1, this is the same as the
%     classification error. K is controlled by the `topK` option.
%
%   Log loss:: `log`
%     L(X,c) = - log(X(c)). This function assumes that X(c) is the
%     predicted probability of class c (hence the vector X must be non
%     negative and sum to one).
%
%   Softmax log loss (multinomial logistic loss):: `softmaxlog`
%     L(X,c) = - log(P(c)) where P(c) = exp(X(c)) / sum_q exp(X(q)).
%     This is the same as the `log` loss, but renormalizes the
%     predictions using the softmax function.
%
%   Multiclass hinge loss:: `mhinge`
%     L(X,c) = max{0, 1 - X(c)}. This function assumes that X(c) is
%     the score margin for class c against the other classes.  See
%     also the `mmhinge` loss below.
%
%   Multiclass structured hinge loss:: `mshinge`
%     L(X,c) = max{0, 1 - M(c)} where M(c) = X(c) - max_{q ~= c}
%     X(q). This is the same as the `mhinge` loss, but computes the
%     margin between the prediction scores first. This is also known
%     the Crammer-Singer loss, an example of a structured prediction
%     loss.
%
%   When C is a vector of binary attribures c in (+1,-1), each scalar
%   prediction score x is interpreted as voting for the presence or
%   absence of a particular attribute. The following losses can be
%   used:
%
%   Binary classification error:: `binaryerror`
%     L(x,c) = (sign(x - t) ~= c). t is a threshold that can be
%     specified using the `threshold` option and defaults to zero. If
%     x is a probability, it should be set to 0.5.
%
%   Binary log loss:: `binarylog`
%     L(x,c) = - log(c(x-0.5) + 0.5). x is assumed to be the
%     probability that the attribute is active (c=+1). Hence x must be
%     a number in the range [0,1]. This is the binary version of the
%     `log` loss.
%
%   Logistic log loss:: `logistic`
%     L(x,c) = log(1 + exp(- cx)). This is the same as the `binarylog`
%     loss, but implicitly normalizes the score x into a probability
%     using the logistic (sigmoid) function: p = sigmoid(x) = 1 / (1 +
%     exp(-x)). This is also equivalent to `softmaxlog` loss where
%     class c=+1 is assigned score x and class c=-1 is assigned score
%     0.
%
%   Hinge loss:: `hinge`
%     L(x,c) = max{0, 1 - cx}. This is the standard hinge loss for
%     binary classification. This is equivalent to the `mshinge` loss
%     if class c=+1 is assigned score x and class c=-1 is assigned
%     score 0.
%
%   VL_NNLOSS(...,'OPT', VALUE, ...) supports these additionals
%   options:
%
%   InstanceWeights:: []
%     Allows to weight the loss as L'(x,c) = WGT L(x,c), where WGT is
%     a per-instance weight extracted from the array
%     `InstanceWeights`. For categorical losses, this is either a H x
%     W x 1 or a H x W x 1 x N array. For attribute losses, this is
%     either a H x W x D or a H x W x D x N array.
%
%   TopK:: 5
%     Top-K value for the top-K error. Note that K should not
%     exceed the number of labels.
%
%   See also: VL_NNSOFTMAX().

% Copyright (C) 2014-15 Andrea Vedaldi.
% Copyright (C) 2016 Karel Lenc.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if ~isempty(varargin) && ~ischar(varargin{1})  % passed in dzdy
  dzdy = varargin{1} ;
  varargin(1) = [] ;
else
  dzdy = [] ;
end

opts.instanceWeights = [] ;
opts.classWeights = [] ;
opts.threshold = 0 ;
opts.loss = 'softmaxlog' ;
opts.topK = 5 ;
opts = vl_argparse(opts, varargin, 'nonrecursive') ;

inputSize = [size(x,1) size(x,2) size(x,3) size(x,4)] ;

% Form 1: C has one label per image. In this case, get C in form 2 or
% form 3.
c = gather(c) ;
if numel(c) == inputSize(4)
  c = reshape(c, [1 1 1 inputSize(4)]) ;
  c = repmat(c, inputSize(1:2)) ;
end

hasIgnoreLabel = any(c(:) == 0);

% --------------------------------------------------------------------
% Spatial weighting
% --------------------------------------------------------------------

% work around a bug in MATLAB, where native cast() would slow
% progressively
if isa(x, 'gpuArray')
  switch classUnderlying(x) ;
    case 'single', cast = @(z) single(z) ;
    case 'double', cast = @(z) double(z) ;
  end
else
  switch class(x)
    case 'single', cast = @(z) single(z) ;
    case 'double', cast = @(z) double(z) ;
  end
end

labelSize = [size(c,1) size(c,2) size(c,3) size(c,4)] ;
assert(isequal(labelSize(1:2), inputSize(1:2))) ;
assert(labelSize(4) == inputSize(4)) ;
instanceWeights = [] ;
switch lower(opts.loss)
  case {'classerror', 'topkerror', 'log', 'softmaxlog', 'mhinge', 'mshinge'}
    % there must be one categorical label per prediction vector
    assert(labelSize(3) == 1) ;

    if hasIgnoreLabel
      % null labels denote instances that should be skipped
      instanceWeights = cast(c(:,:,1,:) ~= 0) ;
    end

  case {'binaryerror', 'binarylog', 'logistic', 'hinge'}

    % there must be one categorical label per prediction scalar
    assert(labelSize(3) == inputSize(3)) ;

    if hasIgnoreLabel
      % null labels denote instances that should be skipped
      instanceWeights = cast(c ~= 0) ;
    end

  otherwise
    error('Unknown loss ''%s''.', opts.loss) ;
end

if ~isempty(opts.instanceWeights)
  % important: this code needs to broadcast opts.instanceWeights to
  % an array of the same size as c
  if isempty(instanceWeights)
    instanceWeights = bsxfun(@times, onesLike(c), opts.instanceWeights) ;
  else
    instanceWeights = bsxfun(@times, instanceWeights, opts.instanceWeights);
  end
end

% --------------------------------------------------------------------
% Do the work
% --------------------------------------------------------------------

switch lower(opts.loss)
  case {'log', 'softmaxlog', 'mhinge', 'mshinge'}
    % from category labels to indexes
    numPixelsPerImage = prod(inputSize(1:2)) ;
    numPixels = numPixelsPerImage * inputSize(4) ;
    imageVolume = numPixelsPerImage * inputSize(3) ;

    n = reshape(0:numPixels-1,labelSize) ;
    offset = 1 + mod(n, numPixelsPerImage) + ...
             imageVolume * fix(n / numPixelsPerImage) ;
    ci = offset + numPixelsPerImage * max(c - 1,0) ;
end

if nargin <= 2 || isempty(dzdy)
  switch lower(opts.loss)
    case 'classerror'
      [~,chat] = max(x,[],3) ;
      t = cast(c ~= chat) ;
    case 'topkerror'
      [~,predictions] = sort(x,3,'descend') ;
      t = 1 - sum(bsxfun(@eq, c, predictions(:,:,1:opts.topK,:)), 3) ;
    case 'log'
      t = - log(x(ci)) ;
    case 'softmaxlog'
      Xmax = max(x,[],3) ;
      ex = exp(bsxfun(@minus, x, Xmax)) ;
      t = Xmax + log(sum(ex,3)) - x(ci) ;
    case 'mhinge'
      t = max(0, 1 - x(ci)) ;
    case 'mshinge'
      Q = x ;
      Q(ci) = -inf ;
      t = max(0, 1 - x(ci) + max(Q,[],3)) ;
    case 'binaryerror'
      t = cast(sign(x - opts.threshold) ~= c) ;
    case 'binarylog'
      t = -log(c.*(x-0.5) + 0.5) ;
    case 'logistic'
      %t = log(1 + exp(-c.*X)) ;
      a = -c.*x ;
      b = max(0, a) ;
      t = b + log(exp(-b) + exp(a-b)) ;
    case 'hinge'
      t = max(0, 1 - c.*x) ;
  end
  if ~isempty(instanceWeights)
    y = instanceWeights(:)' * t(:) ;
  else
    y = sum(t(:));
  end
else
  if ~isempty(instanceWeights)
    dzdy = dzdy * instanceWeights ;
  end
  switch lower(opts.loss)
    case {'classerror', 'topkerror'}
      y = zerosLike(x) ;
    case 'log'
      y = zerosLike(x) ;
      y(ci) = - dzdy ./ max(x(ci), 1e-8) ;
    case 'softmaxlog'
      Xmax = max(x,[],3) ;
      ex = exp(bsxfun(@minus, x, Xmax)) ;
      y = bsxfun(@rdivide, ex, sum(ex,3)) ;
      y(ci) = y(ci) - 1 ;
      y = bsxfun(@times, dzdy, y) ;
    case 'mhinge'
      y = zerosLike(x) ;
      y(ci) = - dzdy .* (x(ci) < 1) ;
    case 'mshinge'
      Q = x ;
      Q(ci) = -inf ;
      [~, q] = max(Q,[],3) ;
      qi = offset + numPixelsPerImage * (q - 1) ;
      W = dzdy .* (x(ci) - x(qi) < 1) ;
      y = zerosLike(x) ;
      y(ci) = - W ;
      y(qi) = + W ;
    case 'binaryerror'
      y = zerosLike(x) ;
    case 'binarylog'
      y = - dzdy ./ (x + (c-1)*0.5) ;
    case 'logistic'
      % t = exp(-Y.*X) / (1 + exp(-Y.*X)) .* (-Y)
      % t = 1 / (1 + exp(Y.*X)) .* (-Y)
      y = - dzdy .* c ./ (1 + exp(c.*x)) ;
    case 'hinge'
      y = - dzdy .* c .* (c.*x < 1) ;
  end
end

% --------------------------------------------------------------------
function y = zerosLike(x)
% --------------------------------------------------------------------
if isa(x,'gpuArray')
  y = gpuArray.zeros(size(x),classUnderlying(x)) ;
else
  y = zeros(size(x),'like',x) ;
end

% --------------------------------------------------------------------
function y = onesLike(x)
% --------------------------------------------------------------------
if isa(x,'gpuArray')
  y = gpuArray.ones(size(x),classUnderlying(x)) ;
else
  y = ones(size(x),'like',x) ;
end


function cae = caenumgradcheck(cae, x, y)
    epsilon = 1e-4;
    er = 1e-6;
    disp('performing numerical gradient checking...')
    for i = 1 : numel(cae.o)
        p_cae = cae; p_cae.c{i} = p_cae.c{i} + epsilon;
        m_cae = cae; m_cae.c{i} = m_cae.c{i} - epsilon;

        [m_cae, p_cae] = caerun(m_cae, p_cae, x, y);
        d = (p_cae.L - m_cae.L) / (2 * epsilon);

        e = abs(d - cae.dc{i});
        if e > er
            disp('OUTPUT BIAS numerical gradient checking failed');
            disp(e);
            disp(d / cae.dc{i});
            keyboard
        end
    end

    for a = 1 : numel(cae.a)

        p_cae = cae; p_cae.b{a} = p_cae.b{a} + epsilon;
        m_cae = cae; m_cae.b{a} = m_cae.b{a} - epsilon;

        [m_cae, p_cae] = caerun(m_cae, p_cae, x, y);
        d = (p_cae.L - m_cae.L) / (2 * epsilon);
%        cae.dok{i}{a}(u) = d;
        e = abs(d - cae.db{a});
        if e > er
            disp('BIAS numerical gradient checking failed');
            disp(e);
            disp(d / cae.db{a});
            keyboard
        end

        for i = 1 : numel(cae.o)
            for u = 1 : numel(cae.ok{i}{a})
                p_cae = cae; p_cae.ok{i}{a}(u) = p_cae.ok{i}{a}(u) + epsilon;
                m_cae = cae; m_cae.ok{i}{a}(u) = m_cae.ok{i}{a}(u) - epsilon;

                [m_cae, p_cae] = caerun(m_cae, p_cae, x, y);
                d = (p_cae.L - m_cae.L) / (2 * epsilon);
%                cae.dok{i}{a}(u) = d;
                e = abs(d - cae.dok{i}{a}(u));
                if e > er
                    disp('OUTPUT KERNEL numerical gradient checking failed');
                    disp(e);
                    disp(d / cae.dok{i}{a}(u));
%                    keyboard
                end
            end
        end

        for i = 1 : numel(cae.i)
            for u = 1 : numel(cae.ik{i}{a})
                p_cae = cae;
                m_cae = cae;
                p_cae.ik{i}{a}(u) = p_cae.ik{i}{a}(u) + epsilon;
                m_cae.ik{i}{a}(u) = m_cae.ik{i}{a}(u) - epsilon;
                [m_cae, p_cae] = caerun(m_cae, p_cae, x, y);
                d = (p_cae.L - m_cae.L) / (2 * epsilon);
%                cae.dik{i}{a}(u) = d;
                e = abs(d - cae.dik{i}{a}(u));
                if e > er
                    disp('INPUT KERNEL numerical gradient checking failed');
                    disp(e);
                    disp(d / cae.dik{i}{a}(u));
                end
            end
        end
    end

    disp('done')

end

function [m_cae, p_cae] = caerun(m_cae, p_cae, x, y)
    m_cae = caeup(m_cae, x); m_cae = caedown(m_cae); m_cae = caebp(m_cae, y);
    p_cae = caeup(p_cae, x); p_cae = caedown(p_cae); p_cae = caebp(p_cae, y);
end

%function checknumgrad(cae,what,x,y)
%    epsilon = 1e-4;
%    er = 1e-9;
%
%    for i = 1 : numel(eval(what))
%        if iscell(eval(['cae.' what]))
%            checknumgrad(cae,[what '{' num2str(i) '}'], x, y)
%        else
%            p_cae = cae;
%            m_cae = cae;
%            eval(['p_cae.' what '(' num2str(i) ')']) = eval([what '(' num2str(i) ')']) + epsilon;
%            eval(['m_cae.' what '(' num2str(i) ')']) = eval([what '(' num2str(i) ')']) - epsilon;
%
%            m_cae = caeff(m_cae, x); m_cae = caedown(m_cae); m_cae = caebp(m_cae, y);
%            p_cae = caeff(p_cae, x); p_cae = caedown(p_cae); p_cae = caebp(p_cae, y);
%
%            d = (p_cae.L - m_cae.L) / (2 * epsilon);
%            e = abs(d - eval(['cae.d' what '(' num2str(i) ')']));
%            if e > er
%                error('numerical gradient checking failed');
%            end
%         end
%     end
%
% end


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [yc,dy,para] = getTrafoFromInstationaryVelocityRK4(vc,yc,varargin)
%
% compute transformation yc by integrating velocity field in time using a
% Runge-Kutta 4 method with fixed time steps. Velocity is time dependent
% here and assumed to be nodal in time. For stationary velocities, use
% getTrafoFromVelocityRK4.m
%
% For more details see Sec. 3 of the paper:
%
% @article{MangRuthotto2017,
%   Title = {A {L}agrangian {G}auss--{N}ewton--{K}rylov solver for mass- and intensity-preserving diffeomorphic image registration},
%   Year = {2017},
%   Journal = {SIAM Journal on Scientific Computing},
%   Author = {A. Mang, L. Ruthotto},
% }
%
% Input:
%  vc   - discrete velocity field (nodal, cell-centered, or staggered in space / nodal in time)
%  yc   - particle positions
%
% Additional REQUIRED Input (provided through varargin)
%
%  omega        - spatial domain (required!)
%  m            - number of cells in each direction (required!)
%
% Optional Input (provided through varargin)
%
%  doDerivative - compute derivative w.r.t. velocity
%  tspan        - time interval (default: [0 1])
%  N            - number of time steps (default: 5)
%  nt           - number of time points for velocity (default: compute from input)
%  storeInter   - store intermediate transformation (e.g., for visualization)
%
% Output:
%
%  yc           - end point of characteristics
%  dy           - derivative w.r.t. vc
%  para         - struct containing info
%
% =========================================================================
function [yc,dy,para] = getTrafoFromInstationaryVelocityRK4(vc,yc,varargin)

if nargin==0,
    runMinimalExample
    return;
end
doDerivative = (nargout>1);
omega = [];
m     = [];
tspan = [0,1];
N     = 5;
nt    = [];
storeInter = false;
for k=1:2:length(varargin)     % overwrites default parameter
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;
if isempty(omega) || isempty(m)
    error('%s - omega and m must be provided through varargin or trafo(''set'',''omega'',omega,''m'',m')
end
dim = numel(omega)/2;
if isempty(nt) % roughly estimate nt
    nt = round(numel(vc)/(prod(m)*dim))-1;
end
ht    = (tspan(2)-tspan(1))/nt;
tspan = (linspace(tspan(1),tspan(2),N)-min(tspan))/abs(ht)+1; % map to nodal indices
dt    = diff(tspan);
dt    = dt(1);
dy    = [];
if doDerivative
    dy = cell(nt+1,1);
    for j=1:nt+1, dy{j} = sparse(numel(yc),prod(m)*dim); end;
    dyi = dy;
end

para = struct('dt',dt,'N',N,'nt',nt,'omega',omega,'m',m);
if storeInter
    para.YC = zeros(numel(yc),N);
    para.YC(:,1) = yc;
end

vc = reshape(vc,[],nt+1);
for k=1:N-1
    % get v1 = v(yc,t(k));
    tk = tspan(k);
    p  = floor(tk); x = tk-p;  % split x into integer/remainder
    dvi      = zeros(nt+1,1);
    dvi(p)   = 1-x;
    if p<nt+1, dvi(p+1) = x; end
    vt = vc*dvi;
    [vi,dydvi] = linearInterGrid(vt,omega,m,yc,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yc);
        for j=1:nt+1
           dyi{j} = dvi(j)*Ty + dydvi*dy{j};
        end
        dytemp  = dyi;
    end
    yi = yc + .5*dt*vi;
	ytemp = vi;

    % get v2 = v(y1,.5*(t(k+1)+t(k)));
    tk1 = (tspan(k+1)+tspan(k))/2;
    p  = floor(tk1); x = tk1-p;  % split x into integer/remainder
    dvi      = zeros(nt+1,1);
    dvi(p)   = 1-x;
    if p<nt+1, dvi(p+1) = x; end
    vt = vc*dvi;
    [vi,dydvi] = linearInterGrid(vt,omega,m,yi,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yi);
        for j=1:nt+1
            dyi{j}    = dvi(j)*Ty + dydvi*(dy{j}+.5*dt*dyi{j});
            dytemp{j} = dytemp{j} + 2*dyi{j};
        end
    end
    yi = yc + .5*dt*vi;
	ytemp = ytemp + 2*vi;

    % get v3 = v(y2,.5*(t(k+1)+t(k)));
    [vi,dydvi] = linearInterGrid(vt,omega,m,yi,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yi);
        for j=1:nt+1
            dyi{j}    = dvi(j)*Ty + dydvi*(dy{j}+.5*dt*dyi{j});
            dytemp{j} = dytemp{j} + 2*dyi{j};
        end
    end
    yi  = yc + dt*vi;
	ytemp = ytemp + 2*vi;

    % get v4 = v(y3,t(k+1)));
    tk1 = tspan(k+1);
    p  = floor(tk1); x = tk1-p;  % split x into integer/remainder
    dvi      = zeros(nt+1,1);
    dvi(p)   = 1-x;
    if p<nt+1, dvi(p+1) = x; end
    vt = vc*dvi;
    [vi,dydvi] = linearInterGrid(vt,omega,m,yi,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yi);
        for j=1:nt+1
            dyi{j} = dvi(j)*Ty + dydvi*(dy{j}+dt*dyi{j});
            dytemp{j} = dytemp{j} + dyi{j};
            dy{j}  = dy{j} + (dt/6)*dytemp{j};
        end
    end
    ytemp = ytemp + vi;
    yc    = yc + (dt/6)*ytemp;
    if storeInter, para.YC(:,k+1) = yc; end
end
if doDerivative, dy = horzcat(dy{:}); end;

function runMinimalExample

omegaV = [-1 1 -1 1];
omega = .1*omegaV;
m     = [32 32];
tspan     = [8 0];
N     = 20;
yc    = getNodalGrid(omega,m)+.0002;

%
% % test cell-centered grid
regularizer('set','regularizer','mbCurvature','alpha',1)
xc      = reshape(getCellCenteredGrid(omegaV,m),[],2);
v0      = [.3*sign(xc(:,1)).*xc(:,1).^2; sin(pi*xc(:,2))];
vc      = v0*[1,1.1,1.5,2];
fctn    = @(vc) getTrafoFromInstationaryVelocityRK4(vc(:),yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
checkDerivative(fctn,vc(:))


% PAC - compute phase-amplitude coupling (power of first input
%         correlation with phase of second). There is no graphical output
%         to this function.
%
% Usage:
%   >> pac(x,y,srate);
%   >> [coh,timesout,freqsout1,freqsout2,cohboot] ...
%                     = pac(x,y,srate,'key1', 'val1', 'key2', val2' ...);
% Inputs:
%    x       = [float array] 2-D data array of size (times,trials) or
%              3-D (1,times,trials)
%    y       = [float array] 2-D or 3-d data array
%    srate   = data sampling rate (Hz)
%
%    Most important optional inputs
%       'method'    = ['mod'|'corrsin'|'corrcos'|'latphase'] modulation
%                     method or correlation of amplitude with sine or cosine of
%                     angle (see ref). 'laphase' compute the phase
%                     histogram at a specific time and requires the
%                     'powerlat' option to be set.
%       'freqs'     = [min max] frequency limits. Default [minfreq 50],
%                     minfreq being determined by the number of data points,
%                     cycles and sampling frequency. Use 0 for minimum frequency
%                     to compute default minfreq. You may also enter an
%                     array of frequencies for the spectral decomposition
%                     (for FFT, closest computed frequency will be returned; use
%                     'padratio' to change FFT freq. resolution).
%       'freqs2'    = [float array] array of frequencies for the second
%                     argument. 'freqs' is used for the first argument.
%                     By default it is the same as 'freqs'.
%       'wavelet'   = 0  -> Use FFTs (with constant window length) { Default }
%                   = >0 -> Number of cycles in each analysis wavelet
%                   = [cycles expfactor] -> if 0 < expfactor < 1,  the number
%                     of wavelet cycles expands with frequency from cycles
%                     If expfactor = 1, no expansion; if = 0, constant
%                     window length (as in FFT)            {default wavelet: 0}
%                   = [cycles array] -> cycle for each frequency. Size of array
%                      must be the same as the number of frequencies
%                     {default cycles: 0}
%       'wavelet2'  = same as 'wavelet' for the second argument. Default is
%                     same as cycles. Note that if the lowest frequency for X
%                     and Y are different and cycle is [cycles expfactor], it
%                     may result in discrepancies in the number of cycles at
%                     the same frequencies for X and Y.
%       'ntimesout' = Number of output times (int<frames-winframes). Enter a
%                     negative value [-S] to subsample original time by S.
%       'timesout'  = Enter an array to obtain spectral decomposition at
%                     specific time values (note: algorithm find closest time
%                     point in data and this might result in an unevenly spaced
%                     time array). Overwrite 'ntimesout'. {def: automatic}
%       'powerlat'  = [float] latency in ms at which to compute phase
%                     histogram
%       'tlimits'   = [min max] time limits in ms.
%
%    Optional Detrending:
%       'detrend'   = ['on'|'off'], Linearly detrend each data epoch   {'off'}
%       'rmerp'     = ['on'|'off'], Remove epoch mean from data epochs {'off'}
%
%    Optional FFT/DFT Parameters:
%       'winsize'   = If cycles==0: data subwindow length (fastest, 2^n<frames);
%                     If cycles >0: *longest* window length to use. This
%                     determines the lowest output frequency. Note that this
%                     parameter is overwritten if the minimum frequency has been set
%                     manually and requires a longer time window {~frames/8}
%       'padratio'  = FFT-length/winframes (2^k)                    {2}
%                     Multiplies the number of output frequencies by dividing
%                     their spacing (standard FFT padding). When cycles~=0,
%                     frequency spacing is divided by padratio.
%       'nfreqs'    = number of output frequencies. For FFT, closest computed
%                     frequency will be returned. Overwrite 'padratio' effects
%                     for wavelets. Default: use 'padratio'.
%       'freqscale' = ['log'|'linear'] frequency scale. Default is 'linear'.
%                     Note that for obtaining 'log' spaced freqs using FFT,
%                     closest correspondent frequencies in the 'linear' space
%                     are returned.
%       'subitc'    = ['on'|'off'] subtract stimulus locked Inter-Trial Coherence
%                    (ITC) from x and y. This computes the  'intrinsic' coherence
%                     x and y not arising from common synchronization to
%                     experimental events. See notes. {default: 'off'}
%       'itctype'   = ['coher'|'phasecoher'] For use with 'subitc', see TIMEF
%                     for more details {default: 'phasecoher'}.
%       'subwin'    = [min max] sub time window in ms (this windowing is
%                     performed after the spectral decomposition).
%
% Outputs:
%        pac         = Matrix (nfreqs1,nfreqs2,timesout) of coherence (complex).
%                      Use 20*log(abs(crossfcoh)) to visualize log spectral diffs.
%        timesout    = Vector of output times (window centers) (ms).
%        freqsout1   = Vector of frequency bin centers for first argument (Hz).
%        freqsout2   = Vector of frequency bin centers for second argument (Hz).
%        alltfX      = single trial spectral decomposition of X
%        alltfY      = single trial spectral decomposition of Y
%
% Author: Arnaud Delorme, SCCN/INC, UCSD 2005-
%
% Ref: Testing for Nested Oscilations (2008) J Neuro Methods 174(1):50-61
%
% See also: TIMEFREQ, CROSSF

% Copyright (C) 2002 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [crossfcoh, timesout1, freqs1, freqs2, crossfcohall, alltfX, alltfY] = pac(X, Y, srate, varargin);

if nargin < 1
    help pac;
    return;
end

% deal with 3-D inputs
% --------------------
if ndims(X) == 3, X = reshape(X, size(X,2), size(X,3)); end
if ndims(Y) == 3, Y = reshape(Y, size(Y,2), size(Y,3)); end
frame = size(X,2);

g = finputcheck(varargin, ...
                { 'alpha'         'real'     [0 0.2]                  [];
                  'baseboot'      'float'    []                       0;
                  'boottype'      'string'   {'times','trials','timestrials'}  'timestrials';
                  'detrend'       'string'   {'on','off'}              'off';
                  'freqs'         'real'     [0 Inf]                  [0 srate/2];
                  'freqs2'        'real'     [0 Inf]                  [];
                  'freqscale'     'string'   { 'linear','log' }       'linear';
                  'itctype'       'string'   {'phasecoher','phasecoher2','coher'}  'phasecoher';
                  'nfreqs'        'integer'  [0 Inf]                  [];
                  'lowmem'        'string'   {'on','off'}              'off';
                  'method'        'string'   { 'mod','corrsin','corrcos','latphase' }         'mod';
                  'naccu'         'integer'  [1 Inf]                   250;
                  'newfig'        'string'   {'on','off'}              'on';
                  'padratio'      'integer'  [1 Inf]                   2;
                  'rmerp'         'string'   {'on','off'}              'off';
                  'rboot'         'real'     []                        [];
                  'subitc'        'string'   {'on','off'}              'off';
                  'subwin'        'real'     []                        []; ...
                  'gammapowerlim' 'real'     []                        []; ...
                  'powerlim'      'real'     []                        []; ...
                  'powerlat'      'real'     []                        []; ...
                  'gammabase'     'real'     []                        []; ...
                  'timesout'      'real'     []                        []; ...
                  'ntimesout'     'integer'  []                        200; ...
                  'tlimits'       'real'     []                        [0 frame/srate];
                  'title'         'string'   []                        '';
                  'vert'          { 'real','cell' }  []                [];
                  'wavelet'       'real'     [0 Inf]                   0;
                  'wavelet2'      'real'     [0 Inf]                   [];
                  'winsize'       'integer'  [0 Inf]                   max(pow2(nextpow2(frame)-3),4) }, 'pac');

if ischar(g), error(g); end

% more defaults
% -------------
if isempty(g.wavelet2), g.wavelet2 = g.wavelet; end
if isempty(g.freqs2),   g.freqs2   = g.freqs;   end

% remove ERP if necessary
% -----------------------
X = squeeze(X);
Y = squeeze(Y);X = squeeze(X);
trials = size(X,2);
if strcmpi(g.rmerp, 'on')
    X = X - repmat(mean(X,2), [1 trials]);
    Y = Y - repmat(mean(Y,2), [1 trials]);
end

% perform timefreq decomposition
% ------------------------------
[alltfX freqs1 timesout1] = timefreq(X, srate, 'ntimesout',  g.ntimesout, 'timesout',  g.timesout,  'winsize',  g.winsize, ...
                                'tlimits', g.tlimits, 'detrend',   g.detrend,   'itctype',  g.itctype, ...
                                'subitc',  g.subitc,  'wavelet',   g.wavelet,   'padratio', g.padratio, ...
                                'freqs',   g.freqs,   'freqscale', g.freqscale, 'nfreqs',   g.nfreqs);
[alltfY freqs2 timesout2] = timefreq(Y, srate, 'ntimesout',  g.ntimesout, 'timesout',  g.timesout,  'winsize',  g.winsize, ...
                                'tlimits', g.tlimits, 'detrend',   g.detrend,   'itctype',  g.itctype, ...
                                'subitc',  g.subitc,  'wavelet',   g.wavelet2,  'padratio', g.padratio, ...
                                'freqs',   g.freqs2,  'freqscale', g.freqscale, 'nfreqs',   g.nfreqs);

% check time limits
% -----------------
if ~isempty(g.subwin)
    ind1      = find(timesout1 > g.subwin(1) & timesout1 < g.subwin(2));
    ind2      = find(timesout2 > g.subwin(1) & timesout2 < g.subwin(2));
    alltfX    = alltfX(:, ind1, :);
    alltfY    = alltfY(:, ind2, :);
    timesout1 = timesout1(ind1);
    timesout2 = timesout2(ind2);
end
if length(timesout1) ~= length(timesout2) || any( timesout1 ~= timesout2)
    disp('Warning: Time points are different for X and Y. Use ''timesout'' to specify common time points');
    [vals ind1 ind2 ] = intersect_bc(timesout1, timesout2);
    fprintf('Searching for common time points: %d found\n', length(vals));
    if length(vals) < 10, error('Less than 10 common data points'); end
    timesout1 = vals;
    timesout2 = vals;
    alltfX = alltfX(:, ind1, :);
    alltfY = alltfY(:, ind2, :);
end

% scan across frequency and time
% -------------------------------
%if isempty(g.alpha)
%    disp('Warning: if significance mask is not applied, result might be slightly')
%    disp('different (since angle is not made uniform and amplitude interpolated)')
%end

cohboot =[];
if ~strcmpi(g.method, 'latphase')
    for find1 = 1:length(freqs1)
        for find2 = 1:length(freqs2)
            for ti = 1:length(timesout1)

                % get data
                % --------
                tmpalltfx = squeeze(alltfX(find1,ti,:));
                tmpalltfy = squeeze(alltfY(find2,ti,:));

                %if ~isempty(g.alpha)
                %    tmpalltfy = angle(tmpalltfy);
                %    tmpalltfx = abs(  tmpalltfx);
                %    [ tmp cohboot(find1,find2,ti,:) newamp newangle ] = ...
                %        bootcircle(tmpalltfx, tmpalltfy, 'naccu', g.naccu);
                %    crossfcoh(find1,find2,ti) = sum ( newamp .* exp(j*newangle) );
                %else
                tmpalltfy = angle(tmpalltfy);
                tmpalltfx = abs(  tmpalltfx);
                if strcmpi(g.method, 'mod')
                    crossfcoh(find1,find2,ti) = sum( tmpalltfx .* exp(j*tmpalltfy) );
                elseif strcmpi(g.method, 'corrsin')
                    tmp = corrcoef( sin(tmpalltfy), tmpalltfx);
                    crossfcoh(find1,find2,ti) = tmp(2);
                else
                    tmp = corrcoef( cos(tmpalltfy), tmpalltfx);
                    crossfcoh(find1,find2,ti) = tmp(2);
                end
            end
        end
    end
elseif 1
    % this option computes power at a given latency
    % then computes the same as above (vectors)

    %if isempty(g.powerlat)
    %    error('You need to specify a latency for the ''powerlat'' option');
    %end

    gammapower = mean(10*log10(alltfX(:,:,:).*conj(alltfX)),1); % average all frequencies for power
    if isempty(g.gammapowerlim)
        g.gammapowerlim = [ min(gammapower(:)) max(gammapower(:)) ];
    end
    fprintf('Gamma power limits: %3.2f to %3.2f\n', g.gammapowerlim(1), g.gammapowerlim(2));
    power = 10*log10(alltfY(:,:,:).*conj(alltfY));
    if isempty(g.powerlim)
        for freq = 1:size(power,1)
            g.powerlim(freq,:) = [ min(power(freq,:)) max(power(freq,:)) ];
        end
    end
    for freq = 1:size(power,1)
        fprintf('Freq %d power limits: %3.2f to %3.2f\n', freqs2(freq), g.powerlim(freq,1), g.powerlim(freq,2));
    end

    % power plot
    %figure; plot(timesout2/1000, (mean(power(9,:,:),3)-mean(power(9,:)))/50);
    %hold on; plot(linspace(0, length(Y)/srate, length(Y)), mean(Y'), 'g');

    % phase with power
    % figure; plot(timesout2/1000, (mean(phaseangle(9,:,:),3)-mean(phaseangle(9,:)))/50);
    % hold on; plot(timesout1/1000, (mean(gammapower,3)-mean(gammapower(:)))/100, 'r');
    %figure; plot((mean(phaseangle(9,:,:),3)-mean(phaseangle(9,:)))/50+j*(mean(gammapower,3)-mean(gammapower(:)))/100, '.');

    matsize               = 32;
    matcenter             = (matsize-1)/2+1;
    matrixfinalgammapower = zeros(size(alltfY,1),size(alltfX,3),matsize,matsize);
    matrixfinalcount      = zeros(size(alltfY,1),size(alltfX,3),matsize,matsize);

    % get power indices
    if isempty(g.gammabase)
        g.gammabase = mean(gammapower(:));
    end
    fprintf('Gamma power average: %3.2f\n', g.gammabase);
    gammapoweradd  = gammapower-g.gammabase;
    gammapower     = floor((gammapower-g.gammapowerlim(1))/(g.gammapowerlim(2)-g.gammapowerlim(1))*(matsize-2))+1;
    phaseangle     = angle(alltfY);
    posx           = zeros(size(power));
    posy           = zeros(size(power));
    for freq = 1:length(freqs2)
        fprintf('Processing frequency %3.2f\n', freqs2(freq));
        power(freq,:,:) = (power(freq,:,:)-g.powerlim(freq,1))/(g.powerlim(freq,2)-g.powerlim(freq,1))*(matsize-3)/2+1;
        complexval      = power(freq,:,:).*exp(j*phaseangle(freq,:,:));
        posx(freq,:,:)  = round(real(complexval)+matcenter);
        posy(freq,:,:)  = round(imag(complexval)+matcenter);
        for trial = 1:size(alltfX,3) % scan trials
            for time = 1:size(alltfX,2)
                %matrixfinal(freq,posx(freq,time,trial),posy(freq,time,trial),gammapower(1,time,trial)) = ...
                %    matrixfinal(freq,posx(freq,time,trial),posy(freq,time,trial),gammapower(1,time,trial))+1;
                matrixfinalgammapower(freq,trial,posx(freq,time,trial),posy(freq,time,trial)) = ...
                    matrixfinalgammapower(freq,trial,posx(freq,time,trial),posy(freq,time,trial))+gammapoweradd(1,time,trial);
                matrixfinalcount(freq,trial,posx(freq,time,trial),posy(freq,time,trial)) = ...
                    matrixfinalcount(freq,trial,posx(freq,time,trial),posy(freq,time,trial))+1;
            end
        end
        %matrixfinal(freq,:,:,:) = convn(squeeze(matrixfinal(freq,:,:,:)), gs, 'same');
        %tmpmat = posx(index,:)+(posy(index,:)-1)*64+(gammapower(:)-1)*64*64;
        matrixfinalcount(freq, find(matrixfinalcount(freq,:) == 0)) = 1;
        matrixfinalgammapower(freq,:,:,:) = matrixfinalgammapower(freq,:,:,:)./matrixfinalcount(freq,:,:,:);
    end

    % average and smooth
    matrixfinalgammapowermean = squeeze(mean(matrixfinalgammapower,2));
    for freq = 1:length(freqs2)
        matrixfinalgammapowermean(freq,:,:) = conv2(squeeze(matrixfinalgammapowermean(freq,:,:)), gauss2d(5,5), 'same');
    end
    %matrixfinalgammapower = matrixfinalgammapower/size(alltfX,3)/size(alltfX,2);

    %vect = linspace(-pi,pi,50);
    %for f = 1:length(freqs2)
    %    crossfcoh(f,:) = hist(tmpalltfy(f,:), vect);
    %end

    % smoothing of output image
    % -------------------------
    %gs = gauss2d(6, 6, 6);
    %crossfcoh = convn(crossfcoh, gs, 'same');
    %freqs1    = freqs2;
    %timesout1 = linspace(-180, 180, size(crossfcoh,2));

    crossfcoh    = matrixfinalgammapowermean;
    crossfcohall = matrixfinalgammapower;

else
    % this option computes power at a given latency
    % then computes the same as above (vectors)

    %if isempty(g.powerlat)
    %    error('You need to specify a latency for the ''powerlat'' option');
    %end

    gammapower = mean(10*log10(alltfX(:,:,:).*conj(alltfX)),1); % average all frequencies for power
    if isempty(g.gammapowerlim)
        g.gammapowerlim = [ min(gammapower(:)) max(gammapower(:)) ];
    end
    power = 10*log10(alltfY(:,:,:).*conj(alltfY));
    if isempty(g.powerlim)
        for freq = 1:size(power,1)
            g.powerlim(freq,:) = [ min(power(freq,:)) max(power(freq,:)) ];
        end
    end

    % power plot
    %figure; plot(timesout2/1000, (mean(power(9,:,:),3)-mean(power(9,:)))/50);
    %hold on; plot(linspace(0, length(Y)/srate, length(Y)), mean(Y'), 'g');

    % phase with power
    % figure; plot(timesout2/1000, (mean(phaseangle(9,:,:),3)-mean(phaseangle(9,:)))/50);
    % hold on; plot(timesout1/1000, (mean(gammapower,3)-mean(gammapower(:)))/100, 'r');
    %figure; plot((mean(phaseangle(9,:,:),3)-mean(phaseangle(9,:)))/50+j*(mean(gammapower,3)-mean(gammapower(:)))/100, '.');

    matsize               = 64;
    matcenter             = (matsize-1)/2+1;
    matrixfinal           = zeros(size(alltfY,1),64,64,64);
    matrixfinalgammapower = zeros(size(alltfY,1),matsize,matsize);
    matrixfinalcount      = zeros(size(alltfY,1),matsize,matsize);

    % get power indices
    gammapoweradd  = gammapower-mean(gammapower(:));
    gammapower     = floor((gammapower-g.gammapowerlim(1))/(g.gammapowerlim(2)-g.gammapowerlim(1))*(matsize-1))+1;
    phaseangle     = angle(alltfY);
    posx           = zeros(size(power));
    posy           = zeros(size(power));
    gs             = gauss3d(6, 6, 6);
    for freq = 1:size(alltfY)
        fprintf('Processing frequency %3.2f\n', freqs2(freq));
        power(freq,:,:) = (power(freq,:,:)-g.powerlim(freq,1))/(g.powerlim(freq,2)-g.powerlim(freq,1))*(matsize-2)/2;
        complexval      = power(freq,:,:).*exp(j*phaseangle(freq,:,:));
        posx(freq,:,:)  = round(real(complexval)+matcenter);
        posy(freq,:,:)  = round(imag(complexval)+matcenter);
        for trial = 1:size(alltfX,3) % scan trials
            for time = 1:size(alltfX,2)
                %matrixfinal(freq,posx(freq,time,trial),posy(freq,time,trial),gammapower(1,time,trial)) = ...
                %    matrixfinal(freq,posx(freq,time,trial),posy(freq,time,trial),gammapower(1,time,trial))+1;
                matrixfinalgammapower(freq,posx(freq,time,trial),posy(freq,time,trial)) = ...
                    matrixfinalgammapower(freq,posx(freq,time,trial),posy(freq,time,trial))+gammapoweradd(1,time,trial);
                matrixfinalcount(freq,posx(freq,time,trial),posy(freq,time,trial)) = ...
                    matrixfinalcount(freq,posx(freq,time,trial),posy(freq,time,trial))+1;
            end
        end
        %matrixfinal(freq,:,:,:) = convn(squeeze(matrixfinal(freq,:,:,:)), gs, 'same');
        %tmpmat = posx(index,:)+(posy(index,:)-1)*64+(gammapower(:)-1)*64*64;
        matrixfinalcount(freq, find(matrixfinalcount(freq,:) == 0)) = 1;
        matrixfinalgammapower(freq,:,:) = matrixfinalgammapower(freq,:,:)./matrixfinalcount(freq, :,:);
        matrixfinalgammapower(freq,:,:) = conv2(squeeze(matrixfinalgammapower(freq,:,:)), gauss2d(5,5), 'same');
    end
    %matrixfinalgammapower = matrixfinalgammapower/size(alltfX,3)/size(alltfX,2);

    %vect = linspace(-pi,pi,50);
    %for f = 1:length(freqs2)
    %    crossfcoh(f,:) = hist(tmpalltfy(f,:), vect);
    %end

    % smoothing of output image
    % -------------------------
    %gs = gauss2d(6, 6, 6);
    %crossfcoh = convn(crossfcoh, gs, 'same');
    %freqs1    = freqs2;
    %timesout1 = linspace(-180, 180, size(crossfcoh,2));

    crossfcoh = matrixfinalgammapower;

end

% 7/31/2014 Ramon: crossfcohall sometimes does not exist depending on choice of input options
if ~exist('crossfcohall', 'var')
    crossfcohall = [];
end


% LAPPLOT -  Compute the discrete laplacian of EEG scalp distribution(s)
%
% Usage:
%   >> laplace = lapplot(map,eloc_file,draw)
%
% Inputs:
%    map        - Activity levels, size (nelectrodes,nmaps)
%    eloc_file	- Electrode location filename (.loc file)
%                 For format, see  >> topoplot example
%    draw       - If defined, draw the map(s) {default: no}
%
% Output:
%    laplace    - Laplacian map, size (nelectrodes,nmaps)
%
% Note: uses DEL2
%
% Author: Scott Makeig, SCCN/INC/UCSD, La Jolla, 1998
%
% See also: TOPOPLOT, GRADPLOT

% Copyright (C) Scott Makeig, SCCN/INC/UCSD, La Jolla, 1998
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

% 01-25-02 reformated help & license, added links -ad

function [laplac] = lapplot(map,filename,draw)

if nargin < 2
	help lapplot;
	return;
end

MAXCHANS = size(map,1);
GRID_SCALE = 2*MAXCHANS+5;
MAX_RADIUS = 0.5;

% ---------------------
% Read the channel file
% ---------------------
if ischar( filename )
	fid = fopen(filename);
	locations = fscanf(fid,'%d %f %f %s',[7 MAXCHANS]);
	fclose(fid);
	locations = locations';
	Th = pi/180*locations(:,2);   % convert degrees to rads
	Rd = locations(:,3);
	ii = find(Rd <= MAX_RADIUS); % interpolate on-scalp channels only
	Th = Th(ii);
	Rd = Rd(ii);
	[x,y] = pol2cart(Th,Rd);
else
	x = real(filename);
	y = imag(filename);
end;

% ---------------------------------------------------
% Locate nearest position of an electrode in the grid
% ---------------------------------------------------
xi = linspace(-0.5,0.5,GRID_SCALE);   % x-axis description (row vector)
yi = linspace(-0.5,0.5,GRID_SCALE);   % y-axis description (row vector)
for i=1:MAXCHANS
   [useless_var horizidx(i)] = min(abs(y(i) - xi));    % find pointers to electrode
   [useless_var vertidx(i)] = min(abs(x(i) - yi));     % positions in Zi
end

% -----------------
% Compute laplacian
% -----------------
for i=1:size(map,2)
   	[Xi,Yi,Zi] = griddata(y,x,map(:,i),yi',xi, 'v4');   % interpolate data

   	laplac2D = del2(Zi);
	positions = horizidx + (vertidx-1)*GRID_SCALE;
	laplac(:,i) = laplac2D(positions(:));

	% ------------------
	% Draw laplacian map
	% ------------------
	if exist('draw');
        mask = (sqrt(Xi.^2+Yi.^2) <= MAX_RADIUS);
        laplac2D(find(mask==0)) = NaN;

		subplot(ceil(sqrt(size(map,2))), ceil(sqrt(size(map,2))), i);
		contour(laplac2D);
		title( int2str(i) );

% %%% Draw Head %%%%
ax = axis;
width = ax(2)-ax(1);
axis([ax(1)-width/3 ax(2)+width/3 ax(3)-width/3 ax(4)+width/3])
steps = 0:2*pi/100:2*pi;
basex = .18*MAX_RADIUS;
tip = MAX_RADIUS*1.15;
base = MAX_RADIUS-.004;
EarX = [.497 .510 .518 .5299 .5419 .54 .547 .532 .510 .489];
EarY = [.0555 .0775 .0783 .0746 .0555 -.0055 -.0932 -.1313 -.1384 -.1199];

HCOLOR = 'k';
HLINEWIDTH = 1.8;

% Plot Head, Ears, Nose
hold on
plot(1+width/2+cos(steps).*MAX_RADIUS*width,...
     1+width/2+sin(steps).*MAX_RADIUS*width,...
    'color',HCOLOR,'Linestyle','-','LineWidth',HLINEWIDTH); % head

plot(1+width/2+[.18*MAX_RADIUS*width;0;-.18*MAX_RADIUS*width],...
     1+width/2+[base;tip;base]*width,...
    'Color',HCOLOR,'LineWidth',HLINEWIDTH);                 % nose

plot(1+width/2+EarX*width,...
     1+width/2+EarY*width,...
           'color',HCOLOR,'LineWidth',HLINEWIDTH)           % l ear
plot(1+width/2-EarX*width,...
     1+width/2+EarY*width,...
           'color',HCOLOR,'LineWidth',HLINEWIDTH)           % r ear

hold off
axis off

	end
end;

return;