
close all

% load the map
if (~exist('map','var')||isempty(map))
    load PortugueseLedgeMap.mat
end
% clear any extra variables
clearvars -except map

%%%%%% SIMULATION TUNING PARAMETERS %%%%%%
% define the region of interest on the map
mapBounds = [4061311, 4062146, 594160, 594995];

% set the type of inter-robot measurements
measType = 4; %1 = rel pos, 2 = range, 3 = bearing, 4 = range & bearing
% set the dimensions of the initial particle grid
pos_width = 100;
% set the number of robots
Nrob = 3;
% set the standard deviation for relative position measurements
relPosStdDev = 2;
% set the standard deviation for range measurements
rangeStdDev = 4; % percent of range
% set the standard deviation for bearing measurements
bearingStdDev = deg2rad(7.5);

% set the threshold when a vehicle is considered converged and able to aid
% others (m)
convergedThresh = 10;

% which robot do we care about most and want to focus on when plotting?
plotRob = 3;

% set the initial positions of the vehicles [N1 N2 N3...; E1 E2 E3...; D1 D2 D3...]
init_pos = [4061950, 4061600,4061800;...
    594450,594600,594700;
    0, 0, 0];

% set the goal positions of the vehicles
goal = [4061700,4061550,4062030;...
    594300,594900,594850;...
    0,0,0];

% set the max amount of time to run the simulation
tMax = 80; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% define the color scheme
colors = [68,119,170;102,204,238;34,136,51;220, 197, 51;238,102,119;170,51,119;187,187,187]/256;

% which colors are assigned to each robot
colorOrder = [6;2;4];
f1 = figure();

% show the map
map.plot(mapBounds);
f1.Position = [100,1,1000,800];
hold on

% initialize the variables
rob = cell(1,Nrob);
posHist = cell(1,Nrob);
h = [];
h2 = [];


% rng(2);

% iterate through and initialize each vehicle
for r=1:Nrob
    
    % get the initial position
    N = init_pos(1,r);
    E = init_pos(2,r);
    D = map.getDepth(N,E) + 40; % override the depth (we're not estimating it)
    goal(3,r) = D;
    init_pos(3,r) = D;

    % set the initial particle grid dimensions
    Erange = rand(1,1);
    Nrange = rand(1,1);
    Drange = rand(1,1);
    pos_D = 5;
    bounds1 = [N-pos_width*Nrange, N+pos_width*(1-Nrange);...
        E-pos_width*Erange, E+pos_width*(1-Erange);...
        D-Drange*pos_D, D+(1-Drange)*pos_D];

    % initialize the AUV class
    rob{r} = AUVSimulatorClass(r,[N;E;D],bounds1,Nrob);
    
    % plot the vehicle
    figure(f1);
    h = rob{r}.plot(h,colors(colorOrder(r),:),plotRob);
    
    % store the position for later
    posHist{r} = rob{r}.pos;
end

drawnow

% gif('sim_results.gif','frame',f1);
dist = zeros(1,Nrob);

% step through time
for t=1:tMax
    % delete the plot data from the last timestep
    delete(h)
    h = [];

    % iterate through the vehicles, moving them towards the goal
    for r=1:Nrob
        posOld = rob{r}.pos;
        rob{r}.sim(map,goal(:,r), [0;0;0],[]);
        % track how far it travelled
        dist(1,r) = dist(1,r) + norm(posOld - rob{r}.pos);
        posHist{r} = [posHist{r}, rob{r}.pos];
    end
    
    % build the measurements between vehicles, store it in data
    data = cell(Nrob,1);
    for r=1:Nrob
        % r is vehicle receiving the measurement
        % take robot-to-robot measurements
        ind = 1;
        robData = [];
        for j=1:Nrob % j is the vehicle providing the measurement
            if r~=j && all(diag(rob{j}.Sigma)<convergedThresh) % if j has converged enough
                d = rob{r}.dim;

                % id of the vehicle providing the measurement
                robData(ind).id = rob{j}.id;
                % get the mmse position
                robData(ind).mu = rob{j}.mu(1:d);
                % calculate the best fit Gaussian
                rob{j}.updateSigma();
                robData(ind).Sigma = rob{j}.Sigma(1:d,1:d);
                robData(ind).measType = measType;
                
                if measType == 1 % relative position measurement
                    robData(ind).meas = rob{r}.pos(1:d)-rob{j}.pos(1:d) + relPosStdDev*randn(d,1);
                    robData(ind).R = relPosStdDev^2*eye(d);
                elseif measType == 2 % range-only
                    range = norm(rob{r}.pos(1:d)-rob{j}.pos(1:d));
                    robData(ind).meas = range + rangeStdDev*0.01*range*randn(1,1);
                    robData(ind).R = (rangeStdDev)^2;
                elseif measType == 3 % bearing-only
                    vec = rob{r}.pos - rob{j}.pos;
                    robData(ind).meas = atan2(vec(2),vec(1)) + bearingStdDev*randn(1,1);
                    robData(ind).R = bearingStdDev^2;
                elseif measType == 4 % range bearing elev
                    range = norm(rob{r}.pos-rob{j}.pos);
                    rangeMeas = range + rangeStdDev*0.01*range*randn(1,1);
                    vec = rob{r}.pos - rob{j}.pos;
                    bearingMeas = atan2(vec(2),vec(1)) + bearingStdDev*randn(1,1);
                    if d > 2 % 3D estimation
                        elevMeas = atan2(vec(3),sqrt(vec(1)^2+vec(2)^2)) + angleStdDev*randn(1,1);
                        robData(ind).meas = [rangeMeas;bearingMeas;elevMeas];
                        robData(ind).R = diag([rangeStdDev^2,bearingStdDev^2,bearingStdDev^2]);
                    else % 2D estimation
                        robData(ind).meas = [rangeMeas;bearingMeas];
                        robData(ind).R = diag([rangeStdDev^2,bearingStdDev^2]);
                    end
                end
                ind = ind + 1;
            end
        end
        data{r} = robData;
    end
    
    
    % Now do the multi-robot TRN
    for r=1:Nrob
        rob{r}.MultiRobotUpdate(data{r});
        
        % plot the vehicles
        h = rob{r}.plot(h,colors(colorOrder(r),:),plotRob);
    end
    
    drawnow
%     gif
%     pause(0.1)

end