/*
 * This file is part of the Owl Positioning System (OwlPS) project.
 * It is subject to the copyright notice and license terms in the
 * COPYRIGHT.t2t file found in the top-level directory of this
 * distribution and at
 *   https://code.lm7.fr/mcy/owlps/src/master/COPYRIGHT.t2t
 * No part of the OwlPS Project, including this file, may be copied,
 * modified, propagated, or distributed except according to the terms
 * contained in the COPYRIGHT.t2t file; the COPYRIGHT.t2t file must be
 * distributed along with this file, either separately or by replacing
 * this notice by the COPYRIGHT.t2t file's contents.
 */


#include "positioning.hh"
#include "realposition.hh"
#include "fbcm.hh"
#include "frbhmbasic.hh"
#include "interlinknetworks.hh"
#include "nss.hh"
#include "request.hh"
#include "result.hh"
#include "resultlist.hh"
#include "timestamp.hh"
#include "configuration.hh"
#include "posexcept.hh"
#include "stock.hh"

#include <owlps.h>

#include <iostream>
#include <unordered_set>

using namespace std ;



/* *** Constructors *** */


Positioning::Positioning()
{
  initialise_algorithms() ;
  loop() ;
}


Positioning::~Positioning()
{
  for (auto i = algorithms.begin() ; i != algorithms.end() ; ++i)
    delete *i ;
  algorithms.clear() ;
}



/* *** Operations *** */


void Positioning::initialise_algorithms()
{
  if (! Configuration::is_configured("positioning.algorithm"))
    throw missing_configuration(
      "No positioning algorithm specified in configuration!") ;

  const vector<string> &algo_names =
    Configuration::string_vector_value("positioning.algorithm") ;

  // We will store the names of the stored algorithms in a set, to
  // avoid multiple occurrences of the same algorithm:
  unordered_set<string> stored_algos ;

  for (auto i = algo_names.begin() ; i != algo_names.end() ; ++i)
    {
      if (*i == "Real")
        {
          auto stored = stored_algos.insert("Real") ;
          if (stored.second) // Check if there was no previous instance
            algorithms.insert(algorithms.begin(), new RealPosition) ;
          /* Note: in order to compute the errors of the other
           * algorithms, Real must be the first for each request,
           * so we add it at the begining. */
        }

      else if (*i == "FBCM")
        {
          auto stored = stored_algos.insert("FBCM") ;
          if (stored.second) // Check if there was no previous instance
            {
              /* Generate the Friis indexes only if the autocalibration
               * is not activated (if it is, Friis indexes will be
               * regenerated each time FBCM wants to computes a position)
               */
              if (! Configuration::autocalibration_enabled())
                Stock::update_all_friis_indexes() ;
              algorithms.push_back(new FBCM) ;
            }
        }

      else if (*i == "FRBHMBasic")
        {
          auto stored = stored_algos.insert("FRBHMBasic") ;
          if (stored.second) // Check if there was no previous instance
            algorithms.push_back(new FRBHMBasic) ;
          // TODO: Pre-compute all per-ReferencePoint friis indexes?
        }

      else if (*i == "InterlinkNetworks")
        {
          auto stored = stored_algos.insert("InterlinkNetworks") ;
          if (stored.second) // Check if there was no previous instance
            algorithms.push_back(new InterlinkNetworks) ;
        }

      else if (*i == "NSS")
        {
          auto stored = stored_algos.insert("NSS") ;
          if (stored.second) // Check if there was no previous instance
            algorithms.push_back(new NSS) ;
        }

      else
        throw bad_configuration(
          "The specified positioning_algorithm \""+ *i +
          "\" is unknown!") ;
    }
}


void Positioning::loop()
{
  vector<PositioningAlgorithm*>::const_iterator algo ;

  while (! input.eof() && owl_run)
    {
      const Request &request = input.get_next_request() ;
      if (! request)
        continue ;

      Point3D real_position ;
      bool compute_error = false ;
      ResultList results(&request) ;
      for (algo = algorithms.begin() ; algo != algorithms.end() ; ++algo)
        {
          const string &algo_name = (*algo)->get_name() ;

          Result res ;
          try
            {
              res = (*algo)->compute(request) ;
            }
          catch (exception &e)
            {
              cerr
                  << "Cannot compute with algorithm "
                  << algo_name << ": \""
                  << e.what() << "\"\n" ;
              continue ;
            }

          // Apply filtering if needed
          filter(request, res) ;

          if (compute_error)
            res.compute_error(real_position) ;
          else if (algo_name == "Real")
            {
              compute_error = true ;
              real_position = res.get_position() ;
            }

          results.add(res) ;
        }

      // If filtering is enabled, update the last known position(s) of
      // the current mobile
      bool do_filter =
        Configuration::float_value("positioning.filter.max-speed") > 0 ;
      if (do_filter)
        {
          const Mobile *const mobile = request.get_mobile() ;
          const_cast<Mobile*>(mobile)->set_last_results(results) ;
        }

      output.write(results) ;
    }
}


/**
 * @param[in] request The request from which `result` has been computed.
 * @param[in,out] result The unfiltered Result, that will be updated by
 * this function if needed.
 */
void Positioning::filter(const Request &request, Result &result)
{
  /* Don't apply filter if filtering is disabled */
  float max_speed =
    Configuration::float_value("positioning.filter.max-speed") ;
  if (max_speed <= 0)
    return ;

  /* Don't apply filter if it is the first result we compute for this
   * mobile (hence we don't have previous results to compare to) */
  const Mobile *const mobile = request.get_mobile() ;
  const ResultList &last_results = mobile->get_last_results() ;
  if(last_results.empty())
    return ;

  /* Don't apply filter if the "algorithm" is the real position */
  string algo_name = result.get_algorithm() ;
  if (algo_name == "Real")
    return ;

  /* Print the common message in verbose mode */
  const Timestamp &timestamp = result.get_request()->get_time_sent() ;
  if (Configuration::is_configured("verbose"))
    cerr
        << "Filtering (algorithm " << algo_name << "): "
        << "for request sent at " << timestamp
        << " by mobile " << mobile->get_mac_addr()
        << " (maximum speed is " << max_speed << " km/h), " ;

  /* Use alternative maximum speed if the reset distance is configured
   * and the mobile is within this distance */
  // Get the unfiltered coordinates:
  const Point3D &pos = result.get_position() ;
  float reset_distance =
    Configuration::float_value("positioning.filter.cp-reset-distance") ;
  if (reset_distance > 0)
    {
      float dist_closest_cp = Stock::distance_from_closest_cp(pos) ;
      assert(dist_closest_cp >= 0) ;
      if (dist_closest_cp <= reset_distance)
        {
          if (Configuration::is_configured("verbose"))
            cerr
                << "the mobile appears to be within the reset distance"
                << " of " << reset_distance
                << " m from a CP" ;
          max_speed =
            Configuration::float_value("positioning.filter.max-speed-cp") ;
          if (max_speed <= 0)
            {
              if (Configuration::is_configured("verbose"))
                cerr
                    << " and the alternative speed is \"unlimited\";"
                    << " no filtering.\n" ;
              return ;
              /* Note: saying that the mobile's speed is unlimited is
               * another way to say that the filter is disabled. */
            }
          if (Configuration::is_configured("verbose"))
            cerr
                << "; maximum speed set to alternative speed ("
                << max_speed << " km/h).\n" ;
        }
    }

  /* Okay, now let's get to it! */

  const Result &prev_result = last_results.get_result_for_algo(algo_name) ;

  // Calculate the travel time (duration)
  const Timestamp &prev_timestamp =
    last_results.get_request()->get_time_sent() ;
  Timestamp duration = prev_timestamp.elapsed(timestamp) ;

  // Get the previous coordinates
  const Point3D &prev_pos = prev_result.get_position() ;

  // Recompute the result by interpolation
  Point3D filtered_pos(prev_pos.interpolate(pos, max_speed, duration)) ;
  if (filtered_pos != pos)
    {
      if (Configuration::is_configured("verbose"))
        cerr << pos << " was replaced by " << filtered_pos << ".\n" ;
      result.set_position(filtered_pos) ;
    }
  else if (Configuration::is_configured("verbose"))
    cerr << "the original position looked good enough and was kept.\n" ;
}