
function simulationRobot()

clf;
clear all

hold on
axis equal
axis([-5 65 -5 65])
Te = .1

rand('seed', 6);


Start = [0 30];
StartX = Start(1);
StartY = Start(2);
plot([StartX], [StartY], 'x');

End = [60 30];
EndX = End(1);
EndY = End(2);
plot([EndX], [EndY], 'x');

Polygons = [
  25, 25, 35, 35, 25, 25 ;
  25, 35, 35, 25, 25, 25 ;
  
  44, 50, 50, 42, 39, 44 ;
  27, 30, 37, 39, 34, 27 ;
];
PolyN = length(Polygons(:, 1)) / 2 - 1;


% Initialize grid
X = [];
Y = [];
for i = 0 : 10 : 60,
    for j = 0 : 10 : 60,
        X = [ X  i ];
        Y = [ Y  j ];
        
        X = [ X rand(1, 1) * 60 ];
        Y = [ Y rand(1, 1) * 60 ];
    end
end
%X = [0 30 60 60 60 30  0 0];
%Y = [0  0  0 30 60 60 60 30];
%X = [(StartX - 1) (StartX + 1) (EndX - 1) (EndX + 1)];
%Y = [ StartY       StartY       EndY       EndY     ];
%X = [];
%Y = [];


for i = 0:PolyN,
   PolyX = Polygons(1 + 2 * i,:);
   PolyY = Polygons(1 + 2 * i + 1,:);
   fill(PolyX, PolyY, 'k');
   
   X = [X PolyX];
   Y = [Y PolyY];
end

[VX,VY] = VORONOI(X, Y);

% Remove segments that are intersecting polygons

function [inter] = intersect(vx, vy)
   inter = 0;
   for k = 0:PolyN,
      PolyX = Polygons(1 + 2 * k, :);
      PolyY = Polygons(1 + 2 * k + 1, :);

      [interX, interY] = polyxpoly(vx, vy, PolyX, PolyY);
      if (length(interX) > 0)
          inter = 1;
          break
      end
   end
end

FilteredVX = [];
FilteredVY = [];

 for j = 1:length(VX),
   vx = VX(:, j)'; 
   vy = VY(:, j)'; 
   
   if intersect(vx, vy) == 0
      FilteredVX = [FilteredVX vx'];
      FilteredVY = [FilteredVY vy'];
   end
 end
 VX = FilteredVX;
 VY = FilteredVY;
 
 plot(VX, VY, '-');
% plot(X, Y, '.');


IDCount = 0;
IDTable = java.util.Hashtable;
IDPoints = [];

function [ret] = id(point)
    key = mat2str(point);
    if IDTable.containsKey(key)
       ret = IDTable.get(key);
    else
       IDCount = IDCount + 1;
       IDTable.put(key, IDCount);
       ret = IDCount;
       IDPoints = [IDPoints point'];
       %text(point(1), point(2), mat2str(IDCount));
    end
end
% Find the shortest path

PathA = [];
PathB = [];
PathW = [];

function addSegment(A, B)
 if id(A) ~= id(B)
     PathA = [PathA id(A)];
     PathB = [PathB id(B)];
     PathW = [PathW pdist2(A, B)];
 end
end

 for i = 1:length(VX),
     A = [VX(1, i) VY(1, i)];
     B = [VX(2, i) VY(2, i)];
     addSegment(A, B);
 end
 
 
 
% Find the nearest point

function addPoint(point)
 pointX = point(1);
 pointY = point(2);
 min_d = Inf;
 P = [0 0];
 A = [0 0];
 B = [0 0];

 for j = 1:length(VX),   
   Q1X = VX(1, j); Q1Y = VY(1, j);
   Q2X = VX(2, j); Q2Y = VY(2, j);
   
   if Q1X ~= Q2X
       m = (Q2Y - Q1Y) / (Q2X - Q1X);
       b = Q1Y - m * Q1X;

       x = (m * pointY + pointX - m * b) / (m * m + 1);
       y = (m * m * pointY + pointX * m + b) / (m * m + 1);
   else % Vertical
       x = Q1X;
       y = pointY;
   end
   
   if ~((x >= min(Q1X, Q2X) && x <= max(Q1X, Q2X)) && (y >= min(Q1Y, Q2Y) && y <= max(Q1Y, Q2Y)))
      continue
   end
   
   d = pdist2(point, [x y]);
   if (d < min_d)
       min_d = d;
       P = [x y];
       A = [Q1X Q1Y];
       B = [Q2X Q2Y];
   end
 end

% plot([point(1) P(1)], [point(2) P(2)], '-', 'LineWidth', 2);
 addSegment(point, P);
 addSegment(P, A);
 addSegment(P, B);
end
 
 addPoint(Start);
 addPoint(End);
 
 DG = sparse(PathA, PathB, PathW, IDCount, IDCount);
 UG = tril(DG + DG');
 [dist, path, pred] = graphshortestpath(UG, id(Start), id(End), 'directed', false);

%  h = view(biograph(UG,[],'ShowArrows','off','ShowWeights','on'));
%  set(h.Nodes(path),'Color',[1 0.4 0.4]);
%  fowEdges = getedgesbynodeid(h,get(h.Nodes(path),'ID'));
%  revEdges = getedgesbynodeid(h,get(h.Nodes(fliplr(path)),'ID'));
%  edges = [fowEdges;revEdges];
%  set(edges,'LineColor',[1 0 0]);
%  set(edges,'LineWidth',1.5);

 
% Path compression
 last_point = IDPoints(:, path(1))';
 FilteredPath = [last_point'];
 for i = 2:length(path),
  curr_point = IDPoints(:, path(i))';
  if intersect([last_point(1) curr_point(1)], [last_point(2) curr_point(2)]) == 1
    last_point = IDPoints(:, path(i - 1))';
    FilteredPath = [FilteredPath last_point'];
  end
 end

 FilteredPath = [FilteredPath curr_point'];
 plot(FilteredPath(1, :), FilteredPath(2, :), 'r--', 'LineWidth', 2);

 Path = FilteredPath;

Xc_last = StartX;
Yc_last = StartY;
fai_last = 0;

robot_width = 1.5;

[Robot] = Robotplot(Xc_last, Yc_last, fai_last, robot_width);

h1 = plot([Robot(1,1) Robot(1,3)], [Robot(1,2) Robot(1,4)], 'b-', 'LineWidth', 4);
h2 = plot([Robot(1,3) Robot(1,5)], [Robot(1,4) Robot(1,6)], 'r-', 'LineWidth', 1);
h3 = plot([Robot(1,5) Robot(1,7)], [Robot(1,6) Robot(1,8)], 'b-', 'LineWidth', 4);
h4 = plot([Robot(1,7) Robot(1,1)], [Robot(1,8) Robot(1,2)], 'r-', 'LineWidth', 1);

set(h1,'EraseMode','xor');
set(h2,'EraseMode','xor');
set(h3,'EraseMode','xor');
set(h4,'EraseMode','xor');

target_id = 2;
target = Path(:, target_id)';
plot([target(1)], [target(2)], '.', 'LineWidth', 1);

while 1==1, 

    % Check target angle
    uw = unwrap([fai_last, atan2(target(2) - Yc_last, target(1) - Xc_last)]);
    diff = uw(1) - uw(2);
    if abs(diff) < 0.01
      omega = 0;
      v = 1;
    else
      v = 0.2;
      if diff > 0
          omega = -0.1570796;
      else
          omega = 0.1570796;
      end
    end

    % Check target distance
    if pdist([Xc_last - target(1) ; Yc_last - target(2)]) < 0.1
        target_id = target_id + 1;
        if target_id > length(Path)
            break
        end
        target = Path(:, target_id)';
        plot([target(1)], [target(2)], '.');
    end



    % Update robot position
    Xc = Xc_last + v * cos(fai_last) * Te;
    Yc = Yc_last + v * sin(fai_last) * Te;
    fai = fai_last + omega * Te;

    Xc_last = Xc;    
    Yc_last = Yc;
    fai_last = fai;

    [Robot] = Robotplot(Xc, Yc, fai, robot_width);

    set(h1,'XData', [Robot(1,1) Robot(1,3)], 'YData', [Robot(1,2) Robot(1,4)]);
    set(h2,'XData', [Robot(1,3) Robot(1,5)], 'YData', [Robot(1,4) Robot(1,6)]);
    set(h3,'XData', [Robot(1,5) Robot(1,7)], 'YData', [Robot(1,6) Robot(1,8)]);
    set(h4,'XData', [Robot(1,7) Robot(1,1)], 'YData', [Robot(1,8) Robot(1,2)]);

    pause(0.001);

    
end

end