% TriangulationEx1.m
%
% Triangulation example for pts = (x1,x2) values and y.
% Create triangles using Delaunay triangulation.
% Determine which triangle input value lies in using decision boundaries.
% Find plane that passes through y values at vertices of triangles.

% First an example of finding center point of circle passing thru 3 pts.
pts_array = [1,4; 10,1; 5,-4];
[center_pt, center_pt_flag] = TriangulationCenterPt(pts_array);

% Triangulation example.
pts = ...
  [-1,  2; ...
    3,  2; ...
    0,  3; ...
    0,  6; ...
    4, 0; ...
    1, -2; ...
   -4, -4; ...
   -4, 3];

% y values corresponding to pts (x1,x2), above.
y = ...
  [0; 1; 2; 3; 4; 5; 6; 7];
  
[triangles, triangle_indices] = TriangulationDelaunay(pts);

% Plot triangles.
figure(1)
hold off

% Plot x1 and x2 axes.
plot([-8,8],[0,0],'k-')
hold on
plot([0,0],[-8,8],'k-')

for triangle_index = 1:size(triangles,3)
  plot([triangles(:,1,triangle_index);triangles(1,1,triangle_index)], ...
       [triangles(:,2,triangle_index);triangles(1,2,triangle_index)], ...
       'r')
end

set(gcf,'color','white')
xlabel('x1')
ylabel('x2')
title('Delaunay triangulation')

% Create decision boundaries for triangles.
db = TriangulationDecBndry(triangles);

% Set probe pts.
x_vec = [2,2; -2,-2; -2,3];

% Walk thru probe pts to calculate estimated y values.
for x_probe_index = 1 : size(x_vec,1)
  % Extract x_probe.
  x_probe = x_vec(x_probe_index,:);

  % Test every triangle to see which one contains x_probe.
  for triangle_index = 1:size(db,3)
    inside_triangle = 1;
    triangle_index = triangle_index;
    % Determine which side of decision boundaries x_probe is on.
    for db_index = 1:size(db,1)
      % Extract decision boundary vec.
      this_db = db(db_index,:,triangle_index);

      % Calculate dot product of [1, probe_pt] and decision bndry vec. 
      decision = this_db * [1,x_probe].';

      % Check whether x_probe is outside triangle.
      if decision < 0
        % x_probe is outside triangle, so move on to next triangle.
        inside_triangle = 0;
        triangle_index = triangle_index;
        break
      end
    end
    % display('Done with db_index loop')

    % If all decisions are positive, x_probe is inside triangle.
    if inside_triangle == 1
      inside_triangle = inside_triangle;
      % Calculate plane passing through the y values.
      x_matrix = [ones(size(triangles,1),1),triangles(:,:,triangle_index)];
      y_vec = y(triangle_indices(triangle_index,:));
      plane_def = (inv(x_matrix) * y_vec).';

      display('x_probe and estimated y value:')
      x_probe = x_probe
      y_est = plane_def * [1, x_probe].'
      plane_def = plane_def
      triangle = triangles(:,:,triangle_index)
      triangle_index = triangle_index;
      x_probe_index = x_probe_index;

      % Finished with this x_probe.
      break
    end
  end
end