function [match, dev, dcm, drotm, conflicts, toBeChecked] = gtINDEXMatchGrains(...
                      grain1, grain2, ph1, dc, drot, tol)
% GTINDEXMATCHGRAINS  Matches multiple indexed grains between two datasets; 
%                     can also be used to find unmerged grains.
%
% [match, dev, dcm, drotm, conflicts, toBeChecked] = gtINDEXMatchGrains(grain1, grain2, ph1, dc, drot, tol)
% --------------------------------------------------------------------------------------------------------
%
% For each grain in the reference set 'grain1' it tries to find the 
% corresponding grain in the set 'grain2'. It uses tolerances in 'tol' 
% or default ones to find matching candidates. The best candidate is 
% selected based on the smallest average deviations of its parameters 
% from the reference grain. Uniqueness of the match is not fulfilled, 
% more than one matches may be found for a grain in 'grain2'.
%
% When used with grain1 = grain2, conflicting grains which should be merged 
% can be found in a dataset. Essentially the same criteria are used as in 
% gtIndexter but the results may be slightly different for matches close 
% to the tolerance limits.  
%
% It considers translation and rotation between the two datasets. 
% Optimized values for the translation and rotation can be achieved by
% using the function iteratively. The first guess by the user has to be 
% close enough to find correct matching grains. Then the returned 
% parameters may be used to update the guess.
% 
% NOTE !
% It needs to be launched from the folder belonging to grain1. 
% Crstallography info is loaded from parameters.mat in the current folder
% for phase 'ph1'. 
%
% ALGORITHM 
% First the new grain centers and orientations are calculated for grain2
% according to the input translation and rotation. The new Rodrigues 
% vectors for grain2 are sought in the fundamental zone that is extended
% to take into account inaccuracies of the measurements. Candidates for 
% each grain1 is found and the best fitting one is selected, based on a 
% figure of merit that is weighted by the actual tolerance values. Median 
% deviations of the grain centers and Rodrigues vectors are calculated, 
% from which updated values of the misfit between the two datasets are 
% given assuming a small misorientation. The grain centers are not
% but only the grain orientations are used to determine the 
% average misorientation of the two datasets.
% Suggestion: to compare orientations, instead of Rodrigues coordinates, 
% it might be easier to use the misorientation matrices and symmetry 
% operators.
% 
%
% OPTIONAL INPUT: 
%   grain1  = indexed grain data of reference set as output from Indexter
%             {if undefined, it is loaded for the current dataset}
%   grain2  = indexed grain data to be matched as output from Indexter
%             {if undefined, it is same as grain1}
%   ph1     = phase ID in grain1 {default is 1}
%   dc      = global translation between datasets from a position given in 
%             grain2 to the same in grain1 {default is [0 0 0]}
%   drot    = global rotation around the origin of grain2; the rotation 
%             matrix that transforms a position vector given in grain2 
%             into grain1 (size 3x3 for column vectors); {default is the
%             identity matrix}:   p1 = drot*p2 + dc;
%   tol     = tolerances for finding matching grains and their default values
%               tol.distf = 0.5   - distance between center of mass;
%                                   mult. factor of smaller bounding box size
%                                     toldist = tol.distf * min(bbxs,bbys)
%               tol.Rdist = 0.02  - absolute distance in Rodrigues space     
%               tol.bbxs  = 2     - max. bounding box size x ratio
%               tol.bbys  = 2     - max. bounding box size y ratio      
%               tol.int   = 1e10  - max. intensity ratio      
%
%
% OUTPUT:
%   match       = array of indices of matching grains [grain_ind1 grain_ind2]
%   dev         = vectors of deviations of grain properties
%   dcm         = updated guess for dc
%   drotm       = updated guess for drot
%   conflicts   = list of conflicts found (same as printed on command line);
%                   conflicts{:,1}: grains from set1 for which more than one
%                                   possible matches were found
%                   conflicts{:,2}: corresponding possible matches from set2
%   toBeChecked = conflicts list formatted to be saved as
%                 parameters.input.forcemerge
%
% USAGE EXAMPLE
%   Revise tolerances; then run:
%     [match,dev,dc,drot,conflicts] = gtINDEXMatchGrains(grain1,grain2,1,[],[],tol);
%   Then run again a few times to iterate:
%     [match,dev,dc,drot,conflicts] = gtINDEXMatchGrains(grain1,grain2,1,dc,drot,tol);
%
%   For checking unmerged grains in a single dataset (revise the tolerances!):
%     gtINDEXMatchGrains;
%     [~,~,~,~,conflicts] = gtINDEXMatchGrains([],[],1,[],[],tol);
%
%
% Version 002 12-09-2012 by PReischig
%   Added conflicts to output.
% 
% Version 001 25-06-2012 by PReischig
%


% Attepts were to determine the average misorientation matrix: 
%   1. Calculating one from the median deviation of the Rodrigues vectors.
%      It works well (small rotations), and is very simple, so this is used. 
%   2. Composed a true rotation matrix from small rotations around the 
%      base axes with the symmetric part of the average misorientation 
%      matrix. It also worked well, but more complicated.
%   3. n-th root mean of misorientation matrices multiplied together. 
%      Didn't work.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialize
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


if ~exist('ph1','var') || isempty(ph1)
  ph1 = 1;	
end

if ~exist('dc','var') || isempty(dc)
  dc = [0 0 0];	
end

if ~exist('drot','var') || isempty(drot)
  drot = [];	
end

if ~exist('cent','var') || isempty(cent)
  cent = false;	
end

if ~exist('grain1','var') || isempty(grain1)
    fname = sprintf('4_grains/phase_%02d/index.mat',ph1);
    fprintf('Loading grain info from %s ...\n',fname)
    grain1 = load(fname);
    grain1 = grain1.grain;
end

if ~exist('grain2','var') || isempty(grain2)
    grain2 = grain1;
end


if ~exist('tol','var') || isempty(tol)
  tol.distf = 0.5;          % 0.5
  tol.Rdist = 0.02;         % 0.05
  tol.bbxs  = 2;            % 3 
  tol.bbys  = 2;            % 3   
  tol.int   = 1e10;         % 1e10
end

nof_grains1 = length(grain1);
nof_grains2 = length(grain2);


%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Prepare grain data
%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Sort grains by Z position and only keep one third at the center 
% if cent
% 	cz1  = gtIndexAllGrainValues(grain1,'center',[],3);
% 	ids1 = (1:length(grain1))';
% 
% 	S = sortrows([ids1,cz1],2);
% 
% 	cgrain1 = [];
% 	for ii = floor(nof_grains1/3):nof_grains1*2/3
% 		cgrain1 = [cgrain1,grain1(S(ii,1))];
% 	end
% 
% 	grain1 = cgrain1;
% 	nof_grains1 = length(grain1);
% end

disp('Loading pixelsize and crystallographic info from parameters.mat ...')
parameters = [];
load parameters
cryst   = parameters.cryst(ph1);
pixsize = 0.5*(parameters.labgeo.pixelsizeu + parameters.labgeo.pixelsizev);


match      = NaN(nof_grains1,2);
match(:,1) = (1:nof_grains1)';


% Load relevant info into the gr1 structure (vectors and matrices):
%gr1.id            = gtIndexAllGrainValues(grain1,'id',[],1);
gr1.ind            = (1:nof_grains1)';

gr1.center(:,1)   = gtIndexAllGrainValues(grain1,'center',[],1);
gr1.center(:,2)   = gtIndexAllGrainValues(grain1,'center',[],2);
gr1.center(:,3)   = gtIndexAllGrainValues(grain1,'center',[],3);

gr1.R_vector(:,1) = gtIndexAllGrainValues(grain1,'R_vector',[],1);
gr1.R_vector(:,2) = gtIndexAllGrainValues(grain1,'R_vector',[],2);
gr1.R_vector(:,3) = gtIndexAllGrainValues(grain1,'R_vector',[],3);

gr1.R_onedge(:,1) = gtIndexAllGrainValues(grain1,'R_onedge',[],1);
gr1.bbxs(:,1)     = gtIndexAllGrainValues(grain1,'stat','bbxsmean',1);
gr1.bbys(:,1)     = gtIndexAllGrainValues(grain1,'stat','bbysmean',1);
gr1.int(:,1)      = gtIndexAllGrainValues(grain1,'stat','intmean',1);


% Load relevant info into the gr2 structure (vectors and matrices):
%gr2.id            = gtIndexAllGrainValues(grain2,'id',[],1);
gr2.ind            = (1:nof_grains2)';

gr2.center(:,1)   = gtIndexAllGrainValues(grain2,'center',[],1);
gr2.center(:,2)   = gtIndexAllGrainValues(grain2,'center',[],2);
gr2.center(:,3)   = gtIndexAllGrainValues(grain2,'center',[],3);

gr2.R_vector(:,1) = gtIndexAllGrainValues(grain2,'R_vector',[],1);
gr2.R_vector(:,2) = gtIndexAllGrainValues(grain2,'R_vector',[],2);
gr2.R_vector(:,3) = gtIndexAllGrainValues(grain2,'R_vector',[],3);

gr2.R_onedge(:,1) = gtIndexAllGrainValues(grain2,'R_onedge',[],1);
gr2.bbxs(:,1)     = gtIndexAllGrainValues(grain2,'stat','bbxsmean',1);
gr2.bbys(:,1)     = gtIndexAllGrainValues(grain2,'stat','bbysmean',1);
gr2.int(:,1)      = gtIndexAllGrainValues(grain2,'stat','intmean',1);


gr2_rot = NaN(3,3,nof_grains2);
dev.rot = NaN(3,3,nof_grains1);


%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reference planes
%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Extension to Rodrigues fundamental zone
% Rext = 0;
Rext = tol.Rdist;

if (any(gtIndexAllGrainValues(grain1,'R_onedge',[],1,[])) || ~isempty(drot))
        
    Bmat = gtCrystHKL2CartesianMatrix(cryst.latticepar);
    spacegroup = cryst.spacegroup;
    
    fzone_acc = gtCrystRodriguesFundZone(spacegroup,0);
    fzone_ext = gtCrystRodriguesFundZone(spacegroup,Rext);
    
    
    if (isfield(cryst,'hklsp') && ~isempty(cryst.hklsp))
        
        % Take the first three plane normals
        hklsp = cryst.hklsp(:,1:3);

        % All signed hkl-s for the given family
        shkl1 = cryst.hklsp(:,cryst.thetatypesp == cryst.thetatypesp(1));
        shkl2 = cryst.hklsp(:,cryst.thetatypesp == cryst.thetatypesp(2));
        shkl3 = cryst.hklsp(:,cryst.thetatypesp == cryst.thetatypesp(3));
    
        % Both hkl and -h-k-l have to be considered:
        shkl1 = [shkl1, -shkl1];
        shkl2 = [shkl2, -shkl2];
        shkl3 = [shkl3, -shkl3];
  
    else
        % Take the {1,0,0} planes (doesn't matter if reflections really exist)
        if size(cryst.hkl,1) == 3
            hklsp = [1 0 0; 0 1 0; 0 0 1]';
        elseif size(cryst.hkl,1) == 4
            hklsp = [1 0 -1 0; 0 1 -1 0; 0 0 0 1]';
        else
            error('Field parameters.cryst.hkl is not set correctly.')
        end
        
        % All signed hkl-s for the given family (both hkl and -h-k-l)
        shkl1 = gtCrystSignedHKLs(hklsp(:,1)',spacegroup)';
        shkl2 = gtCrystSignedHKLs(hklsp(:,2)',spacegroup)';
        shkl3 = gtCrystSignedHKLs(hklsp(:,3)',spacegroup)';
    end
    
    
    % Plane normals with Cartesian Crystal coordinates
    pl_cry = gtCrystHKL2Cartesian(hklsp,Bmat);  % takes and returns column vectors
    
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Grain2 grain centers
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isempty(drot)
    rotmat = eye(3,3);
else
    rotmat = drot;   % for column vectors ! for row vectors, use transpose!
end

% Rotate and translate grain centers
gr2.center = gr2.center*rotmat' + dc(ones(nof_grains2,1),:);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Grain2 Rodrigues coordinates
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculate new Rodrigues lines and vectors inside the fundamental zone 
% for the new, rotated positions

gr2.Rlines = cell(nof_grains2,1);
gr2.Rids = cell(nof_grains2,1);

if (any(gtIndexAllGrainValues(grain1,'R_onedge',[],1,[])) || ~isempty(drot))
   
    disp('Calculating Rodrigues coordinates ...')

    % Generate a prime number larger than 3 for each grain to be used to 
    % generate distinct reflection id-s later 
    pr = primes(1e6);
    pr(1:2) = [];
    pr(nof_grains2+1:end) = [];
    
    
    for ii = 1:nof_grains2
        
        % Crystal to Sample coordinate transformation matrix
        cry2sam = Rod2g(gr2.R_vector(ii,:));
        
        % Plane normals in the Sample ref. of dataset2
        pl_sam2 = cry2sam*pl_cry; 

        % Plane normals rotated to the position in Sample ref. of dataset1
        pl_sam1 = rotmat*pl_sam2;


        % Lines in Rodrigues space (might be empty)
        Rline1 = gtCrystRodriguesVector(pl_sam1(:,1)',shkl1',Bmat,fzone_ext);
        Rline2 = gtCrystRodriguesVector(pl_sam1(:,2)',shkl2',Bmat,fzone_ext);
        Rline3 = gtCrystRodriguesVector(pl_sam1(:,3)',shkl3',Bmat,fzone_ext);

        Rline = [Rline1; Rline2; Rline3];

        % Vector of line indices to identify which plane they belong to
        Rid = [1*ones(size(Rline1,1),1); 2*ones(size(Rline2,1),1); 3*ones(size(Rline3,1),1)]; 
        
        % Find new Rodrigues vector
        Rvec = gtCrystRodriguesTestCore(Rline, Rid, fzone_acc, fzone_ext, Rext);
        
        if isempty(Rvec)
            disp('WARNING! Rodrigues vector could not be fitted. Possible bug.' )
            gr2.R_vector(ii,:) = NaN;
            gr2_rot(:,:,ii)    = NaN;
        else
            gr2.R_vector(ii,:) = Rvec;
            gr2_rot(:,:,ii)    = Rod2g(gr2.R_vector(ii,:));
        end           

        % Rodrigues line and distinct line ID (Rid contains (1,2,3))
        gr2.Rlines{ii} = Rline;
        gr2.Rids{ii}   = Rid*pr(ii); 

        % For debugging: 
        %   Rvec = gtCrystRodriguesTest(pl_sam1', [shkl1(:,1), shkl2(:,1), shkl3(:,1)]', spacegroup, Bmat, 0, 0, 0);
       
    end
else
    for ii = 1:nof_grains2
        gr2_rot(:,:,ii) = Rod2g(gr2.R_vector(ii,:));
    end
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Grain1 Rodrigues coordinates
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculate Rodrigues lines inside the fundamental zone

gr1.Rlines = cell(nof_grains1,1);
gr1.Rids   = cell(nof_grains1,1);

for ii = 1:nof_grains1
  
    if gr1.R_onedge(ii)
        
        % Crystal to Sample coordinate transformation matrix
        cry2sam = Rod2g(gr1.R_vector(ii,:));
        
        % Plane normals in the Sample ref. of dataset1
        pl_sam1 = cry2sam*pl_cry;
        
        % Lines in Rodrigues space (some might be empty)
        Rline1 = gtCrystRodriguesVector(pl_sam1(:,1)', shkl1', Bmat, fzone_ext);
        Rline2 = gtCrystRodriguesVector(pl_sam1(:,2)', shkl2', Bmat, fzone_ext);
        Rline3 = gtCrystRodriguesVector(pl_sam1(:,3)', shkl3', Bmat, fzone_ext);
        
        gr1.Rlines{ii} = [Rline1; Rline2; Rline3];
        
        % Vector of line indices to identify which plane they belong to
        gr1.Rids{ii} = [1*ones(size(Rline1,1),1); 2*ones(size(Rline2,1),1); 3*ones(size(Rline3,1),1)];
        
        % For debugging: find Rodrigues vector
        Rvec = gtCrystRodriguesTestCore(gr1.Rlines{ii}, gr1.Rids{ii}, fzone_acc, fzone_ext, Rext);
        %   Rvec = gtCrystRodriguesTest(pl_sam1', [shkl1(:,1), shkl2(:,1), shkl3(:,1)]', spacegroup, Bmat, 0, 0, 0);
        if ~isempty(Rvec)
            gr1.Rvec(ii,:) = Rvec;
        end
    end
    
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Match grains
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

disp('Matching grains ...')

conflicts = cell(0,2);


for ii = 1:nof_grains1

    % Actual grain data
	act = sfSelectGrains(ii,gr1);      
    
    % Get candidate grain indices to be matched with the actual grain
	tomatch = sfCheckMatching(act, gr2, tol, fzone_acc, fzone_ext, pixsize); 

	if ~isempty(tomatch)

		ltm = length(tomatch);
		
		if ltm > 1

            toldist    = min(act.bbxs, act.bbys) * pixsize * tol.distf;

			dcenter    = sum((act.center(ones(ltm,1),:)   - gr2.center(tomatch,:)).^2,2)   / (toldist^2);
			dR_vector  = sum((act.R_vector(ones(ltm,1),:) - gr2.R_vector(tomatch,:)).^2,2) / (tol.Rdist^2);
			dbbxs      = sum((act.bbxs - gr2.bbxs(tomatch,:)).^2,2)/(act.bbxs^2) / (tol.bbxs^2);
			dbbys      = sum((act.bbys - gr2.bbys(tomatch,:)).^2,2)/(act.bbys^2) / (tol.bbys^2);
			dint       = sum((act.int  - gr2.int(tomatch,:)).^2,2) / (tol.int^2);

			dsum = dcenter + dR_vector + dbbxs + dbbys + dint;
			
			sortM      = [];
			sortM(:,1) = dsum;
			sortM(:,2) = tomatch;
			sortM(:,3) = gr2.ind(tomatch);
			
            % Find best candidate by sorting
			sortM = sortrows(sortM,1);

			tomatch = sortM(:,2);

			disp(['Multiple matches found for grain #' num2str(ii) ':'])
			disp(tomatch)
            
            conflicts{end+1,1} = ii;
            conflicts{end,2}   = sort(tomatch)';
            
		end

		match(ii,2) = tomatch(1);

	end

end

toBeChecked.gr1 = gr1;
toBeChecked.gr2 = gr2;
% if isempty(conflicts)
%     fprintf('\nNo conflicts have been found with the actual tolerances.\n')
%     toBeChecked = [];
% else
%     if (length([conflicts{:,2}]) == length(unique([conflicts{:,2}])))
%         fprintf('Match conflict list is unique.\n')
%     else
%         fprintf('Match conflict list is NOT unique.\n')
%         fprintf('  Some grain(s) in set2 may be assigned to more than one in set1.\n')
%     end
%     
%     if nargout == 6
%       if size(conflicts{1},2)>1
%           for ii=1:size(conflicts,1)
%           tmp{ii} = [conflicts{ii,:}];
%           end
%           tmp2= cell2mat(tmp);
%       else
%         tmp = cell2mat(conflicts);
%         pippo_cc = cell(0,2);
%         for ii=1:size(tmp,1)
%             tmp2 = tmp(ii,:);
%             tmp2 = unique(tmp2);
%             pippo_cc{ii} = tmp2;
%         end
%         tmp = cell2mat(pippo_cc');
%         toBeChecked = unique(tmp, 'rows');
%       end
%     end
% end

    

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Evalute grain match
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dev.xyz   = NaN(nof_grains1,3);
dev.Rvec  = NaN(nof_grains1,3);
dev.dist  = NaN(nof_grains1,1);
dev.Rdist = NaN(nof_grains1,1);
dev.int   = NaN(nof_grains1,1);
dev.bbys  = NaN(nof_grains1,1);
dev.bbxs  = NaN(nof_grains1,1);
dev.rot   = NaN(3,3,nof_grains1);

for ii = 1:size(match,1)
	
	if ~isnan(match(ii,2))
		dev.xyz(ii,:)   = gr2.center(match(ii,2),:) - gr1.center(ii,:);
		dev.dist(ii,:)  = sqrt(dev.xyz(ii,:) * dev.xyz(ii,:)');
		dev.Rvec(ii,:)  = gr2.R_vector(match(ii,2),:) - gr1.R_vector(ii,:);
 		dev.Rdist(ii,:) = sqrt(dev.Rvec(ii,:) * dev.Rvec(ii,:)');
		dev.bbxs(ii,1)  = gr2.bbxs(match(ii,2)) / gr1.bbxs(ii);
		dev.bbys(ii,1)  = gr2.bbys(match(ii,2)) / gr1.bbys(ii);
		dev.int(ii,1)   = gr2.int(match(ii,2))  / gr1.int(ii);
        dev.rot(:,:,ii) = Rod2g(gr1.R_vector(ii,:)) - gr2_rot(:,:,match(ii,2));
	end
	
end


ok = ~isnan(dev.dist(:,1));

Rvec_med = median(dev.Rvec(ok,:),1);
dxyz_med = median(dev.xyz(ok,:));

ddist  = sqrt(sum(dev.dist(ok,:).^2,1)/sum(ok));
dRdist = sqrt(sum(dev.Rdist(ok,:).^2,1)/sum(ok));


dcm = dc - dxyz_med;


% rotasymm = 0.5*(rot_med0 - rot_med0');
% 
% rmc  = gtMathsRotationMatrixComp([1; 0; 0],'col');
% rotx = gtMathsRotationTensor(asind(rotasymm(3,2)),rmc);
% 
% rmc  = gtMathsRotationMatrixComp([0; 1; 0],'col');
% roty = gtMathsRotationTensor(asind(rotasymm(1,3)),rmc);
% 
% rmc  = gtMathsRotationMatrixComp([0; 0; 1],'col');
% rotz = gtMathsRotationTensor(asind(rotasymm(2,1)),rmc);
% 
% rot_med = rotx*roty*rotz;


rot_med = Rod2g(Rvec_med)';

if isempty(drot)
    drotm = rot_med;
else
    drotm = rot_med*drot;
end


fprintf('\nRemaining median deviation of Rodrigues vectors:\n  [%s]',num2str(Rvec_med))

fprintf('\nRemaining median deviation of grain centers:\n  [%s]',num2str(dxyz_med));

fprintf('\nRemaining root mean square deviation of Rodrigues vectors:\n  %g',dRdist);

fprintf('\nRemaining root mean square deviation of grain center distances:\n  %g',ddist);

%disp('Remaining median misorientation of grains:')
%disp(rot_med)

fprintf('\n\nRecommended total displacement correction (dc):\n  [%s]\n',num2str(dcm));

disp('Recommended total rotation correction (drot):')
disp(drotm)

fprintf('Number of matching grains found:\n  %d \n\n',sum(~isnan(match(:,2))))


end % of main function


%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUB-FUNCTIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function tomatch = sfCheckMatching(act, cand, tol, fzone_acc, fzone_ext, pixsize)
        
    tomatch = (1:length(cand.ind))';  % initial indices of grain candidates to be merged
    
    % Delete those indices that don't meet the following constraints:
    
    % Bounding box Y size close enough?
    cons = (cand.bbys > act.bbys/tol.bbys) & ...
        (cand.bbys < tol.bbys*act.bbys);
    cand = sfSelectGrains(cons, cand);
    tomatch(~cons) = [];
    
    % Distance of centers close enough?
    toldist = min(act.bbxs, act.bbys) * pixsize * tol.distf;
    dvec    = repmat(act.center,length(cand.ind),1) - cand.center;
    dist    = sqrt(sum(dvec.*dvec,2));
    cons    = dist < toldist;
    cand    = sfSelectGrains(cons, cand);
    tomatch(~cons) = [];
    
    % Bounding box X size close enough?
    cons = (cand.bbxs > act.bbxs/tol.bbxs) & ...
        (cand.bbxs < tol.bbxs*act.bbxs);
    cand = sfSelectGrains(cons, cand);
    tomatch(~cons) = [];
    
    % Intensity close enough?
    cons = (cand.int > act.int/tol.int) & ...
        (cand.int < tol.int*act.int);
    cand = sfSelectGrains(cons,cand);
    tomatch(~cons) = [];
    
    if isempty(tomatch)
        return
    end
    
    % Rodriguez vector close enough?
    
    % If any of them is on edge of fundamental zone
    if any([act.R_onedge; cand.R_onedge])  
        
        % Do Rodrigues test from scratch
        
        Rlines = vertcat(act.Rlines{:}, cand.Rlines{:});
        Rids   = vertcat(act.Rids{:}, cand.Rids{:});
        
        candinds = zeros(length(act.Rids{:}),1);
        for jj = 1:length(tomatch)
            candinds = [candinds; jj(ones(length(cand.Rids{jj}),1))];
        end
        
        % Find new Rodrigues vector
        [~, ~, goodlines] = gtCrystRodriguesTestCore(Rlines, Rids, ...
                               fzone_acc, fzone_ext, tol.Rdist);
        
        
        % Check which candidate has all of its 3 reflections accepted
        
        candinds = candinds(goodlines);
        candinds(candinds==0) = [];
        
        unicandinds = unique(candinds);
        
        okinds = false(size(tomatch));
        
        for jj = 1:length(unicandinds)
            if (sum(candinds==unicandinds(jj)) == 3)
                okinds(unicandinds(jj)) = true;
            end
        end
        
        tomatch = tomatch(okinds);
        
    else
        dRvec = repmat(act.R_vector,length(cand.ind),1) - cand.R_vector;
        Rdist = sqrt(sum(dRvec.*dRvec,2));
        cons  = Rdist < tol.Rdist;
        tomatch(~cons) = [];
    end
    
end



function grk = sfSelectGrains(keeplist,gr)

    grk.ind      = gr.ind(keeplist);
    grk.int      = gr.int(keeplist);
    grk.center   = gr.center(keeplist,:);
    grk.R_vector = gr.R_vector(keeplist,:);
    grk.R_onedge = gr.R_onedge(keeplist,:);
    grk.bbys     = gr.bbys(keeplist);
    grk.bbxs     = gr.bbxs(keeplist);
    grk.Rlines   = gr.Rlines(keeplist);
    grk.Rids     = gr.Rids(keeplist);
 
end












