function [utility, computation_time, path_length] = main(input_env_step_size, m, v_level, input_N)

% Nomenclature:
% 	• K: number of robots
% 	• G(N,E): environment graph, nodes are convex regions and edges are connections between regions
% 	• G'(N',E'): time-unfolded version of G(N,E), a node in N' is a time-stamped copy of a node in N.
%   That is, n' in N' corresponds to (n,t) for node n in N and time t.
% 	• T: total discrete time steps (the planning horizon)
% 	• k: robot k in {1,…,K}
% 	• P_k = {r_k(0),…,r_k(T)}: path planned by robot k, where each r_k(t) \in N'
% 	• C(P_k): the cost of path P_k, assume all moves have cost one, so C(P_k) = T for horizon T
% 	• P = P_1 U P_2 U … U P_K:
%   union of paths for all robots, represents which nodes are visited at which time
% 	• F(P): team objective function, assume that F is known
% 	• G_c(N, E_c): graph describing connectivity between nodes in G (node i in N and j in N are connected if (i,j) in E_c)
% 	• P(t): a configuration of robots at time t using path P
% 	• T_I: the disconnected interval, during which the network may choose to lose connectivity
% 	• tau: the smallest integer such that tau*T_I > T

addpath('../IIP');

N = input_N;
load_params;

env_step_size = input_env_step_size;
% 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];


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);

t_start = tic;

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

[G, Gc] = generateEnvGraph(environment_size, rho, move_size, obstacle_points, env_step_size);

starting_verts = zeros(N,1);
for i=1:N
    dists = (G.nodes(:,1) - starting_locations(i,1)).^2 + ...
        (G.nodes(:,2) - starting_locations(i,2)).^2;
    [~,near_node] = min(dists);
    starting_verts(i) = near_node;
end

node_list = zeros(ceil(max_dist/env_step_size)*tau,1);
last_node_list = zeros(size(node_list,1),N);
time_index = zeros(ceil(max_dist/env_step_size)*tau,1);
V = cell(N,1);
% set up starting locations
for i=1:N
    V{i} = table(node_list, time_index); % create table within each cell
    V{i}{1,:} = [starting_verts(i), 1];
end
c_row = 2*ones(N,1);
connected_configs = zeros(floor(T/T_I),N);
t = 1;

while t <= floor(T/T_I)
    if v_level
        fprintf("horizon %d of %d ... \n",t,floor(T/T_I));
    end
    iter = 1;
    while ~converged_flag
        if v_level
            fprintf("iteration %d\n",iter);
        end
        for i = randperm(N)
            
            % reset path for robot k
            V{i}{c_row(i):end,:} = 0;
            % enumerate some feasible paths
            [~,path] = dijkstra(G,getVerts(V,i,'end'),max_dist);
            % remove those paths disconnected at T_I
            path_trimmed = cell(size(path));
            for j=1:length(path)
                if ~isnan(path{j})
                    % if its a real path, check connectivity
                    verts = union(path{j}(end), getVerts(V,setdiff(1:N,i),'end'));
                    connected_flag = checkConnected(Gc,verts);
                    % remove those disconnected paths
                    if connected_flag
                        path_trimmed{j} = path{j};
                    end
                end
            end
            
            path_trimmed = path_trimmed(~cellfun('isempty',path_trimmed));
            max_util = 0;
            max_idx = -1;
            for j=1:length(path_trimmed)
                [existing_verts, ~] = getVerts(V,1:N,'all');
                
                verts = [existing_verts; path_trimmed{j}']; 
                util = trajectoryUtility(verts, G, env, sig);
                % find maximum utility
                if util > max_util
                    max_util = util;
                    max_idx = j;
                end
            end

            if max_idx == -1
                if v_level
                    fprintf("shite\n");
                end
                utility = -1;
                tmp = toc(t_start);
                computation_time = -1;
                path_length = -1;
                return
            end
            best_path = path_trimmed{max_idx};
            
            % update the path taken for robot i
            next_idx = find(V{i}.node_list,1,'last');
            time_idx = V{i}.time_index(next_idx);
            V{i}.node_list(next_idx:(next_idx+length(best_path)-1)) = best_path;
            V{i}.time_index(next_idx:(next_idx+length(best_path)-1)) = ...
                time_idx:(time_idx+length(best_path)-1);
            
        end
        
        iter = iter + 1;
        converged_flag = true;
        for i=1:N % check all path's convergence
            if ~all(V{i}.node_list == last_node_list(:,i))
                % if not all are equal, then we're not converged
                converged_flag = false;
            end
            last_node_list(:,i) = V{i}.node_list; % update shift register
        end
        
    end
    
    % update rows in V and reset convergence flag and track connected
    % configs
    for i=1:N
        c_row(i) = find(V{i}.node_list,1,'last')+1;
        connected_configs(t,i) = getVerts(V,i,'end');
    end
    
    converged_flag = false;
    
    pos = G.nodes(connected_configs(t,:),1:2);
    goals = goal_locations;
    if all(sqrt((pos(:,1) - goals(:,1)).^2 + (pos(:,2) - goals(:,2)).^2) < rho)
        goals_reached = true;
    end
    
    t = t + 1;
    
end

verts = getVerts(V,1:N,'all');
utility = trajectoryUtility(verts, G, env, sig);
computation_time = toc(t_start);
path_length = 0;
for i=1:N
    D = dist(G.nodes(getVerts(V,i,'all'),1:2)');
    path_length = path_length + sum(diag(D(:,2:end)));
end

%% plotting

if show_plot
    % plot again after each agent re-plans
    figure(2); clf; hold on;
    colormap parula;
%     imagesc(environment_size(1):0.1:environment_size(2),...
%         environment_size(3):0.1:environment_size(4),Z);
    colors = hsv(N);
    for n=1:N
        plot(G.nodes(nonzeros(V{n}.node_list),1), G.nodes(nonzeros(V{n}.node_list),2),...
            '.', 'color', colors(n,:), 'markersize', 20,'linestyle','-');
        for j=1:length(V{n}.node_list)
            if V{n}.node_list(j) ~= 0
                text(G.nodes(nonzeros(V{n}.node_list(j)),1), G.nodes(nonzeros(V{n}.node_list(j)),2), num2str(j))
            end
        end
        plot(G.nodes(V{n}.node_list(1),1), G.nodes(V{n}.node_list(1),2),...
            'd','color',colors(n,:),'markersize',12,'linewidth', 2);
        plot(G.nodes(nonzeros(connected_configs(:,n)),1), ...
            G.nodes(nonzeros(connected_configs(:,n)),2), ...
            'd','color',colors(n,:),'markersize',12,'linewidth', 2);
        plot(goal_locations(n,1), goal_locations(n,2), '^',...
            'color',colors(n,:),'markersize',12,'linewidth',2)
    end
    %     plot(G.nodes(:,1), G.nodes(:,2), 'k.','markersize',10);
    plot(obstacle_points(:,1), obstacle_points(:,2), 'r.','markersize',16,'linewidth',3,'markerfacecolor','r');
    axis(environment_size);
end

