clear all;
% close all;
clc;

% basic RRT program with obstacles, will integrate with sampling-based intermittent
% interaction code later

show_live_plot = false;
show_end_plot = true;
env = [0, 20, 0, 20]; % environment [xmin, xmax, ymin, ymax]
xdim = env(2) - env(1);
ydim = env(4) - env(3);
dist_inc = 0.5; % incremental distance
init_conf = [10,1]; % initial configuration [x, y]
final_conf = [14,18]; % final configuration [x, y]
dx = 0.1;
star_radius = 5; % radius to look for rewire points
threshold = 0.35;
safe_radius = 2;

[x_obs, y_obs] = generateObstacles(env, threshold, dx, init_conf, final_conf, safe_radius);

obs_points = [x_obs, y_obs];
xlist = dx:dx:xdim;
ylist = dx:dx:ydim;
if show_live_plot
    figure(1); clf; hold on;
    plot(x_obs, y_obs, '.k')
end

% now for the RRT stuff
K = 2000; % start with 1000 vertices in graph
G = zeros(K);
verts = zeros(K,2); % coords of all points
verts(1,:) = init_conf;
% verts(2,:) = final_conf;

for k=3:K
    
    % generate random config
    q_rand = [rand*xdim + env(1), rand*ydim + env(3)];
    
    % locate nearest vertex
    q_near = getNearestVertex(q_rand, verts, -1); % just want one point
    q_new = getNewConfig(q_near, q_rand, dist_inc, obs_points, dx);
    if isempty(q_new)
        continue;
    end
%     cmin = norm(q_new - q_near); % cost to get from nearest point to new point
    
    % we have a working new configuation, add it and an edge
    % check for new neighbors too
    near_index = find(all(verts == q_near,2));
    verts(k,:) = q_new;
    G(near_index, k) = 1;
    G = min(G + G',1);
    if norm(final_conf - q_new) <= dist_inc
        fprintf("final configuration has been reached\n")
        verts(2,:) = final_conf;
        G(2,k) = 1;
        G(k,2) = 1;
        break;
    end
    
    
    % progress updates
    if mod(k,100) == 0
        fprintf("iteration %d\n", k);
    end
    if show_live_plot
        plot(q_new(1), q_new(2), '.b')
        plot([q_near(1), q_new(1)], [q_near(2), q_new(2)], 'g-')
        drawnow;
    end
    
end

% generate shortest path
gr = graph(G);
vert_path = shortestpath(gr,1,2);
if show_end_plot
    figure(1); clf; hold on;
    plot(x_obs, y_obs, '.k')
    plot(verts(vert_path,1), verts(vert_path,2), 'mo-')
end

% dual tree stuff:

%     dists = sqrt((q_new(1) - verts(:,1)).^2 + (q_new(2) - verts(:,2)).^2);
%     new_neighbors = find(dists <= dist_inc); % adding neighbors like this is kinda of bullshit? we need to check for bloackages
%     new_neighbors = new_neighbors(new_neighbors ~= near_index);

%     G(near_index, new_neighbors) = 1;

%         for i=1:length(new_neighbors)
%             plot([verts(new_neighbors(i),1), q_new(1)], [verts(new_neighbors(i),2), q_new(2)], 'g-')
%         end