close all

if (~exist('map','var')||isempty(map))
    load('PortugueseLedgeMap.mat');
end

if (~exist('rob','var')||isempty(rob))
    load('fieldData/3VehicleProcessed.mat');
end


%%

% CODE TO GENERATE COVARIANCE PLOT
f = figure;
colors = [68,119,170;102,204,238;34,136,51;220, 197, 51;238,102,119;170,51,119;187,187,187]/256;
h =[];
plotRob = 3;
for j = 1:2
    subplot(2,1,j)
    
    hold on
    h(end+1) = plot(rob{plotRob}.tTRN - rob{plotRob}.tTRN(1),zeros(size(rob{plotRob}.tTRN)),'k','LineWidth',1);
    h(end+1) = plot(rob{plotRob}.tTRN-rob{plotRob}.tTRN(1),3*sqrt([cellfun(@(v)v(j,j),rob{plotRob}.Sigma)]),'Color',colors(3,:),'LineWidth',2);
    h(end+1) = plot(rob{plotRob}.tTRN-rob{plotRob}.tTRN(1),-3*sqrt([cellfun(@(v)v(j,j),rob{plotRob}.Sigma)]),'Color',colors(3,:),'LineWidth',2);

    h(end+1) = plot(rob{plotRob}.tTRN-rob{plotRob}.tTRN(1),3*sqrt([cellfun(@(v)v(j,j),rob{plotRob}.bestSig)]),'Color',colors(5,:),'LineWidth',2);
    h(end+1) = plot(rob{plotRob}.tTRN-rob{plotRob}.tTRN(1),-3*sqrt([cellfun(@(v)v(j,j),rob{plotRob}.bestSig)]),'Color',colors(5,:),'LineWidth',2);

end

ylabel('E Error [m]','FontWeight','bold','FontSize',16);
xlabel('Elapsed Time [s]','FontWeight','bold','FontSize',16);
a = gca;
a.FontSize = 16;
a.FontWeight = 'bold';
subplot(2,1,1)
ylabel('N Error [m]','FontWeight','bold','FontSize',16);
a = gca;
a.FontSize = 16;
a.FontWeight = 'bold';
legend([h(2),h(5)],'Ego-Only','Cooperative TRN','Location','NorthEast','Orientation','horizontal')
f.Position = [100,100,1000,1000];

indStart = 122;
meanCov(1) = mean(3*sqrt([cellfun(@(v)v(1,1),rob{plotRob}.bestSig(indStart:end))]));
meanCov(2) = mean(3*sqrt([cellfun(@(v)v(2,2),rob{plotRob}.bestSig(indStart:end))]));

figure
plot(rob{plotRob}.posKft(2,:),rob{plotRob}.posKft(1,:),'Color',colors(3,:),'LineWidth',2);
hold on
plot(rob{plotRob}.bestEst(2,:),rob{plotRob}.bestEst(1,:),'Color',colors(5,:),'LineWidth',2);
legend('Ego-Only','Cooperative TRN','Location','SouthEast')
axis equal



%%

% CODE TO GENERATE MISSION ANIMATION
colors = [68,119,170;102,204,238;34,136,51;220, 197, 51;238,102,119;170,51,119;187,187,187]/256;
f=figure();
mapBounds = [4061311, 4062146, 594160, 594995];
map.plot(mapBounds);

f.Position = [1000,200,1000,800];
hold on
h1 = [];
h2 = [];
h3 = [];
% if saving as a gif:
% gif('ThreeVehicleSharkLater.gif','frame',f,'LoopCount',0,'DelayTime',0.02);


tTot = [rob{1}.tTRN;rob{2}.tTRN;rob{3}.tTRN];
tTot = sort(tTot);

% Adjust iStart and IEnd to plot different time bounds
% iStart = find(tTot > rob{2}.tTRN(420),1);
% iEnd = find(tTot > rob{2}.tTRN(900),1);
% iStart = 1482;
iStart = 1776;
iEnd = 2135;
% iEnd = 665;
msgInd1 = find(rob{1}.tRange > tTot(iStart),1);
msgInd2 = find(rob{2}.tRange > tTot(iStart),1);
m1 = [];
m2 = [];
for i = iStart:iEnd
    t = tTot(i);
    h4 = text(4062100,594224,-100,['Time = ',num2str(round(t-rob{3}.tTRN(1)),'%d'),' sec'],'FontWeight','bold','FontSize',16,'Color','w');
    i3 = find(rob{3}.tTRN == tTot(i));
    if ~isempty(i3)
        delete(h3);
        h3 = rob{3}.plot(i3,h3,colors(4,:),3);
    end
    
    i1 = find(rob{1}.tTRN == tTot(i));
    if ~isempty(i1)
        delete(h1);
        h1 = rob{1}.plot(i1,h1,colors(6,:),3);
        
        if (rob{1}.messages{3,msgInd1}.time < t)
            msgInd1 = msgInd1 + 1;
            if (all(diag(rob{1}.messages{3,msgInd1}.Sigma(1:2,1:2)) < 15))
                
                range = rob{1}.messages{3,msgInd1}.rangeMeas;
                heading = rob{1}.messages{3,msgInd1}.headingMeas;
                posProj = rob{1}.mu(1:2,i1)+[range*cos(heading);...
                    range*sin(heading)];
                m1 = plot3([rob{1}.mu(1,i1), posProj(1)],[rob{1}.mu(2,i1), posProj(2)],[-100,-100],'Color',colors(5,:),'LineWidth',2);
            end
        end
    end
    
    i2 = find(rob{2}.tTRN == tTot(i));
    if ~isempty(i2)
        delete(h2);
        h2 = rob{2}.plot(i2,h2,colors(2,:),3);
        
        if (rob{2}.messages{3,msgInd2}.time < t)
            msgInd2 = msgInd2 + 1;
            if (all(diag(rob{2}.messages{3,msgInd2}.Sigma(1:2,1:2)) < 15))
                
                range = rob{2}.messages{3,msgInd2}.rangeMeas;
                heading = rob{2}.messages{3,msgInd2}.headingMeas;
                posProj = rob{2}.mu(1:2,i2)+[range*cos(heading);...
                    range*sin(heading)];
                m2 = plot3([rob{2}.mu(1,i2), posProj(1)],[rob{2}.mu(2,i2), posProj(2)],[-100,-100],'Color',colors(5,:),'LineWidth',2);
                
            end
        end
    end
    
%             gif
    pause(0.1);
    delete(h4);
    delete(m1);
    delete(m2);
end
%%
% CODE TO PLOT VEHICLE TRAJECTORIES

colors = [68,119,170;102,204,238;34,136,51;220, 197, 51;238,102,119;170,51,119;187,187,187]/256;
f=figure();
mapBounds = [4061311, 4062146, 594160, 594995];
map.plot(mapBounds);

f.Position = [1000,200,1000,800];
hold on

t3 = 400;
[~,t1] = min(abs(rob{1}.tTRN-rob{3}.tTRN(t3)));
[~,t2] = min(abs(rob{2}.tTRN-rob{3}.tTRN(t3)));
plot3(rob{3}.bestEst(1,1:t3),rob{3}.bestEst(2,1:t3),-100*ones(size(rob{3}.bestEst(1,1:t3))),'Color',colors(4,:),'LineWidth',3);
plot3(rob{2}.mu(1,1:t2),rob{2}.mu(2,1:t2),-100*ones(size(rob{2}.mu(1,1:t2))),'Color',colors(6,:),'LineWidth',3);
plot3(rob{1}.mu(1,1:t1),rob{1}.mu(2,1:t1),-100*ones(size(rob{1}.mu(1,1:t1))),'Color',colors(2,:),'LineWidth',3);