close all
clear all

pkg load signal

# Use with files generated by export_stats.sh

dataDir = 'SET_PATH_TO_RESULTS_DIR';
resultsToShow = 1; % 1=single loc, 2=merged loc, 3=consecutive

datasetPrefix = 'Stat';
datasets = [0 1 6 7 9 14 11 111]; % 0 1 6 7 8 9 11 12
datasetsName = {'SURF' 'SIFT' 'ORB' 'FAST/FREAK' 'FAST/BRIEF' 'GFTT/FREAK' 'GFTT/BRIEF' 'BRISK' 'GFTT/ORB' 'KAZE' 'ORB-OCTREE' 'SuperPoint' 'SURF/FREAK' 'GFTT/DAISY' 'SURF/DAISY'};
datasetsName{112} = 'SuperGlue'
sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};

if resultsToShow == 3
  sep = [0, 1000, 3000, 5000, 7000, 9000];
  sepName = {'17:27', '17:54', '18:27', '18:56', '19:35'};
  datasetPrefix = 'Consecutive'
endif

percentResults = {};
totalResults = {};
locResults = {};

figure

colors = get(gca, 'ColorOrder');
tmp=colors(3,:);
colors(3,:) = colors(5,:);
colors(5,:) = tmp;

globalSeparators = [];
globalx = [];
globaly = [];
globalc = [];

for d=1:length(datasets)

data = dlmread([dataDir '/' datasetPrefix  num2str(datasets(d)) '-Loop-Map_id-' '.txt'], '\t', 1, 0, "emptyvalue", NaN);

curvesBeg = 2;
curvesEnd = size(data,2)-5;  % -4 for '0', -5 for '1'

if resultsToShow == 2
  curvesBeg = 8; % 2 if only 4 merged_reduced in stats, 8 to skip first 6
  curvesEnd = size(data,2);
elseif resultsToShow == 3
  curvesEnd = size(data,2);
endif
curves = curvesEnd - curvesBeg + 1;

percentResultsTmp = zeros(curves, length(sep)-1);
totalResultsTmp = zeros(curves, length(sep)-1);
locResultsTmp = zeros(curves, length(sep)-1);

offset = 1;

for i = 1:curves
  index = i + curvesBeg - 1;
  separators = [];
  x_all = [];
  y_all = [];
  m_all = [];
  previousMax = 0;
  for j = 1:length(sep)-1
    x = data(:,1);
    y = data(:,index);
    y = y(x>=sep(j) & x<=sep(j+1), :);
    x = x(x>=sep(j) & x<=sep(j+1), :);
    minimum = x(1,1);
    separators = [separators previousMax];
    x = x - (minimum-previousMax);
    previousMax = x(end,1);
    y = y + 1;
    m = y;
    y(y>0) = 1;
    y(isnan(y)) = 0;
    percent = sum(y)/length(y);
    percentResultsTmp(i,j) = percent;
    locResultsTmp(i,j) = sum(y);
    totalResultsTmp(i,j) = length(y);
    y(y>0) =  -(d-1)*curves -i - (d-1)*offset;
    %x(y==0) = nan;
    m(y==0) = nan;
    y(y==0) = nan;
    if resultsToShow == 2
      if i==1 %% Merged 1, 6
        m(m==1) = 1;
        m(m==2) = 6;
      elseif i==2 %% Merged 1,3(2 sessions),5
        m(m==1) = 1;
        m(m==2) = 3;
        m(m==3) = 3;
        m(m==4) = 5;
      elseif i==3 %% Merged 2(2 sessions),4,6
        m(m==1) = 2;
        m(m==2) = 2;
        m(m==4) = 6;
        m(m==3) = 4;
      elseif i>=4 %% Merged 1, 2(2 sessions), 3(2 sessions),4,5,6
        m(m==1) = 1;
        m(m==2) = 2;
        m(m==3) = 2;
        m(m==4) = 3;
        m(m==5) = 3;
        m(m==6) = 4;
        m(m==7) = 5;
        m(m==8) = 6;
      endif
    endif
    x = upsample(x, 2);
    y = upsample(y, 2);
    m = upsample(m, 2);
    x(2:2:end-1) = x(3:2:end);
    y(2:2:end-1) = y(3:2:end);
    m(2:2:end) = m(1:2:end);
    x = x(1:end-1);
    y = y(1:end-1);
    m = m(1:end-1);
  
    x_all = [x_all nan x'];
    y_all = [y_all nan y'];
    m_all = [m_all nan m'];
  endfor
  if resultsToShow == 2
    globalx = [globalx x_all];
    globaly = [globaly y_all];
    globalc = [globalc m_all];
  else
    plot(x_all,y_all, 'linewidth', 3, 'color', colors(i,:))
    hold on
  endif
  separators = [separators previousMax];
  globalSeparators = separators;
endfor
percentResults{1,d} = percentResultsTmp;
totalResults{1,d} = totalResultsTmp;
locResults{1,d} = locResultsTmp;
endfor

if resultsToShow == 2
  indColors = ones(length(globalc), 3);
  for j=1:length(globalc)
    if ~isnan(globalc(j))
      indColors(j,:) = colors(globalc(j),:);
     endif
  endfor
  for i=1:6
    tmpx = globalx;
    tmpy = globaly;
    tmpx(globalc~=i) = nan;
    tmpy(globalc~=i) = nan;
    plot(tmpx, tmpy, 'linewidth', 3, 'color', colors(i,:));
    if i==1
      hold on
    endif
  endfor
endif

for j=1:length(globalSeparators)
  x = globalSeparators(j);
  plot([x,x],[(-length(datasets)*(curves+1)) ,0], 'k','linewidth', 2);
endfor

for d=1:length(datasets)
annotation ("textbox", [0, 0.96-((d-0.5)/length(datasets))*0.95, 0,0], 'string', datasetsName{datasets(d)+1})
endfor
for s=1:length(sep)-1
annotation ("textbox", [0.1 + ((separators(s+1)-separators(s))/2+separators(s))/separators(end)*0.75, 0.98, 0,0], 'string', sepName{s})
endfor
axis('tight')



set(gca, 'units', 'normalized');
Tight = get(gca, 'Position');
NewPos = [Tight(1) 0.01 0.77 0.95]; %New plot position [X Y W H]
set(gca, 'Position', NewPos);
if length(sep) == 7
  legend('16:46', '17:27', '17:54', '18:27', '18:56', '19:35', "location", 'northeastoutside' )
else
  legend('16:46', '17:27', '17:54', '18:27', '18:56', "location", 'northeastoutside' )
endif
box off
axis off

#disp(percentResults);
#disp(totalResults);

figure; 
for d=1:length(datasets)
  subplot(4,2,d)
  data=percentResults{1,d}*100;
  data(isnan(data)) = 0;
  hAxes = gca;
  % Upscaling the image to reduce anti-aliasing effect in pfd viewers
  scale = 50;
  tickXStep = zeros(1, size(data, 2));
  tickYStep = zeros(1, size(data, 1));
  dataUp = upsample(upsample(data',scale)',scale);
  for x=0:size(data, 2)-1
    for y=1:scale-1
       dataUp(:,(x*scale+1)+y) = dataUp(:,x*scale+1);
    endfor
    tickXStep(1,x+1) = scale/2 + scale*x;
  endfor
  for x=0:size(data, 1)-1
    for y=1:scale-1
       dataUp((x*scale+1)+y,:) = dataUp(x*scale+1,:);
    endfor
    tickYStep(1,x+1) = scale/2 + scale*x;
  endfor
  imagesc( hAxes, dataUp, [0, 100])
  %title({"",datasetsName{datasets(d)+1}})
  colors = [ones(100,1) [1:100]'*0.01 [1:100]'*0];
  colors(1,:) = 1;
  colormap( hAxes ,  colors)
  c = colorbar;
  labels = {};
  for v=get(c,'ytick'), labels{end+1} = sprintf('%d%%',v); end
  set(c,'yticklabel',labels);
  if mod(d,2) == 1
    ylabel("Map")
  endif
  xlabel([datasetsName{datasets(d)+1} " Localization"])
  set(gca, "xaxislocation", "top");
  set(gca, 'XTick', tickXStep)
  set(gca, 'XTickLabel', sepName, 'fontsize',7)
  set(gca, 'YTick', tickYStep)
  if resultsToShow == 3
    set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56'}, 'fontsize',7)
  elseif resultsToShow == 2
    set(gca, 'YTickLabel', {'1+6', '1+3+5', '2+4+6', '1+2+3+4+5+6', '1-2-3-4-5-6', 'not set'}, 'fontsize',7)
  else
    set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56', '19:35'}, 'fontsize',7)
  endif
endfor
    
% compute cumulative localizations
cumResults = zeros(curves+2, length(datasets)+1);
for d=1:length(datasets)
  cumResults(1,d+1) = datasets(d);
  cumResults(2:end-1,d+1) = round(sum(locResults{1,d}, 2) ./ sum(totalResults{1,d}, 2) * 100);
  if resultsToShow == 1
    cumResults(end,d+1) = round(sum(sum(locResults{1,d}.*eye(curves,curves))) / sum(totalResults{1,d},2)(1,1) * 100);
  endif
end
cumResults(2:end-1,1) = 1:curves;
cumResults