/*
 * 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 "stock.hh"
#include "configuration.hh"
#include "posexcept.hh"
#include "area.hh"
#include "csvstringreader.hh"
#include "autocalibrationmesh.hh"
#include "autocalibrationline.hh"

#include <iostream>
#include <cmath>

using namespace std ;



/* *** Attribute definitions *** */

unordered_map<string, Building> Stock::buildings ;

unordered_map<Point3D, Waypoint> Stock::waypoints ;

unordered_map<string, Mobile> Stock::mobiles ;

unordered_map<string, CapturePoint> Stock::cps ;

unordered_set<ReferencePoint> Stock::reference_points ;

unordered_set<CalibrationRequest> Stock::calibration_requests ;


void Stock::clear()
{
  buildings.clear() ;

  // Waypoints are normally deleted in Building::~Building(), so we must
  // not clear waypoints before buildings, but we clear it *after*, just
  // in case.
  waypoints.clear() ;

  mobiles.clear() ;
  cps.clear() ;
  reference_points.clear() ;
  calibration_requests.clear() ;
}



/* *** Building operations *** */


/**
 * The name of the Building is initialised.
 */
const Building& Stock::find_create_building(const string &name)
{
  auto i = buildings.find(name) ;
  if (i != buildings.end())
    return i->second ;

  Building &building = buildings[name] ;
  building.set_name(name) ;
  return building ;
}


/**
 * @param name The name of the Building to search for.
 * It must be a valid name, as no check is performed.
 * @returns A const reference to the Building.
 * @throw element_not_found is thrown if the Building corresponding
 * to `name` does not exist.
 */
const Building& Stock::get_building(const string &name)
{
  auto i = buildings.find(name) ;
  if (i != buildings.end())
    return i->second ;
  throw element_not_found("No Building with name \"" + name + "\"!") ;
}


/**
 * @returns A pointer to the first Area in which `point` was found.
 * @returns nullptr if `point` does not belong to any Area.
 */
const Area* Stock::in_which_area_is(const Point3D &point)
{
  for (auto b = buildings.begin() ; b != buildings.end() ; ++b)
    {
      const unordered_map<string, const Area*> &areas =
        b->second.get_areas() ;
      for (auto a = areas.begin() ; a != areas.end() ; ++a)
        if (a->second->contains_point(point))
          return a->second ;
    }

  return nullptr ;
}



/* *** Waypoint operations *** */


const Waypoint& Stock::
find_create_waypoint(const Point3D &point)
{
  auto i = waypoints.find(point) ;
  if (i != waypoints.end())
    return i->second ;

  Waypoint &waypoint = waypoints[point] ;
  waypoint.set_coordinates(point) ;
  return waypoint ;
}


void Stock::waypoint_remove_building(const Waypoint &point,
                                     Building *const building)
{
  auto i = waypoints.find(point) ;

  Waypoint *waypoint = const_cast<Waypoint*>(&i->second) ;
  waypoint->remove_building(building) ;

  // If the Waypoint is not linked to any Building any more, we
  // delete it
  if (! waypoint->get_1st_building())
    waypoints.erase(i) ;
}



/* *** Mobile operations *** */


/**
 * @param mac The MAC address of the Mobile to search for.
 * It must be a valid MAC address, as no check is performed.
 * @returns `true` if a mobile with this MAC address exists.
 * @returns `false` if this MAC address was not found.
 */
bool Stock::mobile_exists(const std::string &mac)
{
  auto i = mobiles.find(mac) ;
  return i != mobiles.end() ;
}


/**
 * @param mac The MAC address of the Mobile to search for.
 * It must be a valid MAC address, as no check is performed.
 * @returns A const reference to the Mobile.
 * @throw element_not_found is thrown if the Mobile corresponding
 * to `mac` does not exist.
 */
const Mobile& Stock::get_mobile(const string &mac)
{
  auto i = mobiles.find(mac) ;
  if (i != mobiles.end())
    return i->second ;
  throw element_not_found("No Mobile with MAC address \"" +
                          mac + "\"!") ;
}


/**
 * If created, the MAC address of the Mobile is initialised.
 */
const Mobile& Stock::find_create_mobile(const string &mac)
{
  auto i = mobiles.find(mac) ;
  if (i != mobiles.end())
    return i->second ;

  Mobile &mobile = mobiles[mac] ;
  mobile.set_mac_addr(mac) ;
  return mobile ;
}


/**
 * If the Mobile already exists, it is replaced by `source.`
 */
const Mobile& Stock::find_create_mobile(const Mobile &source)
{
  const string &mac = source.get_mac_addr() ;
  auto i = mobiles.find(mac) ;
  if (i != mobiles.end())
    return i->second ;

  mobiles[mac] = source ;
  return mobiles[mac] ;
}



/* *** CapturePoint operations *** */


/**
 * @param mac The MAC address of the CapturePoint to search for.
 * It must be a valid MAC address, as no check is performed.
 * @returns `true` if a CP with this MAC address exists.
 * @returns `false` if this MAC address was not found.
 */
bool Stock::cp_exists(const std::string &mac)
{
  auto i = cps.find(mac) ;
  return i != cps.end() ;
}


/**
 * @param mac The MAC address of the CapturePoint to search for.
 * It must be a valid MAC address, as no check is performed.
 * @returns A const reference to the CapturePoint.
 * @throw element_not_found is thrown if the CapturePoint corresponding
 * to `mac` does not exist.
 */
const CapturePoint& Stock::get_cp(const string &mac)
{
  auto i = cps.find(mac) ;
  if (i != cps.end())
    return i->second ;
  throw element_not_found("No CapturePoint with MAC address \"" +
                          mac + "\"!") ;
}


/**
 * The MAC address of the CapturePoint is initialised.
 */
const CapturePoint& Stock::find_create_cp(const string &mac)
{
  auto i = cps.find(mac) ;
  if (i != cps.end())
    return i->second ;

  CapturePoint &cp = cps[mac] ;
  cp.set_mac_addr(mac) ;

  if (Configuration::is_configured("verbose"))
    cerr
        << "New CP \"" << mac
        << "\" (total: " << nb_cps() << " CPs).\n" ;

  return cp ;
}


/**
 * If the CapturePoint already exists, it is replaced by `source.`
 */
const CapturePoint& Stock::find_create_cp(const CapturePoint &source)
{
  const string &mac = source.get_mac_addr() ;
  auto i = cps.find(mac) ;
  if (i != cps.end())
    return i->second ;

  cps[mac] = source ;
  return cps[mac] ;
}


void Stock::update_all_friis_indexes()
{
  for (auto cp = cps.begin() ; cp != cps.end() ; ++cp)
    {
      double friis_idx_sum = 0 ;
      int nb_friis_idx = 0 ;

      // Compute main general term, independant from scans
      double const_term = cp->second.friis_constant_term() ;

      /*
       * Compute indexes for each ReferencePoint. The Friis index for an
       * CP is the average of all indexes in all ReferencePoint.
       */
      for (auto rp = reference_points.begin() ;
           rp != reference_points.end() ; ++rp)
        {
          int nb_idx = 0 ;
          float cp_friis_idx_sum =
            rp->friis_indexes_for_cp(cp->second, const_term, nb_idx) ;
          if (nb_idx != 0)
            {
              friis_idx_sum += cp_friis_idx_sum ;
              nb_friis_idx += nb_idx ;
            }
        }

      if (nb_friis_idx > 0)
        cp->second.set_friis_index(friis_idx_sum / nb_friis_idx) ;
    }
}


double Stock::cp_matrix_get_ss(const std::string &mac_transmitter,
                               const std::string &mac_receiver)
{
  const CapturePoint &receiver = get_cp(mac_receiver) ;
  const ReferencePoint &rp =
    get_reference_point(receiver.get_coordinates()) ;

  return rp.average_measurements(mac_transmitter) ;
}


/**
 * @returns A const pointer to the CP `coord` is the coordinates of.
 * @returns nullptr if `coord` is not the coordinates of any CP.
 */
const CapturePoint* Stock::is_cp_coordinate(const Point3D &coord)
{
  for (auto cp = cps.begin() ; cp != cps.end() ; ++cp)
    if (cp->second.get_coordinates() == coord)
      return &cp->second ;

  return nullptr ;
}


/**
 * @returns The distance from `location` to the closest CP.
 * @returns -1 if there are no CPs in stock.
 */
float Stock::distance_from_closest_cp(const Point3D &location)
{
  auto cp = cps.begin() ;
  if (cp == cps.end())
    return -1 ;

  float min_dist = location.square_distance(cp->second.get_coordinates()) ;
  ++cp ;

  while (cp != cps.end())
    {
      float tmp_dist =
        location.square_distance(cp->second.get_coordinates()) ;
      if (tmp_dist < min_dist)
        min_dist = tmp_dist ;
      ++cp ;
    }

  return sqrtf(min_dist) ;
}



/* *** ReferencePoint operations *** */


bool Stock::reference_point_exists(const ReferencePoint &point)
{
  auto i = reference_points.find(point) ;
  return i != reference_points.end() ;
}


/**
 * @param point Coordinates of the wanted ReferencePoint.
 * @returns The ReferencePoint at the given coordinates, if found.
 * @throw element_not_found is thrown if no ReferencePoint exists at the
 * given coordinates.
 */
const ReferencePoint& Stock::
get_reference_point(const Point3D &point)
{
  /* Note that we can look up a ReferencePoint in the unordered_set
   * #reference_points using only its coordinates (Point3D) only because
   * we defined `hash` and `equal_to` to take into consideration only
   * the coordinates of the ReferencePoint (see referencepoint.hh).
   */
  auto i = reference_points.find(ReferencePoint(point));
  if (i != reference_points.end())
    return *i ;
  throw element_not_found("No ReferencePoint with coordinates " +
                          static_cast<string>(point) + "!") ;
}


const ReferencePoint& Stock::
find_create_reference_point(const ReferencePoint &point)
{
  // unordered_set::insert() do all the job: see the documentation at
  // http://en.cppreference.com/w/cpp/container/unordered_set/insert
  auto ret = reference_points.insert(point) ;
  return *ret.first ;
}


const ReferencePoint& Stock::
closest_reference_point(const Request &request)
{
  if (reference_points.empty())
    throw element_not_found(
      "Cannot search for the closest reference point: reference points'"
      " list is empty!") ;

  bool ignore_cps = Configuration::bool_value(
                      "positioning.nss.ignore-cp-reference-points") ;
  auto i = reference_points.begin() ;

  if (ignore_cps)
    {
      // Fast-forward to the next non-CP reference point
      while (i != reference_points.end() && is_cp_coordinate(*i))
        ++i ;

      // No non-CP reference point was found, we are forced to consider
      // the CP reference points
      if (i == reference_points.end())
        ignore_cps = false ;
    }

  float similarity = i->similarity(request) ;
  auto closest = i ;

  for (++i ; i != reference_points.end() ; ++i)
    {
      if (ignore_cps && is_cp_coordinate(*i))
        continue ;

      float tmp_similarity = i->similarity(request) ;
      if (tmp_similarity < similarity)
        {
          similarity = tmp_similarity ;
          closest = i ;
        }
    }

  return *closest ;
}


void Stock::regenerate_reference_points()
{
  assert(Configuration::autocalibration_enabled()) ;
  assert(! cps.empty()) ;
  assert(! reference_points.empty()) ;

  delete_non_cp_calibration_requests() ;

  const string &ac_mode = Configuration::string_value(
                            "positioning.generate-reference-points") ;
  assert(! ac_mode.empty()) ;

  if (ac_mode.find("mesh") != string::npos)
    generate_reference_points_mesh() ;

  if (ac_mode.find("line") != string::npos)
    generate_reference_points_line() ;

  if (ac_mode.find("list") != string::npos)
    generate_reference_points_list() ;
}


void Stock::generate_reference_points_mesh()
{
  if (! (Configuration::is_configured("positioning.area-start") &&
         Configuration::is_configured("positioning.area-stop")))
    throw missing_configuration(
      "In order to generate a meshing of reference points, you must"
      " define the deployment area.") ;

  if (reference_points.size() < 3)
    {
      if (Configuration::is_configured("verbose"))
        cerr << reference_points.size() << " true reference points is"
             << " not enough to generate a meshing of reference"
             << " points.\n" ;
      return ;
    }

  Point3D start(Configuration::string_value("positioning.area-start")) ;
  Point3D stop(Configuration::string_value("positioning.area-stop")) ;
  float step_x =
    Configuration::float_value("positioning.generated-meshing-grain-x") ;
  float step_y =
    Configuration::float_value("positioning.generated-meshing-grain-y") ;
  float step_z =
    Configuration::float_value("positioning.generated-meshing-grain-z") ;

  /* Generate RPs */
  for (float x = start.get_x() ; x <= stop.get_x() ; x += step_x)
    for (float y = start.get_y() ; y <= stop.get_y() ; y += step_y)
      for (float z = start.get_z() ; z <= stop.get_z() ; z += step_z)
        {
          AutocalibrationMesh ac(Point3D(x,y,z)) ;
          generate_reference_point(ac) ;
        }
}


void Stock::generate_reference_points_line()
{
  if (! Configuration::is_configured("positioning.generated-line-path"))
    throw missing_configuration(
      "In order to generate a line of reference points, you must"
      " define a path.") ;

  if (reference_points.empty())
    {
      if (Configuration::is_configured("verbose"))
        cerr << "At least one true reference points is necessary to"
             << " generate a path of reference points.\n" ;
      return ;
    }

  const string &path_csv =
    Configuration::string_value("positioning.generated-line-path") ;
  CSVStringReader path_parser(path_csv) ;
  Point3D p ;
  vector<Point3D> read_points ;
  const CapturePoint *cp1 = nullptr ;

  // Read points until the first CP coordinate
  while (path_parser.read_point3d(p))
    {
      cp1 = Stock::is_cp_coordinate(p) ;
      if (cp1)
        break ;
      read_points.push_back(p) ;
    }

  // We reached the end of the points' list without finding the
  // coordinates of a CP
  if (! cp1)
    {
      cerr << "Warning! positioning.generated-line-path doesn't contain"
           << "any point that is the coordinates of capture point!"
           << endl ;
      return ;
    }

  // Generate the first RPs (between the begining and cp1)
  if (! read_points.empty())
    {
      auto first = read_points.begin() ;
      for (auto end = first + 1 ; end != read_points.end() ; ++end)
        {
          generate_interpolated_reference_points(*first, *end, cp1) ;
          first = end ;
        }
      generate_interpolated_reference_points(
        *first, cp1->get_coordinates(), cp1) ;
      read_points.clear() ;
    }

  // Read the next points and generate the RPs
  const CapturePoint *cp2 = nullptr ;
  while (path_parser.read_point3d(p))
    {
      cp2 = Stock::is_cp_coordinate(p) ;
      if (! cp2)
        {
          read_points.push_back(p) ;
          continue ;
        }

      // We don't have any intermediate points between the two CPs
      if (read_points.empty())
        generate_interpolated_reference_points(
          cp1->get_coordinates(), cp2->get_coordinates(), cp1, cp2) ;

      // Generate the RPs between cp1, the intermediate points, and cp2
      else
        {
          auto first = read_points.begin() ;
          generate_interpolated_reference_points(
            cp1->get_coordinates(), *first, cp1, cp2) ;
          for (auto end = first + 1 ; end != read_points.end() ; ++end)
            {
              generate_interpolated_reference_points(*first, *end,
                                                     cp1, cp2) ;
              first = end ;
            }
          generate_interpolated_reference_points(
            *first, cp2->get_coordinates(), cp1, cp2) ;
          read_points.clear() ;
        }

      cp1 = cp2 ;
      cp2 = nullptr ;
    }

  // Generate the last RPs (after the last RP)
  if (read_points.empty())
    return ;
  auto first = read_points.begin() ;
  generate_interpolated_reference_points(
    cp1->get_coordinates(), *first, cp1) ;
  for (auto end = first + 1 ; end != read_points.end() ; ++end)
    {
      generate_interpolated_reference_points(*first, *end, cp1) ;
      first = end ;
    }
}


/**
 * This function interpolates the coordinates between `start` and `end`
 * and generates all the corresponding reference points (the two
 * extremities are generated as well).
 *
 * @param start The first point of the line.
 * @param end The last point of the line.
 * @param cp1 The first capture point to use to generate the reference
 * points.
 * @param cp2 The second capture point to use (optional).
 */
void Stock::generate_interpolated_reference_points(
  const Point3D &start, const Point3D &end,
  const CapturePoint *const cp1, const CapturePoint *const cp2)
{
  assert(cp1) ;

  // Generate the first RP
  AutocalibrationLine ac_start(start, cp1, cp2) ;
  generate_reference_point(ac_start) ;

  /* Compute the intermediate coordinates */
  float step_hint =
    Configuration::float_value("positioning.generated-line-step") ;
  vector<Point3D> interpolated ; // List of intermediate points
  // Generate the intermediate coordinates
  start.interpolate(end, step_hint, interpolated) ;

  // Generate the corresponding reference points
  for (auto i = interpolated.begin() ; i != interpolated.end() ; ++i)
    {
      AutocalibrationLine ac(*i, cp1, cp2) ;
      generate_reference_point(ac) ;
    }

  // Generate the last RP
  AutocalibrationLine ac_end(end, cp1, cp2) ;
  generate_reference_point(ac_end) ;
}


void Stock::generate_reference_points_list()
{
  if (! Configuration::is_configured("positioning.generated-points-list"))
    throw missing_configuration(
      "In order to generate a list of reference points, you must"
      " define the list.") ;

  if (reference_points.size() < 3)
    {
      if (Configuration::is_configured("verbose"))
        cerr << reference_points.size() << " true reference points is"
             << " not enough to generate a meshing of reference"
             << " points.\n" ;
      return ;
    }

  const string &rp_list_csv =
    Configuration::string_value("positioning.generated-points-list") ;

  CSVStringReader rp_list_parser(rp_list_csv) ;

  Point3D rp ;
  while (rp_list_parser.read_point3d(rp))
    {
      AutocalibrationMesh ac(rp) ;
      generate_reference_point(ac) ;
    }
}


/**
 * @param ac The pre-initialised Autocalibration to use.
 */
void Stock::generate_reference_point(Autocalibration &ac)
{
  try
    {
      ac.generate_reference_point() ;
    }
  catch (autocalibration_error &e)
    {
      if (Configuration::is_configured("verbose"))
        cerr << e.what() << '\n' ;
    }
}



/* *** CalibrationRequest operations *** */


/**
 * @returns The CalibrationRequest created or found.
 */
const CalibrationRequest& Stock::
store_calibration_request(const CalibrationRequest &request)
{
  const CalibrationRequest &calibration_request =
    find_create_calibration_request(request) ;
  calibration_request.reference_point_backward_link() ;
  return calibration_request ;
}


void Stock::
delete_calibration_request(const CalibrationRequest &request)
{
#ifndef NDEBUG
  auto found = calibration_requests.find(request) ;
  if (found == calibration_requests.end())
    {
      cerr
          << "Error! Cannot delete an unstored CalibrationRequest: "
          << request << endl ;
      return ;
    }
#endif // NDEBUG

  calibration_requests.erase(request) ;
}


/**
 * @param timeout The maximum age of the calibration requests (seconds).
 */
void Stock::delete_calibration_requests_older_than(const int timeout)
{
  assert(timeout > 0) ;

  auto cr = calibration_requests.begin() ;
  while (cr != calibration_requests.end())
    {
      Timestamp request_time ;
      if (Configuration::bool_value("replay"))
        request_time = cr->get_time_sent() ;
      else
        request_time = cr->get_time_received() ;
      Timestamp elapsed(request_time.elapsed()) ;
      uint64_t elapsed_sec = static_cast<uint64_t>(elapsed) / 1000 ;
      if (elapsed_sec >= static_cast<unsigned int>(timeout))
        {
          ReferencePoint *rp = cr->get_reference_point() ;
          if (rp)
            {
              rp->delete_request(&*cr) ;
              if (rp->get_requests().empty())
                reference_points.erase(*rp) ;
            }
          cr = calibration_requests.erase(cr) ;
        }
      else
        ++cr ;
    }
}


const CalibrationRequest& Stock::
find_create_calibration_request(const CalibrationRequest &request)
{
  auto ret = calibration_requests.insert(request) ;
  return *ret.first ;
}


const CalibrationRequest& Stock::
closest_calibration_request(const Request &request)
{
  if (calibration_requests.empty())
    throw element_not_found(
      "Cannot search for the closest calibration request: calibration"
      " requests' list is empty!") ;

  bool ignore_cps = Configuration::bool_value(
                      "positioning.nss.ignore-cp-reference-points") ;
  auto i = calibration_requests.begin() ;

  if (ignore_cps)
    {
      // Fast-forward to the next non-CP reference point
      while (i != calibration_requests.end() &&
             is_cp_coordinate(*i->get_reference_point()))
        ++i ;

      // No non-CP reference point was found, we are forced to consider
      // the CP reference points
      if (i == calibration_requests.end())
        {
          i = calibration_requests.begin() ;
          ignore_cps = false ;
        }
    }

  float similarity = i->similarity(request) ;
  auto closest = i ;

  for (++i ; i != calibration_requests.end() ; ++i)
    {
      if (ignore_cps && is_cp_coordinate(*i->get_reference_point()))
        continue ;

      assert(! i->get_measurements().empty()) ;

      float tmp_similarity = i->similarity(request) ;
      if (tmp_similarity < similarity)
        {
          similarity = tmp_similarity ;
          closest = i ;
        }
    }

  return *closest ;
}


void Stock::delete_non_cp_calibration_requests()
{
  auto rp = reference_points.begin() ;
  while (rp != reference_points.end())
    {
      ReferencePoint rp_copy(*rp) ;
      if (rp_copy.delete_generated_requests()) // rp_copy was modified
        {
          rp = reference_points.erase(rp) ;
          // Reinsert the modified copy if it still contains at least
          // one CalibrationRequest:
          if (! rp_copy.get_requests().empty())
            reference_points.insert(rp_copy) ;
          /* __Note on the above insert__
           * From the boost::unordered documentation:
           * "insert is only allowed to invalidate iterators when the
           *  insertion causes the load factor to be greater than or
           *  equal to the maximum load factor."
           * In our case, we always remove an element prior to insert,
           * so the load factor will never change; therefore insert()
           * should be safe here.
           */
        }
      else // rp_copy was not modified
        ++rp ;
    }
}