function interpolated = addfoodpoints(waypoints,times)
% Given a set of waypoints and the number of iterations, this function uses
% linear interpolation to add the number of waypoints on the segments that
% are linear. Please be noted if the planned path is not linear, this
% function should not be used. 

  interpolated = waypoints;
  for i = 1:1:times
      newlyallocated = [];
      count = 1;
      interpolatedsize = size(interpolated);
       for j = 1:1:interpolatedsize(2) -1
           sizeval = size(newlyallocated);
           sizeindex = sizeval(2);
           
           newlyallocated(:,sizeindex+1) = interpolated(:,j);
           newlyallocated(:,sizeindex+2) = (interpolated(:,j) + interpolated(:,j+1))/2;
           if j == interpolatedsize(2) -1
               newlyallocated(:,sizeindex+3) = interpolated(:,j+1);
           end
       end
       
      
      interpolated = newlyallocated;
  end
end