function [p1, p2, p3, p4, V] = findtetra (point, space)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% findtetra.m                           %
% written 3/3/00 by Ben Olding          %
% Given a point & list of data (space), %
% finds enclosing tetrahedron with the  %
% smallest volume                       %
% If the point is outside of space,     %
% function returns 4 closest points &   %
% indicates failure with V=-1           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Requires: tetravolume.m, coplanar.m

% Space is [X Y Z C M Y]
% Point is [x y z]

% Any point can be used to divide space into 8 sectors
% Sector 1 = X>x, Y>y, Z>z
% Sector 2 = X>x, Y<y, Z>z
% Sector 3 = X<x, Y<y, Z>z
% Sector 4 = X<x, Y>y, Z>z
% Sector 5 = X>x, Y>y, Z<z
% Sector 6 = X>x, Y<y, Z<z
% Sector 7 = X<x, Y<y, Z<z
% Sector 8 = X<x, Y>y, Z<z

X = space(:,1);
Y = space(:,2);
Z = space(:,3);
x = point(1);
y = point(2);
z = point(3);

% make a 7th column, distance from point
space(:,7) = sqrt((X-x).^2+(Y-y).^2+(Z-z).^2);

% sort from closest to farthest
space = sortrows(space,7);

X = space(:,1);
Y = space(:,2);
Z = space(:,3);

% find closest points in 8 sectors
sector1=space(find(X>x & Y>y & Z>z),:);
sector2=space(find(X>x & Y<y & Z>z),:);
sector3=space(find(X<x & Y<y & Z>z),:);
sector4=space(find(X<x & Y>y & Z>z),:);
sector5=space(find(X>x & Y>y & Z<z),:);
sector6=space(find(X>x & Y<y & Z<z),:);
sector7=space(find(X<x & Y<y & Z<z),:);
sector8=space(find(X<x & Y>y & Z<z),:);

% sort the sectors by the distances & choose top
if ~isempty(sector1),
  cube(1,:)=[sector1(1,:), 1];
else
  cube(1,:)=[repmat(-1,1,7), 1];
end;
if ~isempty(sector2),
  cube(2,:)=[sector2(1,:), 2];
else
  cube(2,:)=[repmat(-1,1,7), 2];
end;
if ~isempty(sector3),
  cube(3,:)=[sector3(1,:), 3];
else
  cube(3,:)=[repmat(-1,1,7), 3];
end;
if ~isempty(sector4),
  cube(4,:)=[sector4(1,:), 4];
else
  cube(4,:)=[repmat(-1,1,7), 4];
end;
if ~isempty(sector5),
  cube(5,:)=[sector5(1,:) 5];
else
  cube(5,:)=[repmat(-1,1,7), 5];
end;
if ~isempty(sector6),
  cube(6,:)=[sector6(1,:) 6];
else
  cube(6,:)=[repmat(-1,1,7), 6];
end;
if ~isempty(sector7),
  cube(7,:)=[sector7(1,:) 7];
else
  cube(7,:)=[repmat(-1,1,7), 7];
end;
if ~isempty(sector8),
  cube(8,:)=[sector8(1,:) 8];
else
  cube(8,:)=[repmat(-1,1,7), 8];
end;

cube=sortrows(cube,7);

% check for internal points
if cube(1,7)>0,
  n=0;
  i=1;
  j=2;
  k=3;
  l=4;
  
  % find volumes of all possible tetrahedrons
  while i<6.
    if ~coplanar([cube(i,8) cube(j,8) cube(k,8) cube(l,8)]),
      n=n+1;
      v(n,:)=[tetravolume(cube(i,1:3), cube(j,1:3), ...
			cube(k,1:3), cube(l,1:3)), ...
	    cube(i,8), cube(j,8), cube(k,8), cube(l,8)];
    end;
    l=l+1;
    if l>8,
      k=k+1;
      if k>7,
	j=j+1;
	if j>6,
	  i=i+1;
	  j=i+1;
	end;
	k=j+1;
      end;
      l=k+1;
    end;
  end;  
  
  % Sort volumes from smallest to largest
  v=sortrows(v,1);
  cube=sortrows(cube,8);
  enclosed=0;
  n=0;
  
  % Find smallest volume that encloses the point
  while ~enclosed,
    n =n+1;
    v1=tetravolume(cube(v(n,2),1:3), cube(v(n,3),1:3),...
		   cube(v(n,4),1:3), point);
    v2=tetravolume(cube(v(n,2),1:3), cube(v(n,3),1:3),...
		   cube(v(n,5),1:3), point);
    v3=tetravolume(cube(v(n,2),1:3), cube(v(n,4),1:3),...
		   cube(v(n,5),1:3), point);
    v4=tetravolume(cube(v(n,3),1:3), cube(v(n,4),1:3),...
		   cube(v(n,5),1:3), point);
    
    % if v1+v2+v3+v4 is within 1/100% of v(n,1), point is enclosed       
    if abs((v1+v2+v3+v4-v(n,1))/v(n,1)) < 0.0001,
      enclosed=1;
      p1 = cube(v(n,2),1:6);
      p2 = cube(v(n,3),1:6);
      p3 = cube(v(n,4),1:6);
      p4 = cube(v(n,5),1:6);
      V  = v(n,1);
    end;
  end;

% check if point is exactly equal to data
elseif cube(1,7)==0,
  p1 = cube(1,:);
  p2 = space(2,:);
  p3 = space(3,:);
  p4 = space(4,:);
  V = 0;

% else point is outside data gamut
else
  p1 = space(1,:);
  p2 = space(2,:);
  p3 = space(3,:);
  p4 = space(4,:);
  V = -1;
end;      



