clear all
close all

% load the data
load fieldData/PTBdata20210428/PontusData.mat
load fieldData/PTBdata20210428/TethysData.mat
load fieldData/PTBdata20210428/BrizoData.mat



% Create the first vehicle
if (~exist('d1','var')||isempty(d1))
    load fieldData/PTBdata20210428/PontusTRN.mat
    
    load fieldData/PTBdata20210428/PontusParticles.mat
    
    % initialize the object with particle data
    d1 = LRAUVDataClass(1,3,particles1,t_trn1);
    kft = [N1';E1';Z1']; % add inertial data
    d1.posKft = kft; 
    d1.tKft = t1;

    % add relative measurements (no truth)
    d1.addRelative(t1,R1mod,[],H1,[],[]);
    
    % generate messages to send to vehicle 3
    d1.generateMessages(3);
end

% create the 2nd vehicle
if (~exist('d2','var')||isempty(d2))
    
    load fieldData/PTBdata20210428/TethysParticles.mat
    load fieldData/PTBdata20210428/TethysTRN.mat
    
    % initialize the object with particle data
    d2 = LRAUVDataClass(2,3,particles2,t_trn2);
    kft = [N2';E2';Z2'];
    d2.posKft = kft;
    d2. tKft = t2;

    % add relative measurements
    d2.addRelative(t2,R2mod,[],H2,[],[]);

    % generate messages to send to vehicle 3
    d2.generateMessages(3);
end

% create the third vehicle
if (~exist('d3','var')||isempty(d3))
    
    load fieldData/PTBdata20210428/BrizoParticles.mat
    load fieldData/PTBdata20210428/BrizoTRN.mat
    
    % initialize the object with particle data
    d3 = LRAUVDataClass(3,3,particles3,t_trn3);
    kft = [N3';E3';Z3'];
    d3.posKft = kft;
    d3. tKft = t3;

    % reinitialize the particles to stay in a grid
    d3.resetParticles(1,length(d3.tTRN));
    
    % no relative measurements for vehicle 3
end

%clear intermediate data
clearvars -except d1 d2 d3

Nrob = 3;
rob = cell(1,Nrob);
rob{1} = d1;
rob{2} = d2;
rob{3} = d3;

% only doing cooperative TRN for vehicle 3
rob{3}.muComm = cell(Nrob,length(rob{3}.tTRN));
rob{3}.SigmaComm = cell(Nrob,length(rob{3}.tTRN));

T = length(rob{3}.tTRN);
messageInd = ones(Nrob-1,1);
r = 3;
for t=1:T % iterate through vehicle 3's time
    rob{r}.MRProcessUpdate(t); % do a process update
    for j=1:Nrob-1 % iterate through aiding vehicles
        if messageInd(j) <= size(rob{j}.messages,2) % if we have a new message
            NextMessage = rob{j}.messages{r,messageInd(j)};
            if rob{r}.tTRN(t) >= NextMessage.time
                rob{r}.MultiRobotUpdate(NextMessage,t); % do the multi-robot update
                messageInd(j) = messageInd(j)+1;
            end
        end
        [mu,sig] = rob{r}.getBestEst(t); % calculate the best estimate
        rob{r}.bestEst(:,t) = mu;
        rob{r}.bestSig{t} = sig;
    end
end

% clear intermediate data
clearvars -except rob Nrob