% Run efficient place recognition method with either a periodic
% relocalization procedure or an event-based relocalization procedure
% (Version 0.1)
% 
% S ... pairwise similarity matrix
% S_DB ... pairwise intra-database similarity matrix
% reloc ... relocalization method. Valid values are "periodic" or "event"
% K ... initial number of matching candidates
% v ... number of added subsequent database descriptors of the matching
%       candidates in DB
% T_reloc ... if periodic relocalization is chosen, T_reloc defines the
%             number of query images before a new relocalization
%
%
%   =====================================================================
%   Copyright (C) 2021  Stefan Schubert, stefan.schubert@etit.tu-chemnitz.de
%   
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%   
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%   
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%   =====================================================================
% 
function S_new = run_epr(S, S_DB, reloc, K, v, T_reloc)

  %% check if relocalization method is valid
  periodic = false;
  event = false;
  if strcmp(reloc, "periodic")
    periodic = true;
  elseif strcmp(reloc, "event")
    event = true;
  else
    error('Relocalization method does not exist. Implemented methods are ["periodic", "event"].');
  end
    
  %% if required, set default parameters
  if ~exist('m', 'var'); K = 5; end
  if ~exist('v', 'var'); v = 5; end
  if ~exist('T', 'var'); T_reloc = 100; end
  
  %% parameter tuning
  % handle loops and stops
  p_DB = 1 - 1e-6;
  mu = median(S_DB(:));
  sigma = 1.4826*median(abs(S_DB(:)-mu));
  theta_DB = icdf('Normal', p_DB, mu, sigma);
  
  % event-based relocalization
  if event
    p = 0.95;
    mu = median(S(:,1));
    sigma = 1.4826*median(abs(S(:,1)-mu));
    theta_s = icdf('Normal', p, mu, sigma);
  end
  
  %% binarize S_DB
  S_DB = S_DB >= theta_DB;

  %% init  
  S_new = min(S(:))*ones(size(S), 'single');
  
  %% perform initial relocalization with 1st query
  S_new(:,1) = S(:,1);
  
  %% run over queries
  for q = 2:size(S,2)
    
    % use sequence
    db_last = get_best(S_new(:,q-1), S_DB, K);
    db_last = db_last(:) + (0:v);
    db_last = min(db_last(:), size(S_DB,1));
    db = unique(db_last(:));

    S_new(db,q) = S(db,q);

    % relocalization (periodic)
    if event && all(S(db,q) < theta_s) ... (event-based)
      || periodic && mod(q, T_reloc) == 0 %(periodic)
      
      S_new(:,q) = S(:,q);
    else
      % add loops & stops
      db = get_best(S_new(:,q), S_DB, K);
      S_new(db,q) = S(db,q);
    end
  end
end

%%
function db = get_best(S, S_DB, K)
  % find m highest similarities
  [~,db] = maxk(S, K);

  % search in S_DB for similar DB-images
  [db,~] = find(S_DB(:,db));
  db = unique(db);
end
