close all

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

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

minVal = [inf;inf;inf];
maxVal = [0;0;0];
for r= 1:2
    T = length(rob{r}.tTRN);
    for t=1:T
        newMin = min(rob{r}.part{t},[],2);
        minVal = min(newMin,minVal);
        newMax = max(rob{r}.part{t},[],2);
        maxVal = max(newMax,maxVal);
    end
    minVal = min([rob{r}.mu,minVal],[],2);
    maxVal = max([rob{r}.mu,maxVal],[],2);

end

% CODE TO GENERATE COVARIANCE & ERROR PLOTS
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 =[];
for j = 1:2
    subplot(2,1,j)
    
    hold on
    h(end+1) = plot(rob{2}.tTRN - rob{2}.tTRN(1),zeros(size(rob{2}.tTRN)),'k','LineWidth',1);
    h(end+1) = plot(rob{2}.tTRN - rob{2}.tTRN(1),rob{2}.mu(j,:)-rob{2}.truth(j,:),'-.','Color',colors(3,:),'LineWidth',2);
    h(end+1) = plot(rob{2}.tTRN-rob{2}.tTRN(1),3*sqrt([cellfun(@(v)v(j,j),rob{2}.Sigma)]),'Color',colors(3,:),'LineWidth',2);
    h(end+1) = plot(rob{2}.tTRN-rob{2}.tTRN(1),-3*sqrt([cellfun(@(v)v(j,j),rob{2}.Sigma)]),'Color',colors(3,:),'LineWidth',2);
    
    h(end+1) = plot(rob{2}.tTRN-rob{2}.tTRN(1),rob{2}.bestEst(j,:)-rob{2}.truth(j,:),'Color',colors(5,:),'LineStyle',':','LineWidth',3);
    h(end+1) = plot(rob{2}.tTRN-rob{2}.tTRN(1),3*sqrt([cellfun(@(v)v(j,j),rob{2}.bestSig)]),'Color',colors(5,:),'LineWidth',2);
    h(end+1) = plot(rob{2}.tTRN-rob{2}.tTRN(1),-3*sqrt([cellfun(@(v)v(j,j),rob{2}.bestSig)]),'Color',colors(5,:),'LineWidth',2);
    
    xlim([0 7001]);
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(3),h(6)],'Ego-Only','Cooperative TRN','Location','NorthEast','Orientation','horizontal')
f.Position = [100,100,1000,1000];

indStart = find(rob{2}.tTRN > rob{1}.messages{2,1}.time,1,'first');
% indStart = 866;
meanCov(1) = mean(3*sqrt([cellfun(@(v)v(1,1),rob{2}.bestSig(indStart:end))]));
meanCov(2) = mean(3*sqrt([cellfun(@(v)v(2,2),rob{2}.bestSig(indStart:end))]));

f = figure;
t = rob{2}.tTRN - rob{2}.tTRN(1);
tMR = [];
errMR = [];
errStd = [];

for i = 1:length(t)
    tMR(end+1) = t(i);
    errMR(end+1) = norm(rob{2}.bestEst(:,i)-rob{2}.truth(:,i));
    errStd(end+1) = norm(rob{2}.mu(:,i)-rob{2}.truth(:,i));
end


hold on
h(1) = plot(tMR,errStd,'-.','Color',colors(3,:),'LineWidth',2);

h(2) = plot(tMR,errMR,':','Color',colors(5,:),'LineWidth',3);
xlim([0 7001]);
xlabel('Elapsed Time [s]','FontWeight','bold','FontSize',16);
ylabel('Total Error [m]','FontWeight','bold','FontSize',16);
legend([h(1),h(2)],'Ego-Only','Cooperative TRN','Location','NorthEast','Orientation','horizontal')
a = gca;
a.FontSize = 16;
a.FontWeight = 'bold';

f.Position = [1000,800,1000,500];

meanErr = mean(errMR(indStart:end));


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

f.Position = [1000,200,1000,800];
hold on
h1 = [];
h2 = [];
h3 = [];
gif('TwoVehicle.gif','frame',f,'LoopCount',0);
% pause();
tTot = [rob{1}.tTRN;rob{2}.tTRN];
tTot = sort(tTot);
% iStart = find(tTot > rob{2}.tTRN(420),1);
% iEnd = find(tTot > rob{2}.tTRN(900),1);
iStart = 1512;
iEnd = 1828;
msgInd = find(rob{1}.tRange > tTot(iStart),1);
for i = iStart:iEnd%length(tTot)
    t = tTot(i);
%     h4 = text(4062170,594040,-100,['Time = ',num2str(t-rob{2}.tTRN(1),4),' sec'],'FontWeight','bold','FontSize',16,'Color','w');
h4 = text(4062100,594224,-100,['Time = ',num2str(t-rob{2}.tTRN(1),4),' sec'],'FontWeight','bold','FontSize',16,'Color','w');
    i2 = find(rob{2}.tTRN == tTot(i));
    if ~isempty(i2)
        delete(h2);
        delete(h3);
        h2 = rob{2}.plot(i2,h2,colors(4,:),2);
    end
    
    i1 = find(rob{1}.tTRN == tTot(i));
    if ~isempty(i1)
        delete(h1);
        delete(h3);
        h1 = rob{1}.plot(i1,h1,colors(6,:),2);
        
        if (rob{1}.messages{2,msgInd}.time < t) && all(diag(rob{1}.messages{2,msgInd}.Sigma) < 15)
            
            range = rob{1}.messages{2,msgInd}.rangeMeas;
            heading = rob{1}.messages{2,msgInd}.headingMeas;
            posProj = rob{1}.mu(1:2,i1)+[range*cos(heading);...
                range*sin(heading)];
            h3 = plot3([rob{1}.mu(1,i1), posProj(1)],[rob{1}.mu(2,i1), posProj(2)],[-100,-100],'Color',colors(5,:),'LineWidth',2);
            msgInd = msgInd + 1;
%             pause(0.5);
        end
    end

        gif
        delete(h4);
%     pause(0.1);
end
