function [utility, computation_time, path_length] = multi_agent_path_planning(input_pj, m, v_level, input_N)

N = input_N;
load_params;

Pj = input_pj;
% obstacle stuff
[x_pts, y_pts] = generateObstacles(environment_size, threshold, obs_step_size, starting_locations, goal_locations, safe_radius);
obstacle_points = [x_pts, y_pts];

% load('obstacle_saved_easy','obstacle_points');
% obstacle_points(obstacle_points(:,1) >= 5,:) = [];
% obstacle_points(:,1) = obstacle_points(:,1) - 1.5;
% obstacle_points(:,2) = obstacle_points(:,2) - 2;

dummy = rrtAgent(N, mean(starting_locations,1), mean(goal_locations,1), token_ownership, ...
    environment_size, obstacle_points, distance_increment, step_size, ...
    planning_iterations, move_speed, collision_radius, v_level);

t_start = tic;

dummy = dummy.planMultiGoalPath;
if isempty(dummy.path_waypoints)
    utility = -1;
    computation_time = -1;
    path_length = -1;
    tmp = toc(t_start);
    return
end

% figure(1); clf; hold on;
% plot(obstacle_points(:,1), obstacle_points(:,2), 'k.');
% plot(starting_locations(:,1), starting_locations(:,2), 'bo');
% plot(goal_locations(:,1), goal_locations(:,2), 'bo');

t_int = (dummy.path_len)./(dummy.move_speed*(Tk+1));
if t_int < 2*sig
    
    if v_level
        fprintf('not enough room to generate constrained bounding sets\n');
    end
    
    utility = -1;
    computation_time = -1;
    path_length = -1;
    tmp = toc(t_start);
    return
end
sample_centers = zeros(Tk,2); % center of sampling bounding areas
sample_slopes = zeros(Tk,1);
for k=1:(Tk)
    sample_centers(k,:) = dummy.timeLookup(k*t_int, {dummy.path_waypoints});
    tmp_pt = dummy.timeLookup(k*t_int - 0.01, {dummy.path_waypoints});
    sample_slopes(k) = (tmp_pt(2) - sample_centers(k,2))/(tmp_pt(1) - sample_centers(k,1));
end

plcs = cell(Tk,Pj);
sample_sets = cell(Tk,1);
sample_sets_const = cell(Tk,1);

% bounding sets
w = t_int;
h = 1.5; %%%%%%%%%%%%%%%%%%%%%%%%%temp
a = atan(w/h);
r = (w/2)/sin(a);

for k=1:Tk
    th = atan(sample_slopes(k)) + [pi/2 - a, pi/2 + a, (3*pi/2)- a, (3*pi/2) + a];
    [c_pt_x,c_pt_y] = pol2cart(th,r);
    sample_sets{k} = [sample_centers(k,1) + c_pt_x', sample_centers(k,2) + c_pt_y'];
    sample_sets{k} = [sample_sets{k}; sample_sets{k}(1,:)];
    
    [c_pt_x_tmp,c_pt_y_tmp] = pol2cart(th,r - rho/2); % bound away from sides of set
    bdd_set_tmp = [sample_centers(k,1) + c_pt_x_tmp', sample_centers(k,2) + c_pt_y_tmp'];
    sample_sets_const{k} = getConstBoundSet(Z,x_area,y_area,bdd_set_tmp,rho, obstacle_points); % bounding set for tight sampling case
    
    % generate samples for this time slice
    x_len = max(sample_sets{k}(:,1)) - min(sample_sets{k}(:,1));
    y_len = max(sample_sets{k}(:,2)) - min(sample_sets{k}(:,2));
    x_min = min(sample_sets{k}(:,1));
    y_min = min(sample_sets{k}(:,2));

    counter = 1;
    sample_itr = 0;
    while counter <= Pj && sample_itr < max_sample_itr
        tmp_pt = [x_min + x_len*rand(N,1), y_min + y_len*rand(N,1)];
        collision_flag = dummy.checkObstaclesOnly(tmp_pt, sample_centers(k,:));
        if all(inpolygon(tmp_pt(:,1), tmp_pt(:,2), sample_sets{k}(:,1), sample_sets{k}(:,2))) && ~collision_flag
            plcs{k,counter} = tmp_pt;
            counter = counter + 1;
        end
        sample_itr = sample_itr + 1;
    end
    if isempty(plcs{k,end})
        if v_level
            fprintf('ran out of sample iterations, bailing\n')
        end
        utility = -1;
        computation_time = -1;
        path_length = -1;
        tmp = toc(t_start);
        return
    end
    const_empty_flag = false(N,1);
    for j=1:Pj
        const_empty_flag = or(inpolygon(plcs{k,j}(:,1), plcs{k,j}(:,2), ...
            sample_sets_const{k}(:,1), sample_sets_const{k}(:,2)), const_empty_flag);
    end
    if ~all(const_empty_flag)
        if v_level
            fprintf('insufficient sampling, constrained sets empty, bailing\n')
        end
        utility = -1;
        computation_time = -1;
        path_length = -1;
        tmp = toc(t_start);
        return
    end
end

% select the samples based on coverage environmental heatmap
[plcs_sel, gdy_interaction_times] = selectSamples(sample_sets_const, plcs, rho, sig, N, env, K);

% plan paths for all agents
agents = cell(N,1);
agent_paths = cell(N,1);
agent_pts = [];

for i=1:N
    agent_goals = [plcs_sel{i};goal_locations(i,:)];
    agents{i} = rrtAgent(N, starting_locations(i,:), agent_goals, token_ownership, ...
        environment_size, obstacle_points, distance_increment, step_size, ...
        planning_iterations, move_speed, collision_radius);
    agents{i}.id = i;
end

for k=1:Tk+1
    time_obstacles = cell(N,1);
    for i=1:N
        agents{i}.time_obs = time_obstacles;
        agents{i} = agents{i}.planMultiGoalPath;
        if agents{i}.path_failed
            utility = -1;
            computation_time = -1;
            path_length = -1;
            tmp = toc(t_start);
            return
        end
        time_obstacles{i} = agents{i}.path_waypoints;
        agent_paths{i} = [agent_paths{i};agents{i}.path_waypoints];
        agent_pts = [agent_pts;agents{i}.path_waypoints];
    end
end

% evaluate performance:
% verts in Tj*N
utility = pointTrajectoryUtility(agent_pts, env, sig);
computation_time = toc(t_start);
path_length = 0;
for i=1:N
    path_length = path_length + agents{i}.path_len;
end

%% plotting stuff

if show_plot
    figure(1); clf; hold on; grid on;
    set(gcf, 'Position',  [200, 200, 600, 600])
    colormap parula;
    my_grey = [0.7 0.7 0.7];
    if show_plot_env
    imagesc(environment_size(1):0.1:environment_size(2),...
        environment_size(3):0.1:environment_size(4),Z);
    end
    colors = hsv(N);
    plot(dummy.obs_points(:,1), dummy.obs_points(:,2), 'k.', 'markersize', ms+8, 'linewidth', lw);
    for a=1:N
        sl_handle = plot(starting_locations(a,1), starting_locations(a,2), 's', 'markersize', ms, 'linewidth', lw, 'color', colors(a,:));
        gl_handle = plot(goal_locations(a,1), goal_locations(a,2), 'd', 'markersize', ms, 'linewidth', lw, 'color', colors(a,:));
    end
    core_handle = plot(dummy.path_waypoints(:,1), dummy.path_waypoints(:,2), 'm.-', 'markersize', ms, 'linewidth', lw);
    for k=1:Tk
        for j=1:Pj
            plot(plcs{k,j}(:,1), plcs{k,j}(:,2), '.', 'markersize', ms, 'linewidth', lw, ...
                'markerfacecolor',my_grey, 'markeredgecolor',my_grey);
        end
        for i=1:N
            plot(plcs_sel{i}(k,1), plcs_sel{i}(k,2), '.', 'markersize', ms+8, 'linewidth', lw, ...
                'markerfacecolor',colors(i,:), 'markeredgecolor',colors(i,:));
        end
        bdd_box_handle = plot(sample_sets{k}(:,1), sample_sets{k}(:,2), 'k--', 'markersize', ms, 'linewidth', lw);
        cnst_box_handle = plot(sample_sets_const{k}(:,1), sample_sets_const{k}(:,2), 'k:', 'markersize', ms, 'linewidth', lw);
    end
    
    for i=1:N
        t = 0.1:0.02:(agents{i}.path_len/agents{i}.move_speed);
        for j=1:length(t)
            pt = agents{i}.timeLookup(t(j), {agent_paths{i}});
            plot(pt(1), pt(2), '.', 'markersize', ms, 'linewidth', lw, ...
                'markerfacecolor',colors(i,:), 'markeredgecolor',colors(i,:));
        end
    end
    legend([core_handle,bdd_box_handle,cnst_box_handle,sl_handle,gl_handle],'Core path','$B_k$','$\overline{B_k}$',...
        'Start','Goal','Location','best','Interpreter','Latex');
    axis image
    axis(environment_size)
    set(gca,'FontSize',fs)

end

end
