/*
 * This file is part of the Owl Positioning System (OwlPS).
 * OwlPS is a project of the University of Franche-Comté
 * (Université de Franche-Comté), France.
 */


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

#include <owlps.h>

#include <iostream>
#include <boost/tr1/unordered_set.hpp>

using namespace std ;
using std::tr1::unordered_set ;



/* *** Constructors *** */


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


Positioning::~Positioning()
{
  for (vector<PositioningAlgorithm*>::const_iterator 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 (vector<string>::const_iterator i = algo_names.begin() ;
       i != algo_names.end() ; ++i)
    {
      if (*i == "Real")
        {
          pair<unordered_set<string>::iterator, bool> 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")
        {
          pair<unordered_set<string>::iterator, bool> 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::
                  bool_value("positioning.generate-reference-points"))
                Stock::update_all_friis_indexes() ;
              algorithms.push_back(new FBCM) ;
            }
        }

      else if (*i == "FRBHMBasic")
        {
          pair<unordered_set<string>::iterator, bool> 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")
        {
          pair<unordered_set<string>::iterator, bool> stored =
            stored_algos.insert("InterlinkNetworks") ;
          if (stored.second) // Check if there was no previous instance
            algorithms.push_back(new InterlinkNetworks) ;
        }

      else if (*i == "NSS")
        {
          pair<unordered_set<string>::iterator, bool> 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)
        {
          Result res ;

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

          if (compute_error)
            res.compute_error(real_position) ;
          else if ((*algo)->get_name() == "Real")
            {
              compute_error = true ;
              real_position = res.get_position() ;
            }

          results.add(res) ;
        }

      output.write(results) ;
    }
}