function [Rout, good_plnorms, Rdist, Redge] = gtCrystRodriguesTest(...
         plnorm, hkl, spacegroup, Bmat, shkl_flag, plot_flag, toldist)
%
% FUNCTION [Rout, good_plnorms, Rdist, Redge] = gtCrystRodriguesTest(...
%          plnorm, hkl, spacegroup, Bmat, shkl_flag, plot_flag, toldist)
%
% The use of gtCrystRodriguesTestCore instead of this function is recommended.
%
% Performs an orientation test in Rodrigues space on the specified plane
% normals. Gives which plane normals constitute a consistent crystal and 
% its fitted Rodrigues vector.
% Also determines how far this point is from the nearest boundary of the
% fundamental zone, and whether this distance is smaller than the specified
% limit.
%
% INPUT
%   plnorm      - plane normal coordinates in sample coords (nx3)
%   hkl         - hkl reflection types, eg. [1 1 1; 0 0 2; 2 2 0] 
%                 OR if known, specific hkl reflections, eg. [-1 1 -1; 0 0
%                 2; 2 -2 0]. (nx3; nx4 for hexagonal lattice)
%   spacegroup  - crystallographic spacegroup
%   Bmat        - transformation matrix from HKL to Cartesian coordinates
%   shkl_flag   - set to true for speeding up, if the signed hkl-s
%                 are used
%   plot_flag   - if true, figure is plotted showing data in Rodrigues
%                 space
%   toldist     - tolerance for distance in Rodrigues space (if empty, 
%                 default value is used)
%
% OUTPUT
%   Rout         - fitted Rodrigues vector
%   good_plnorms - the good plane normals that fit the solution
%   Rdist        - distance between Rout and the nearest fundamental zone 
%                  boundary plane in Rodrigues space
%   Redge        - true if Rdist is smaller than the specified tolerance
%   

     
% Tolerance for distance in Rodrigues's space; default=0.02
if isempty(toldist)
    tol.dist = 0.02;     
else
    tol.dist = toldist;
end

tol.pd = tol.dist/5;   % approximate tolerance angle for parallelism (radian)
tol.pc = cos(tol.pd);  % tolerance for parallelism (cosine of angle)
tol.sm = tol.dist;     % safety margin in Rodr. space (tolerance to extend fund. zone)

nof_pln = size(plnorm,1);  % number of plane normals

good_plnorms = false(1,nof_pln);
Rout   = [];
R.l    = [];
R.wpl  = [];
R.enda = [];
R.endb = [];
Rdist  = [];
Redge  = [];

intsc.lines  = [];
intsc.planes = [];
intsc.Rvec   = [];

fzone_ext = gtCrystRodriguesFundZone(spacegroup,tol.sm);  % fund. zone with safety margin
fzone_acc = gtCrystRodriguesFundZone(spacegroup,0);       % exact fund. zone

symm = gtCrystGetSymmetryOperators([], spacegroup);

%% Determine line coordinates in Rodrigues space

% If the signed hkl is known for each plane normal
if shkl_flag
	
	% Loop through all plane normals  
	for i = 1:nof_pln                                  
	
		% Lines coordinates
		[Rvec,isec1,isec2] = gtCrystRodriguesVector(plnorm(i,:),hkl(i,:),Bmat,fzone_ext);
		
		R.l    = [R.l; Rvec];                     % all lines in Rodrigues's space (may be empty)
		R.wpl  = [R.wpl; i*ones(size(Rvec,1),1)]; % which plane normal it belongs to 
		R.enda = [R.enda; isec1];                 % intersection A with fund zone
		R.endb = [R.endb; isec2];                 % intersection B with fund zone
	end

% If the signed hkl-s are unknown, and only the hkl families are known
else
	
	% Loop through all plane normals
	for i = 1:nof_pln
		%[Rvec,isec1,isec2] = gtRodriguesVectors2(plnorm(i,:),hkl(i,:),spacegroup,latticepar,tol.sm);
		
		% All signed hkl-s for given hkl family
		shkls = gtCrystSignedHKLs(hkl(i,:), symm);
		
		[Rvec,isec1,isec2] = gtCrystRodriguesVector(plnorm(i,:),shkls,Bmat,fzone_ext);
		
		R.l    = [R.l; Rvec];                     % all lines in Rodrigues's space (may be empty)
		R.wpl  = [R.wpl; i*ones(size(Rvec,1),1)]; % which plane normal it belongs to 
		R.enda = [R.enda; isec1];                 % intersection A with fund zone
		R.endb = [R.endb; isec2];                 % intersection B with fund zone
	end	
	
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Determine line intersections -> base nodes

nof_lines = length(R.wpl);

Lp = false(nof_lines);   % 
Ld = zeros(nof_lines);   % distance of Rlines pairwise
Lc = zeros(nof_lines);   % angle of Rlines pairwise

Lrx = zeros(nof_lines);
Lry = zeros(nof_lines);
Lrz = zeros(nof_lines);

% store line id-s for identification
[Lidb,Lida] = meshgrid(1:nof_lines);

Pida = repmat(R.wpl,1,nof_lines);
Pidb = Pida';


% Determine intersections of all lines with one another in Rodr. space
for i = 1:nof_lines-1
	% distance pair-wise
	Ld(i,i+1:end) = gtMathsLinesDists(R.l(i,:),R.l(i+1:end,:));  
	
	% dot product of the direction vectors pair-wise (=cosine of the angles)
	Lc(i,i+1:end) = sum(repmat(R.l(i,4:6),nof_lines-i,1).*R.l(i+1:end,4:6),2);

	% intersection points in Rodr. space
	r = gtMathsLinePairsIntersections(R.l(i,:),R.l(i+1:end,:));
	Lrx(i,i+1:end) = r(:,1);
	Lry(i,i+1:end) = r(:,2);
	Lrz(i,i+1:end) = r(:,3);
end

% Which lines are identical pair-wise (small distance and angle) 
%   -> double detection of Friedel pairs or wrong indexing
Lp = (Ld <= tol.pd) & (abs(Lc) >= tol.pc);

% Make matrices symmetric
Ld = Ld+Ld';
Lc = Lc+Lc';
Lp = Lp | Lp';

Lrx = Lrx+Lrx';
Lry = Lry+Lry';
Lrz = Lrz+Lrz';

% Which lines are closer than the tolerance ? 
Lnear = Ld <= tol.dist;
for i = 1:nof_lines
	Lnear(i,i) = false;
end

% Lines that are close but not identical (parallel) constitute the base set:
baseset = Lnear & (~Lp);

% Nodes are the intersection points of the base set:
Nbase   = [Lrx(baseset),Lry(baseset),Lrz(baseset)];
LAbase  = Lida(baseset);
LBbase  = Lidb(baseset);
PLAbase = Pida(baseset);
PLBbase = Pidb(baseset);

% Which nodes are in the (extended) fundamental zone ?
% inzone = false(size(Nbase,1),1);
% for i = 1:size(Nbase,1)
% 	inzone(i) = gtMathsIsPointInPolyhedron(Nbase(i,:),fzone_ext);
% end
inzone = gtMathsIsPointInPolyhedron(Nbase,fzone_ext);

% Keep only base nodes in the fund. zone
Nbase   = Nbase(inzone,:);
LAbase  = LAbase(inzone);
LBbase  = LBbase(inzone);
PLAbase = PLAbase(inzone);
PLBbase = PLBbase(inzone);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Find neighbouring nodes

% Check which base nodes are close to each other -> neighbouring nodes
told = tol.dist^2;

nof_Nbase = size(Nbase,1);        % no. of nodes
Nneib = false(nof_Nbase);         % neighbouring nodes

parfor ii = 1:nof_Nbase-1
  
  dist = [Nbase(ii,1)-Nbase(:,1), ...
          Nbase(ii,2)-Nbase(:,2), ...
          Nbase(ii,3)-Nbase(:,3)];
  
  dist = sum(dist.*dist,2);
  
  distok = dist <= told;
  
  Nneib(ii,:) = distok;
end

% Set diagonals to false
Nneib(sub2ind([nof_Nbase nof_Nbase],1:nof_Nbase,1:nof_Nbase)) = false;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Find the highest agglomeration of nodes

% Loop through neighbouring nodes to find the highest agglomeration

Nnpl = zeros(nof_Nbase,1); % number of planes belonging to the nodes

finish = false;

% as long as there is nodes remaining to be checked
while ismember(true,Nneib)

	for i = 1:nof_Nbase
		inb = Nneib(:,i);           % index of remaining neighbouring nodes

		pls = PLAbase(inb);         % plane indices
		pls = [pls; PLBbase(inb)];  
		pls = unique(pls);          % all planes indices which come close this node
		Npls{i} = pls;              % store planes being close to this node
		
		Nnpl(i)  = length(pls);     % no. of those planes

		lis = LAbase(inb);          % line indices
		lis = [lis; LBbase(inb)];
		lis = unique(lis);          % all line indices which come close this node
		Nlis{i} = lis;              % lines being close to this node

		% if pls includes all plane normals
		if length(pls)==nof_pln
			
			% Rodrigues vector is the fitted intersection of the lines
			Rvec = gtMathsLinesIntersection(R.l(lis,:))';
			
			% If Rvec is in the fund. zone, we found the solution
			if gtMathsIsPointInPolyhedron(Rvec,fzone_acc)
				finish = true;
				break  % for loop
			end
		end
		
	end

	% Solution found with all planes
	if finish
		intsc.lines  = lis;
		intsc.planes = pls';
		intsc.Rvec   = Rvec;
		break % while loop
	end
	
	
	% node index with most planes
	[~,ibn] = max(Nnpl); 

	% Rodrigues vector
	Rvec = gtMathsLinesIntersection(R.l(Nlis{ibn},:))';

	
	% Test if Rvec is in the accurate fund. zone 
	if gtMathsIsPointInPolyhedron(Rvec,fzone_acc)
		intsc.lines  = (Nlis{ibn})';
		intsc.planes = (Npls{ibn})';
		intsc.Rvec   = Rvec;
		break % while loop
	end 

	
	% Test if Rvec is in the extended fund. zone 
	if gtMathsIsPointInPolyhedron(Rvec,fzone_ext)
		intsc.lines  = (Nlis{ibn})';
		intsc.planes = (Npls{ibn})';
		
		% Use its intersections with the accurate fund. zone, instead of Rvec
		Rsecs = gtMathsLinePolyhedronIntersections([0 0 0 Rvec],fzone_acc);
		
		% Distances between the two intersections and Rvec
		Rsecdist1 = norm(Rvec - Rsecs(1,:));
		Rsecdist2 = norm(Rvec - Rsecs(2,:));

		% Use the closer intersection
		if Rsecdist1 < Rsecdist2
			intsc.Rvec = Rsecs(1,:);
		else
			intsc.Rvec = Rsecs(2,:);
		end
			
		break % while loop
	end
	
	
	% The node with the most plane normals doesn't provide a solution in the 
	% extended fund. zone. Ignore best node in the neighbourhood matrix, and start the
	% loop again:
	Nneib(Nneib(:,ibn),:) = false;  
	Nneib(:,Nneib(ibn,:)) = false;
			
	Nneib(ibn,:) = false;
	Nneib(:,ibn) = false;
		
end
	

if plot_flag
  sfPlotInts(intsc,R,fzone_acc)
end

if isempty(intsc.Rvec)
	return
end


Rout = intsc.Rvec;
good_plnorms(intsc.planes) = true;


% Minimum distance from edge of exact fundamental zone:

% Normalise fund. zone face direction vectors
fzonedirnorm = fzone_acc(:,4:6)./repmat(sqrt(sum(fzone_acc(:,4:6).*fzone_acc(:,4:6),2)),1,3);

% Difference vector between Rout and fund. zone face position vectors
diffRout = repmat(Rout,size(fzone_acc,1),1)-fzone_acc(:,1:3);

% Signed distances between Rout point and fund. zone faces
Rdistall = sum(diffRout.*fzonedirnorm,2);

% Distance between Rout and nearest fund. zone face
Rdist = min(abs(Rdistall));

% Distance smaller than tolerance ?
Redge = Rdist <= tol.dist;

% If R vector is slightly out of the fundamental zone boundaries, set it
% back on the nearest boundary plane.
% if Rdistall(minind) > 0
% 	Rout  = Rout - Rdistall(minind)*fzone_acc(minind,4:6);
% 	Rdist = 0;
% end

	
end

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

%% Plotting

function sfPlotInts(intsc,R,fzone_acc)

figure, hold on, axis equal
xlabel('x')
ylabel('y')
zlabel('z')
grid

rangemin = min(fzone_acc(:,1:3));
rangemax = max(fzone_acc(:,1:3));

xlim([rangemin(1) rangemax(1)]);
ylim([rangemin(2) rangemax(2)]);
zlim([rangemin(3) rangemax(3)]);

nof_lines = size(R.l,1);

lc = autumn(nof_lines);
for i = 1:nof_lines
	
	isec = gtMathsLinePolyhedronIntersections(R.l(i,:),fzone_acc);
	
	if ~isempty(isec)
		R.enda(i,:) = isec(1,:);
		R.endb(i,:) = isec(2,:);
	
		if ismember(i,intsc.lines)
			plot3([R.enda(i,1) R.endb(i,1)],[R.enda(i,2) R.endb(i,2)],[R.enda(i,3) R.endb(i,3)],'Color','k') %lc(i,:))
		else
			plot3([R.enda(i,1) R.endb(i,1)],[R.enda(i,2) R.endb(i,2)],[R.enda(i,3) R.endb(i,3)],'Color',lc(i,:))
		end
	end
end

if ~isempty(intsc.Rvec)
	plot3(intsc.Rvec(1),intsc.Rvec(2),intsc.Rvec(3),'bo')
	plot3(intsc.Rvec(1),intsc.Rvec(2),intsc.Rvec(3),'bo','MarkerSize',15)
end


end