clear all;
clc;


threshold = 0.45;
safe_radius = 1;
% step_size = 0.1;
env_step_size = 0.1;

starting_locations = [1,2;1 2.5;1 3];
goal_locations = [9.5,2;9.5 2.5;9.5 3];
environment_size = [0 10 0 5];

% [x_pts, y_pts] = generateObstacles(environment_size, threshold, env_step_size, starting_locations, goal_locations, safe_radius);
center_points = [7 2;
                 3.75 3.75];
obstacle_radius = 1.0;

% [X,Y] = meshgrid((environment_size(1)-obstacle_radius):env_step_size:(obstacle_radius+environment_size(2)), ...
%     (environment_size(3)-obstacle_radius):env_step_size:(environment_size(4)+obstacle_radius));
[X,Y] = meshgrid((environment_size(1)):env_step_size:(environment_size(2)), ...
    (environment_size(3)):env_step_size:(environment_size(4)));
x_pts = X(:);
y_pts = Y(:);
obstacle_points = zeros(length(x_pts),2);

for i=1:size(center_points,1)
    init_d = sqrt((center_points(i,1) - x_pts).^2 + (center_points(i,2) - y_pts).^2);
    x_pts_tmp = x_pts(init_d <= obstacle_radius);
    y_pts_tmp = y_pts(init_d <= obstacle_radius);
    last_idx = nnz(obstacle_points(:,1));
    obstacle_points(last_idx+1:(last_idx+length(x_pts_tmp)),:) = [x_pts_tmp, y_pts_tmp];
end

nonzeros_x_obstacle = nonzeros(obstacle_points(:,1));
nonzeros_y_obstacle = nonzeros(obstacle_points(:,2));
obstacle_points = [nonzeros_x_obstacle, nonzeros_y_obstacle];


% [X,Y] = meshgrid(4:env_step_size:6,0:env_step_size:5);
% obstacle_points = [X(:),Y(:)];

% for i=1:size(starting_locations,1)
%     init_d = sqrt((starting_locations(i,1) - x_pts).^2 + (starting_locations(i,2) - y_pts).^2);
%     x_pts = x_pts(init_d >= safe_radius);
%     y_pts = y_pts(init_d >= safe_radius);
% end
% for i=1:size(goal_locations,1)
%     final_d = sqrt((goal_locations(i,1) - x_pts).^2 + (goal_locations(i,2) - y_pts).^2);
%     x_pts = x_pts(final_d >= safe_radius);
%     y_pts = y_pts(final_d >= safe_radius);
% end

ms = 8;
lw = 2;
fs = 14;
figure(1); clf; hold on;
plot(obstacle_points(:,1), obstacle_points(:,2), 'k.', 'markersize', ms, 'linewidth', lw);

axis image
axis(environment_size)
set(gca,'FontSize',fs)

% save('obstacle_saved_easy.mat','obstacle_points');