% big-ass parameter file
% INTERMITENT INTERACTION PLANNING
% sample selection information
Tk = 3;
K = 1; 
Pj = 200;
rho = 0.5;
sig = 0.25;
environment_size = [0 5 0 5];

% environmental heatmap info
mu = [environment_size(1) + rand(3,1)*environment_size(2), ...
      environment_size(3) + rand(3,1)*environment_size(4)];
gmmMeans = size(mu,1);
sigma = 3*ones(1,2,gmmMeans);
sigma(:,:,1) = [5 5];
sigma(:,:,2) = [5 5];

% multi-agent path planning information
collision_radius = 0.1;
env_step_size = 0.2; % (upper bound)
es = environment_size;
sb = [es(1)+0.1, es(1)+0.9, es(3)+0.1, es(4)-0.1];
gb = [es(2)-0.9, es(2)-0.1, es(3)+0.1, es(4)-0.1];

connected_flag = false;
collision_flag = false;
while collision_flag || ~connected_flag
    starting_locations = [sb(1) + (sb(2) - sb(1))*rand(N,1), sb(3) + (sb(4) - sb(3))*rand(N,1)];
    collision_flag = any(nonzeros(dist(starting_locations')) <= collision_radius,'all');
    adj = dist(starting_locations') <= rho - (env_step_size)/2;
    adj = adj - diag(diag(adj));
    [~,C] = conncompAdj(adj);    
    if all(C == 1)
        connected_flag = true;
    else
        connected_flag = false;
    end
end

connected_flag = false;
collision_flag = false;
while collision_flag || ~connected_flag
    goal_locations = [gb(1) + (gb(2) - gb(1))*rand(N,1), gb(3) + (gb(4) - gb(3))*rand(N,1)];
    collision_flag = any(nonzeros(dist(goal_locations')) <= collision_radius,'all');
    adj = dist(goal_locations') <= rho - (env_step_size)/2;
    adj = adj - diag(diag(adj));
    [~,C] = conncompAdj(adj);
    if all(C == 1)
        connected_flag = true;
    else
        connected_flag = false;
    end
end

% %%%%%%%%%%%%%%%%%%%%%%%% temp
% starting_locations = [0.5 3;
%                       0.15 3.4;
%                       0.5 3.8];
% goal_locations = [4.5 1;
%                   4.85 1.4;
%                   4.5 1.8];
% %%%%%%%%%%%%%%%%%%%%%%%% temp
                  
token_ownership = true;
threshold = 0.4;
safe_radius = 0.6;
step_size = 0.1;
move_speed = 1;
planning_iterations = 300;
max_sample_itr = 10^6;
distance_increment = 1;

% build environmental heatmap
[gmPDF, Z, gmObj, x_area, y_area] = buildGMM(environment_size, mu, sigma);
env.Z = Z;
env.x_area = x_area;
env.y_area = y_area;
env.gmObj = gmObj;

% plotting parameters
lw = 1.5;
fs = 16;
ms = 10;
show_plot = true;
show_plot_env = false;

% Hollinger parameters
obs_step_size = 0.1;
move_size = distance_increment; 
T_I = 2; % interval of disconnection
max_dist = T_I*move_speed;
T = (Tk+1)*T_I; % total horizon length
tau = T/T_I;
converged_flag = false;
goals_reached = false;
