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 = 2; % incremental distance
init_conf = [10,1]; % initial configuration [x, y]
final_conf = [14,18]; % final configuration [x, y]
dx = 0.1;
star_radius = dist_inc; % 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 = 1000; % start with 1000 vertices in graph
G = zeros(K);
verts = zeros(K,4); % coords of all points and theyre costs and parents [x coord, y coord, cost to go, parent]
verts(1,:) = [init_conf, 0, 1];
% 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(1:2), q_rand, dist_inc, obs_points, dx);
    if isempty(q_new)
        continue;
    end
    % got a new configuration, update cost to go and parent information
    near_index = find(all(verts == q_near,2));
    q_new = [q_new, q_near(3) + norm(q_new-q_near(1:2)), near_index];
    
    cmin = q_new(3); % cost to get from nearest point to new point
    ind_min = -1;
    nearest_vertices = getNearestVertex(q_new, verts, star_radius);
    for i=1:size(nearest_vertices,1)
        % itrerate through nearest neighbors to look for easier method
        [collision_flag, end_point] = checkCollisions(q_new(1:2), nearest_vertices(i,1:2), dx, obs_points);
        new_cost = (nearest_vertices(i,3)+norm(nearest_vertices(i,1:2) - q_new(1:2)));
        if ~collision_flag && new_cost < cmin
            cmin = new_cost; % new lowest cost source vertex
            ind_min = i; % index of new lowest cost source vertex
        end
    end
    if ind_min ~= -1
        new_parent = find(all(verts == nearest_vertices(ind_min,:),2));
        if new_parent ~= 2 % never add final config as a parent node, dont make no sense
            q_new(3) = cmin; % update cost and parent
            q_new(4) = new_parent;
        end
    end
    
    rewired_list = zeros(size(nearest_vertices,1),1); % list of vertices that get rewired to use q_new
    for i=1:size(nearest_vertices,1)
        
        % look for lower cost if q_new was parent
        new_cost = q_new(3) + norm(q_new(1:2) - nearest_vertices(i,1:2));
        % check collisions
        [collision_flag, ~] = checkCollisions(q_new(1:2), nearest_vertices(i,1:2), dx, obs_points);
        if new_cost < nearest_vertices(i,3) && ~collision_flag
            % we need to update the parent of this vert
            rewired_vert = find(all(verts == nearest_vertices(i,:),2));
            verts(rewired_vert,4) = k;
            verts(rewired_vert,3) = new_cost; % update cost to get there too
            rewired_list(i) = rewired_vert;
        end
        
    end
    
    verts(k,:) = q_new;
    G(near_index, k) = 1;
    G = min(G + G',1);
    if norm(final_conf - q_new(1:2)) <= dist_inc
%         fprintf("final configuration has been reached\n")
        verts(2,:) = [final_conf, norm(final_conf - q_new(1:2)), k];
        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([verts(q_new(4),1), q_new(1)], [verts(q_new(4),2), q_new(2)], 'g-')
        % also plot line to original nearest vertex
        plot([q_near(1), q_new(1)], [q_near(2), q_new(2)], 'r:')
        % also also plot rewired neighbors (somehow)
        rewired_list = rewired_list(rewired_list ~= 0); % clear out zeros
        for i=1:length(rewired_list)
            plot([verts(rewired_list(i),1), q_new(1)], [verts(rewired_list(i),2), q_new(2)], 'b:')
            plot(verts(rewired_list(i),1), verts(rewired_list(i),2), 'b*')
        end
        axis(env);
        drawnow;
    end
    
%     if any(verts(:,4) == 2)
%         fprintf("F\n")
%     end
    
end

%% plotting
% generate shortest path
% gr = graph(G);
% vert_path = shortestpath(gr,1,2);
if show_end_plot
    figure(1); clf; hold on;
    plot_vert = 2;
    while true
        % follow final config backward to starting config
        fprintf("next vert: %d\n", plot_vert);
        plot(verts(plot_vert,1), verts(plot_vert,2), 'mo');
        xs = [verts(plot_vert,1), verts(verts(plot_vert,4),1)];
        ys = [verts(plot_vert,2), verts(verts(plot_vert,4),2)];
        plot(xs, ys, 'm-');
        if plot_vert == 1
            break;
        end
        plot_vert = verts(plot_vert,4); % go back to parent
    end
    plot(x_obs, y_obs, '.k')
    plot(final_conf(1), final_conf(2), '*r')
    plot(init_conf(1), init_conf(2), '*g')
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