clear all
close all

% load the data
load fieldData/PTdata20201206/PontusData.mat
load fieldData/PTdata20201206/TethysData.mat

% Create the first vehicle
if (~exist('d1','var')||isempty(d1))
    load fieldData/PTdata20201206/PontusTRN.mat
    
    load fieldData/PTdata20201206/PontusParticles.mat
    % find the "truth" (portions where TRN has converged
    converged = [450:950,1200:1600];
    Ncorr = mean(navN1(converged)-mmseN1(converged));
    Ecorr = mean(navE1(converged)-mmseE1(converged));
    Zcorr = mean(navZ1(converged)-mmseZ1(converged));
    
    % initialize the object with particle data
    d1 = LRAUVDataClass(1,2,particles1,t_trn1);
    kft = [N1';E1';Z1'];
    d1.posKft = kft;
    d1.tKft = t1;
    
    % add truth data
    d1.truth = [navN1'-Ncorr;navE1'-Ecorr;navZ1'-Zcorr];
    
    % add relative measurements
    %     d1.addRelative(t1,R1,R1truth,H1,H1truth,100);
    inds = find(abs(R1 - R1truth)>100); % filter out bad measurements
    R1(inds) = 0;
    d1.addRelative(t1,R1,[],H1,[],[]);
    d1.heading = d1.heading + 0.2618; % correct for heading offset
    
    %     d1.resetParticles(1,length(d1.tTRN));
    % generate messages to send to vehicle 2
    d1.generateMessages(2);
end

% create the 2nd vehicle
if (~exist('d2','var')||isempty(d2))
    
    load fieldData/PTdata20201206/TethysParticles.mat
    load fieldData/PTdata20201206/TethysTRN.mat
    
    % initialize the object with particle data
    d2 = LRAUVDataClass(2,2,particles2,t_trn2);
    kft = [N2';E2';Z2'];
    d2.posKft = kft;
    d2. tKft = t2;
    
    % find the truth data
    converged = 400:1300;
    Ncorr = mean(navN2(converged)-mmseN2(converged));
    Ecorr = mean(navE2(converged)-mmseE2(converged));
    Zcorr = mean(navZ2(converged)-mmseZ2(converged));
    d2.truth = [navN2'-Ncorr;navE2'-Ecorr;navZ2'-Zcorr];
    
    %     d2.addRelative(t2,R2,R2truth,H2,H2truth,100);
    % add relative measurements
    inds = find(abs(R2 - R2truth)>100);
    R2(inds) = 0;
    d2.addRelative(t2,R2,[],H2,[],[]);
    d2.heading = d2.heading - 0.2618;
    
    % reset the particles to stay spread out
    d2.resetParticles(1,length(d2.tTRN));

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


clearvars -except d1 d2

% do multi-robot TRN
Nrob = 2;
rob = cell(1,Nrob);
rob{1} = d1;
rob{2} = d2;

% initialize cells
rob{1}.muComm = cell(Nrob,length(rob{1}.tTRN));
rob{1}.SigmaComm = cell(Nrob,length(rob{1}.tTRN));
rob{2}.muComm = cell(Nrob,length(rob{2}.tTRN));
rob{2}.SigmaComm = cell(Nrob,length(rob{2}.tTRN));

for i=1:Nrob % for each robot
    T = length(rob{i}.tTRN);
    messageInd = 1;
    if i==1
        otherRob = 2;
    else
        otherRob = 1;
    end
    
    for t=1:T % iterate through time
        rob{i}.MRProcessUpdate(t); % do process update
        if messageInd <= size(rob{otherRob}.messages,2)
            % see if there's a new measurement available
            NextMessage = rob{otherRob}.messages{i,messageInd};
            if rob{i}.tTRN(t) >= NextMessage.time
                % do multi-robot update
                rob{i}.MultiRobotUpdate(NextMessage,t);
                messageInd = messageInd+1;
            end
        end
        % calculate best estimate
        [mu,sig] = rob{i}.getBestEst(t);
        rob{i}.bestEst(:,t) = mu;
        rob{i}.bestSig{t} = sig;
        
    end
end

% clear intermediate variables
clearvars -except rob Nrob