classdef rrtAgent
    properties
        N
        id
        start
        goal_list
        goal
        path
        path_verts
        path_waypoints
        path_len
        verts
        token
        dx
        collision_radius
        dist_inc
        env
        obs_points
        time_obs
        move_speed
        planning_itr
        planning_itr_base
        vert_num
        show_live_plot
        show_end_plot
        current_goal_index
        replanning_iteration
        replan
        path_failed
        v_level
    end
    methods
        function obj = rrtAgent(agent_number, starting_location, goal_locations, token_ownership, ...
                environment_size, obstacle_points, distance_increment, step_size, ...
                planning_iterations, move_speed, collision_radius, v_level)
            % initializer of agent
            if nargin < 4
                error("missing some information")
            else
                if length(goal_locations) < 1
                    error("no goal information!")
                end
            end
            
            obj.N = agent_number;
            obj.start = struct('parent',[],'coords',starting_location,'index',1,'cost',0);
            obj.planning_itr = planning_iterations;
            obj.planning_itr_base = planning_iterations;
            obj.goal_list = cell(size(goal_locations,1),1);
            obj.current_goal_index = 1;
            obj.replanning_iteration = 1;
            for i=1:size(goal_locations,1)
                obj.goal_list{i} = struct('coords',goal_locations(i,:),'parent',-1,'cost',Inf,'index',obj.planning_itr+i);
                % assume sequential visiting!
            end
            obj.goal = struct('coords',goal_locations(1,:),'parent',-1,'cost',Inf,'index',obj.planning_itr+1);
            obj.path = {}; % vertex indexs to follow for a path
            obj.path_verts = {};
            obj.path_len = 0;
            obj.vert_num = obj.planning_itr;
            obj.verts = cell(obj.planning_itr,1); % all vertex sample information
            obj.verts{1} = struct('parent',[],'coords',obj.start,'index',1,'cost',0); % preloaded w info
            obj.token = token_ownership;
            obj.dx = step_size;
            obj.collision_radius = collision_radius;
            obj.dist_inc = distance_increment;
            obj.env = environment_size; % [xmin xmax ymin ymax]
            obj.obs_points = obstacle_points; % obstacle points (blocked points)
            obj.time_obs = {};
            obj.move_speed = move_speed;
            obj.replan = false;
            obj.path_failed = false;
            obj.v_level;
            
            % temp plotting parameters
            obj.show_live_plot = false;
            obj.show_end_plot = false;
            
        end
        
        function obj = planMultiGoalPath(obj)
            % plan a path that hits all the goals in goal_locations
            obj.goal = obj.goal_list{obj.current_goal_index}; % update goal position
            obj = obj.resetSamples;
            if obj.v_level
                fprintf("Agent %d planning path %d/%d...\n", obj.id,obj.current_goal_index,length(obj.goal_list));
            end
            [collision_flag, ~] = obj.checkCollisions(obj.start, obj.goal, false);
            
            if ~collision_flag
                % if no collisions, in time or space, then path is
                % planned
                % add linear vertices and add to path
                total_dist = norm(obj.start.coords - obj.goal.coords);
                total_steps = floor(total_dist/obj.dist_inc);
                unit_vec = (obj.goal.coords - obj.start.coords)/total_dist;
                for i=2:total_steps+1
                    new_coords = obj.start.coords + (i-1)*obj.dist_inc*unit_vec;
                    obj.verts{i} = struct('parent',i-1,'coords',new_coords,'index',i,'cost',0);
                end
                % update goal with information
                obj.start = obj.goal;
                obj.start.parent = [];
                obj.start.cost = 0;
                obj.start.index = 1;
                
                obj.goal.cost = total_dist;
                obj.goal.parent = total_steps+1;
                
                obj.path = num2cell(total_steps+1:-1:1);
                if obj.v_level
                    fprintf("Was able to print a straight line path!\n");
                end
            else
                obj = obj.planPath;
                while obj.replan
                    obj.planning_itr = obj.planning_itr + 250;
                    obj = obj.resetSamples; 
                    obj = obj.planPath;
                end
                obj.planning_itr = obj.planning_itr_base;
                obj.start = obj.goal;
                obj.start.parent = [];
                obj.start.cost = 0;
                obj.start.index = 1;
            end
            
            if ~isempty(obj.path)
                obj.path_verts = {};
                path_tmp = fliplr(obj.path);
                for i=1:length(obj.path)
                    obj.path_verts{i} = obj.verts{path_tmp{i}}; % append all vertices from path
                end
                obj.path_verts{end+1} = obj.goal;
            else
                return % path didnt work, exit
            end
            
            obj.current_goal_index = obj.current_goal_index + 1;
            if obj.current_goal_index > length(obj.goal_list)+1
                error('iterated too many times in multi-goal path planning');
            end
            
            obj.path_waypoints = zeros(length(obj.path_verts),2);
            for i=1:length(obj.path_verts)
                obj.path_waypoints(i,:) = obj.path_verts{i}.coords;
            end
            D = dist(obj.path_waypoints');
            obj.path_len = obj.path_len + sum(diag(D(1:end-1,2:end)));
        end
        
        function intercept_time = timeIntersect(obj, waypoints)
            % checks the waypoint list provided with the known time
            % obstacles (other agents' waypoint lists)
            
            intercept_time = -1;
            D = dist(waypoints');
            dist_list = diag(D(1:end-1,2:end));
            total_dist = sum(dist_list);
            total_time = total_dist/obj.move_speed;
            t = (obj.dx*obj.move_speed):(obj.dx*obj.move_speed):total_time; % time parameter
            
            % now we need to determine, based on time, what set of waypoints we are
            % passing between
            
            q_time = (obj.dx*obj.move_speed); % start query time
            while q_time <= total_time
                q_point_waypoint = obj.timeLookup(q_time, {waypoints});
                q_point_obstacle = obj.timeLookup(q_time, obj.time_obs);
                if size(q_point_waypoint,1) > 1
                    error('waypoint-based point should be single point!');
                end
                dists = sqrt((q_point_waypoint(:,1) - q_point_obstacle(:,1)).^2 ...
                    + (q_point_waypoint(:,2) - q_point_obstacle(:,2)).^2);
                min_dist = min(dists); % minimum distance from point to obstacle
                if min_dist <= obj.collision_radius
                    intercept_time = q_time; % collision!
                    break;
                else
                    % slightly questionable
                    q_time = q_time + 0.90*(min_dist./(2*obj.move_speed)); % move forward in time based on distance
                    % hopefully this will keep us from overshooting the
                    % actual collision point because we are moving time
                    % forward assuming agents travel in a straight line
                    % toward one another
                end
            end
        end
        
        function q_point = timeLookup(obj, q_time, waypoints)
            % need a way to convert times, and lists of waypoints, into a
            % 2D point(s) in space
            if ~iscell(waypoints)
                error('waypoints must be cell array')
            end
            
            if ~all(cellfun(@isempty,waypoints))
                q_point = zeros(length(waypoints),2);
                for j=1:length(waypoints)
                    wpts_tmp = waypoints{j};
                    if isempty(wpts_tmp)
                        continue; % skip empty waypoints
                    end
                    D = dist(wpts_tmp');
                    dist_list = diag(D(1:end-1,2:end));
                    q_dist = q_time*obj.move_speed;
                    dist_list_sum = [0;cumsum(dist_list)];
                    wp_index = -1;
                    for i=1:length(dist_list_sum)
                        if q_dist <= dist_list_sum(i)
                            wp_index = i;
                            break;
                        end
                    end
                    
                    if wp_index ~= -1
                        unit_vec = wpts_tmp(wp_index,:) - wpts_tmp(wp_index - 1,:);
                        unit_vec = unit_vec/norm(unit_vec);
                        q_point(j,:) = wpts_tmp(wp_index - 1,:) + unit_vec*(q_dist - dist_list_sum(wp_index - 1));
                    else
                        q_point(j,:) = wpts_tmp(end,:); % assume that the agent stops at end
                    end
                end
            else
                q_point = zeros(0,2);
            end
            
        end
        
        function obj = resetSamples(obj)
            % resets all path planning variables so that we can replan from
            % a new location without accidentally reusing samples
            obj.verts = cell(obj.planning_itr,1); % reset position samples
            obj.vert_num = obj.planning_itr;
            obj.path = {};
            obj.verts{1} = obj.start; % preloaded w info
            
        end
        
        function obj = planPath(obj)
            
            if obj.show_live_plot
                figure(1); clf; hold on;
                plot(obj.obs_points(:,1), obj.obs_points(:,2), '.k');
                axis(obj.env);
            end
            if obj.show_end_plot
                figure(2); clf; hold on;
                plot(obj.obs_points(:,1), obj.obs_points(:,2), '.k');
                axis(obj.env);
            end
            
            for k=2:obj.planning_itr
                % generate random point
                xdim = obj.env(2) - obj.env(1);
                ydim = obj.env(4) - obj.env(3);
                pt_rand = [rand*xdim + obj.env(1), rand*ydim + obj.env(3)];
                vert_near = getNearestVertex(obj, pt_rand, -1); % vert_near nearest vertex
                vert_new = getNewConfig(obj, vert_near, pt_rand, k); % pick from vert near and generate new config w min cost
                % check if valid
                if isempty(vert_new)
                    continue;
                end
                obj.verts{k} = vert_new; % update vertex list w new config
                obj = obj.rewireNeighbors(vert_new);
                
                % examine goal positions
                goal_dist = norm(obj.goal.coords - vert_new.coords);
                [collision_flag, ~] = obj.checkCollisions(vert_new, obj.goal, true);
                if goal_dist < obj.dist_inc && ~collision_flag
                    % can touch goal location
                    if (goal_dist + vert_new.cost) < obj.goal.cost
                        obj.goal.cost = goal_dist + vert_new.cost;
                        obj.goal.parent = vert_new.index;
                    end
                end
                
                % progress updates:
                if mod(k,100) == 0
                    if obj.v_level
                        fprintf("iteration: %d\n", k);
                    end
                end
                
                % plot things
                if obj.show_live_plot
                    figure(1);
                    xs = [vert_new.coords(1), obj.verts{vert_new.parent}.coords(1)];
                    ys = [vert_new.coords(2), obj.verts{vert_new.parent}.coords(2)];
                    plot(xs, ys, 'b.--');
                    axis(obj.env);
                    drawnow;
                end
                
            end
            
            if obj.goal.parent == -1
                if obj.v_level
                    fprintf("failed to make it to goal position, re-planning iteration %d\n",...
                    obj.replanning_iteration);
                end
                obj.replanning_iteration = obj.replanning_iteration + 1;
                obj.replan = true;
                if obj.replanning_iteration > 3
                    obj.replan = false;
                    obj.path_failed = true;
                    if obj.v_level
                        fprintf("iterations exceeded, bailing\n");
                    end
%                     error("failed to make it to goal, quitting...");
                end
                return
            else
                obj.replan = false;
                obj.replanning_iteration = 1;
                if obj.show_end_plot
                    figure(2);
                    plot(obj.goal.coords(1,1), obj.goal.coords(1,2), 'r*');
                    plot(obj.start(1,1), obj.start(1,2), 'g*');
                    plot_vert = obj.goal.parent;
                    plot([obj.goal.coords(1) obj.verts{plot_vert}.coords(1)], [obj.goal.coords(2) obj.verts{plot_vert}.coords(2)], 'b.--');
                    while true
                        if obj.v_level
                            fprintf("next vertex to plot: %d\n", plot_vert);
                        end
                        if plot_vert == 1
                            xs = obj.verts{1}.coords(1);
                            ys = obj.verts{1}.coords(2);
                        else
                            xs = [obj.verts{plot_vert}.coords(1), obj.verts{obj.verts{plot_vert}.parent}.coords(1)];
                            ys = [obj.verts{plot_vert}.coords(2), obj.verts{obj.verts{plot_vert}.parent}.coords(2)];
                        end
                        plot(xs, ys, 'b.--');
                        if plot_vert == 1
                            break;
                        end
                        plot_vert = obj.verts{plot_vert}.parent;
                    end
                    axis(obj.env);
                end
                
                next_vert = obj.goal.parent;
                c = 1;
                while true
                    obj.path{c} = next_vert;
                    if next_vert == 1
                        break;
                    end
                    next_vert = obj.verts{next_vert}.parent;
                    c = c + 1;
                end
            end
        end
        
        function ancestors = lineageLookup(obj, vert_start)
            % look up all parents of starting vertex and compose into a
            % list
            ancestors = {};
            ancestors{end+1} = vert_start; % start with starting vertex of course
            next_vert = vert_start.parent;
            
            while true
                if isempty(next_vert)
                    break;
                else
                    ancestors{end+1} = obj.verts{next_vert};
                    next_vert = obj.verts{next_vert}.parent; % continue iterating
                end
            end
        end
        
        function collision_flag = checkObstaclesOnly(obj, pts_sample, pt_goal)
            collision_flag = false;
            for i=1:size(pts_sample,1)
                pt = pts_sample(i,:);
                total_dist = norm(pt - pt_goal);
                for j=1:ceil(total_dist./obj.dx)
                    vec = (pt_goal - pt)./norm(pt_goal - pt);
                    if ~isempty(obj.obs_points)
                        dists = sqrt((obj.obs_points(:,1) - pt(1)).^2 + (obj.obs_points(:,2) - pt(2)).^2);
                    else
                        dists = [];
                    end
                    if any(dists <= obj.collision_radius)
                        collision_flag = true;
                    end
                    pt = pt + obj.dx.*vec; % walk towards sample center
                end
            end
        end
        
        function [collision_flag, stop_point] = checkCollisions(obj, vert_start, vert_end, stop_short)
            if isstruct(vert_start)
                pt_start = vert_start.coords;
            else
                pt_start = vert_start;
            end
            if isstruct(vert_end)
                pt_end = vert_end.coords;
            else
                pt_end = vert_end;
            end
            total_dist = norm(pt_end - pt_start);
            if stop_short
                step_num = ceil(min(obj.dist_inc,total_dist)./obj.dx);
            else
                step_num = ceil(total_dist./obj.dx);
            end
            vec = ((pt_end - pt_start)./total_dist).*obj.dx;
            pt = pt_start;
            collision_flag = false;
            if ~isempty(obj.obs_points)
                for i=1:step_num
                    dists = sqrt((pt(1) - obj.obs_points(:,1)).^2 + (pt(2) - obj.obs_points(:,2)).^2);
                    if any(dists <= obj.collision_radius)
                        collision_flag = true;
                        break;
                    end
                    pt = pt + vec;
                end
                stop_point = pt;
            else
                stop_point = pt_end;
            end
%             %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%             % build waypoint list so we can check possible time collisions
%             if isstruct(vert_start)
%                 vert_list = obj.lineageLookup(vert_start);
%                 waypoints = zeros(length(vert_list)+1,2);
%                 waypoints(1,:) = stop_point;
%                 for i=1:length(vert_list)
%                     waypoints(i+1,:) = vert_list{i}.coords;
%                 end
%                 waypoints = flipud(waypoints);
%                 
%             else
%                 error('vertex starting position should be a struct!');
%             end
%             intercept_time = obj.timeIntersect(waypoints);
%             if intercept_time ~= -1
%                 collision_flag = true;
%             end
%             %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            
        end
        
        function vert_near = getNearestVertex(obj, pt_test, search_radius)
            % find nearest set of points within search radius, if search
            % radius is -1, we only want the nearest point
            dmin = Inf;
            if search_radius == -1
                for i=1:obj.vert_num
                    if isempty(obj.verts{i})
                        continue;
                    end
                    tmp_dist = norm(pt_test - obj.verts{i}.coords);
                    if tmp_dist < dmin
                        dmin = tmp_dist;
                        vert_near = obj.verts{i};
                    end
                end
            else
                vert_near = {};
                c = 1;
                for i=1:obj.vert_num
                    if isempty(obj.verts{i})
                        continue;
                    end
                    tmp_dist = norm(pt_test - obj.verts{i}.coords);
                    if tmp_dist < search_radius
                        vert_near{c} = obj.verts{i};
                        c = c + 1; % inc counter
                    end
                end
            end
        end
        
        function new_config = getNewConfig(obj, vert_near, pt_rand, iteration)
            % compare near neighbors and pick cheapest one to be parent
            if length(vert_near) > 1
                error("list is too long!")
            end
            [~, stop_point] = obj.checkCollisions(vert_near, pt_rand, true); % not looking for collision, but new point
            new_cost = vert_near.cost + norm(vert_near.coords - stop_point);
            new_config = struct('parent',vert_near.index,'coords',stop_point,'index',iteration,'cost',new_cost);
            
            % now look through close vertices to see if they would be
            % cheaper parents
            new_parent = vert_near.index;
            cmin = new_config.cost;
            verts_near = obj.getNearestVertex(new_config.coords, obj.dist_inc); % this time its a list
            for i=1:length(verts_near)
                [collision_flag, ~] = obj.checkCollisions(verts_near{i}, new_config, true); % only looking for collision
                if ~collision_flag
                    tmp_cost = verts_near{i}.cost + norm(verts_near{i}.coords - new_config.coords);
                    if tmp_cost < cmin
                        cmin = tmp_cost;
                        new_parent = verts_near{i}.index; % save new cost and index of new parent
                    end
                end
            end
            % update with new information
            new_config.parent = new_parent;
            new_config.cost = cmin;
            
        end
        
        function obj = rewireNeighbors(obj, new_config)
            % rewire nearby vertices with new configuration as parent
            verts_near = obj.getNearestVertex(new_config.coords, obj.dist_inc); % this time its a list
            for i=1:length(verts_near)
                [collision_flag, ~] = obj.checkCollisions(verts_near{i}, new_config, true);
                if ~collision_flag
                    new_cost = new_config.cost + norm(new_config.coords - verts_near{i}.coords); % updated cost to get to vert
                    if new_cost < verts_near{i}.cost
                        % rewire!
                        index_rewire = verts_near{i}.index;
                        obj.verts{index_rewire}.parent = new_config.index;
                        obj.verts{index_rewire}.cost = new_cost;
                    end
                end
            end
        end
    end
end


