% group monte carlo runner
clear all; close all; clc;

addpath('../HS');

M = 10;
A = 42; % should be divisable by N
m = 1;
N = 3;
% assume upper bound of 10 fails per iteration
j = 10*M*(A/N);
v_level = 0; % quiet mode

xdata = [floor(linspace(200,1200,M))', linspace(0.15,0.1,M)'];
% xdata = [600*ones(M,1), 0.1*ones(M,1)];
Ndata = N*ones(M,1);
if length(Ndata) ~= M || length(xdata) ~= M
    error('check ya self son');
end
seedsLAB = zeros(M,A);
utilLAB = zeros(M,2*A); % [[IIP_1 IIP_2 ... IIP_A], [HS_1 HS_2 ... HS_A]]
comp_timeLAB = zeros(M,2*A);
path_lengthsLAB = zeros(M,2*A);

seeds = zeros(M,A);
util = zeros(M,2*A);
comp_time = zeros(M,2*A);
path_lengths = zeros(M,2*A);

% parpool(N);

start_time = tic;

spmd

    n = labindex;
    offset = n*j;

    m = 1;
    while m <= M

        fprintf("\nworker %d beginning iteration %d with offset %d --------- \n",n,m,offset);
        input_pj = xdata(m,1);
        input_env_step_size = xdata(m,2);
        input_N = Ndata(m);

        a = (n-1)*(A/N) + 1;
        a_stop = n*(A/N);

        while a <= a_stop
            fprintf("\nworker %d averaging iteration %d, offset %d --------- \n",n,a,offset);
            rng(offset);
            [utilLAB(m,a), comp_timeLAB(m,a), path_lengthsLAB(m,a)] = multi_agent_path_planning(input_pj, m, v_level, input_N);
            if utilLAB(m,a) == -1 || comp_timeLAB(m,a) == -1
                fprintf("\niteration %d, run %d, has failed \n",m,a);
                offset = offset + 1;
                continue;
            end
            
            rng(offset);
            [utilLAB(m,A+a), comp_timeLAB(m,A+a), path_lengthsLAB(m,A+a)] = main(input_env_step_size, m, v_level, input_N);
            if utilLAB(m,A+a) == -1 || comp_timeLAB(m,A+a) == -1
                fprintf("\niteration %d, run %d, has failed \n",m,a);
                offset = offset + 1;
                continue;
            end
            
            seedsLAB(m,a) = offset;
            a = a + 1;
            offset = offset + 1;
        end
        m = m + 1;
        
    end

end

stop_time = toc(start_time);

% re-assemble matrices
for n=1:N
    a = (n-1)*(A/N) + 1;
    a_stop = n*(A/N);
    
    tmp = seedsLAB{n};
    seeds(:,a:a_stop) = tmp(:,a:a_stop);
    tmp = utilLAB{n};
    util(:,a:a_stop) = tmp(:,a:a_stop);
    util(:,(a:a_stop)+A) = tmp(:,(a:a_stop)+A);
    tmp = comp_timeLAB{n};
    comp_time(:,a:a_stop) = tmp(:,a:a_stop);
    comp_time(:,(a:a_stop)+A) = tmp(:,(a:a_stop)+A);
    tmp = path_lengthsLAB{n};
    path_lengths(:,a:a_stop) = tmp(:,a:a_stop);
    path_lengths(:,(a:a_stop)+A) = tmp(:,(a:a_stop)+A);
end

time_str = datestr(now,31);
time_str = regexprep(time_str(6:end-3),':','-');
time_str = regexprep(time_str,' ','_');
filename = ['averaged_M',num2str(M),'_A',num2str(A),'_',time_str,'.mat'];
save(filename,'util','comp_time','xdata','Ndata','seeds','A','M','path_lengths','stop_time');

offset
m
stop_time

