#include "stock.hh"
#include "configuration.hh"
#include "posexcept.hh"
#include "area.hh"

#include <iostream>

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



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

unordered_map<string, Building> Stock::buildings ;

unordered_map<Point3D, Waypoint> Stock::waypoints ;

unordered_map<string, Mobile> Stock::mobiles ;

uint32_t Stock::nb_virtual_mobiles = 0 ;

unordered_map<string, AccessPoint> Stock::aps ;

unordered_set<ReferencePoint,
              boost::hash<ReferencePoint>,
              reference_point_equal_to> 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() ;
  aps.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)
{
  unordered_map<string, Building>::const_iterator 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.
 * @return A const reference to the Building.
 * @throw element_not_found is thrown if the Building corresponding
 * to \em name does not exist.
 */
const Building& Stock::get_building(const string &name)
{
  unordered_map<string, Building>::const_iterator 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 \em point was found.
 * @returns NULL if \em point does not belong to any Area.
 */
const Area* Stock::in_which_area_is(const Point3D &point)
{
  for (unordered_map<string, Building>::const_iterator
       b = buildings.begin() ; b != buildings.end() ; ++b)
    {
      const unordered_map<string, Area*> &areas = b->second.get_areas() ;
      for (unordered_map<string, Area*>::const_iterator
           a = areas.begin() ; a != areas.end() ; ++a)
        if (a->second->contains_point(point))
          return a->second ;
    }

  return NULL ;
}



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


const Waypoint& Stock::
find_create_waypoint(const Point3D &point)
{
  unordered_map<Point3D, Waypoint>::const_iterator 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,
                                     const Building *building)
{
  unordered_map<Point3D, Waypoint>::iterator 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() == NULL)
    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.
 * @return \em true if a mobile with this MAC address exists.
 * @return \em false if this MAC address was not found.
 */
bool Stock::mobile_exists(const std::string &mac)
{
  unordered_map<string, Mobile>::const_iterator 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.
 * @return A const reference to the Mobile.
 * @throw element_not_found is thrown if the Mobile corresponding
 * to \em mac does not exist.
 */
const Mobile& Stock::get_mobile(const string &mac)
{
  unordered_map<string, Mobile>::const_iterator 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)
{
  unordered_map<string, Mobile>::const_iterator 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 the \em source.
 */
const Mobile& Stock::find_create_mobile(const Mobile &source)
{
  const string &mac = source.get_mac_addr() ;
  unordered_map<string, Mobile>::const_iterator i = mobiles.find(mac) ;
  if (i != mobiles.end())
    return i->second ;

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



/* *** AccessPoint operations *** */


/**
 * @param mac The MAC address of the AccessPoint to search for.
 * It must be a valid MAC address, as no check is performed.
 * @return \em true if an AP with this MAC address exists.
 * @return \em false if this MAC address was not found.
 */
bool Stock::ap_exists(const std::string &mac)
{
  unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
  return i != aps.end() ;
}


/**
 * @param mac The MAC address of the AccessPoint to search for.
 * It must be a valid MAC address, as no check is performed.
 * @return A const reference to the AccessPoint.
 * @throw element_not_found is thrown if the AccessPoint corresponding
 * to \em mac does not exist.
 */
const AccessPoint& Stock::get_ap(const string &mac)
{
  unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
  if (i != aps.end())
    return i->second ;
  throw element_not_found("No AccessPoint with MAC address « " +
                          mac + " »!") ;
}


/**
 * The MAC address of the AccessPoint is initialised.
 */
const AccessPoint& Stock::find_create_ap(const string &mac)
{
  unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
  if (i != aps.end())
    return i->second ;

  AccessPoint &ap = aps[mac] ;
  ap.set_mac_addr(mac) ;

  if (Configuration::is_configured("verbose"))
    cerr
        << "New AP « " << mac
        << " » (total: " << nb_aps() << " APs).\n" ;

  return ap ;
}


/**
 * If the AccessPoint already exists, it is replaced by the \em source.
 */
const AccessPoint& Stock::find_create_ap(const AccessPoint &source)
{
  const string &mac = source.get_mac_addr() ;
  unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
  if (i != aps.end())
    return i->second ;

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


void Stock::update_all_friis_indexes()
{
  for (unordered_map<string, AccessPoint>::iterator ap = aps.begin() ;
       ap != aps.end() ; ++ap)
    {
      string ap_mac = ap->second.get_mac_addr() ;
      double friis_idx_sum = 0 ;
      int nb_friis_idx = 0 ;

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

      /*
       * Compute indexes for each ReferencePoint. The Friis index for an
       * AP is the average of all indexes in all ReferencePoint.
       */
      for (unordered_set<ReferencePoint>::const_iterator rp =
             reference_points.begin() ; rp != reference_points.end() ;
           ++rp)
        {
          int nb_idx = 0 ;
          float ap_friis_idx_sum =
            rp->friis_indexes_for_ap(ap->second, const_term, nb_idx) ;
          if (nb_idx != 0)
            {
              friis_idx_sum += ap_friis_idx_sum ;
              nb_friis_idx += nb_idx ;
            }
        }

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


double Stock::ap_matrix_get_ss(const std::string &mac_transmitter,
                               const std::string &mac_receiver)
{
  const AccessPoint &receiver = get_ap(mac_receiver) ;
  const ReferencePoint &rp =
    get_reference_point(receiver.get_coordinates()) ;

  return rp.average_measurements(mac_transmitter) ;
}


/**
 * @returns \em true if \em coord are the coordinates of an existing
 * AP, \em false if not.
 */
bool Stock::is_ap_coordinate(const Point3D &coord)
{
  for (unordered_map<string, AccessPoint>::const_iterator ap =
         aps.begin() ; ap != aps.end() ; ++ap)
    if (ap->second.get_coordinates() == coord)
      return true ;

  return false ;
}



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


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


/**
 * @param point Coordinates of the wanted ReferencePoint.
 * @return 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 ReferencePoint &point)
{
  unordered_set<ReferencePoint>::const_iterator i =
    reference_points.find(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://boost.org/doc/libs/1_42_0/doc/html/boost/unordered_set.html
  pair<unordered_set<ReferencePoint>::iterator, bool> 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_aps = Configuration::bool_value(
                      "positioning.radar-ignore-ap-reference-points") ;
  unordered_set<ReferencePoint>::const_iterator i =
    reference_points.begin() ;

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

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

  float distance = i->ss_square_distance(request) ;
  unordered_set<ReferencePoint>::const_iterator closest = i ;

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

      float tmp_distance = i->ss_square_distance(request) ;
      if (tmp_distance < distance)
        {
          distance = tmp_distance ;
          closest = i ;
        }
    }

  return *closest ;
}


void Stock::regenerate_reference_points()
{
  assert(! aps.empty()) ;
  assert(! reference_points.empty()) ;

  delete_non_ap_calibration_requests() ;

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

  /* Generate RPs */
  if (! Configuration::is_configured("positioning.area-start") ||
      ! Configuration::is_configured("positioning.area-stop"))
    throw missing_configuration(
      "You must define the deployment area in order to generate"
      " reference points.") ;
  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 z = 1 ; // FIXME
  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)
      {
        Point3D current_point(x,y,z) ;
        generate_reference_point(current_point) ;
      }
}


/**
 * The reference point P we want to generate will end-up containing
 * as many calibration requests as the total number of APs.
 * For each of these requests, we consider a transmitter TRX (which is
 * a virtual mobile) and a receiver RX (the current AP).
 * To generate the signal strength (SS) received in P by RX from TRX,
 * we first select two reference APs REF1 and REF2. Then, we average
 * the real measurements REF1->RX and REF2->RX, weighting in function
 * of the angles REF-RX-TRX and the distance from RX to TRX to compute
 * the SS.
 * Hope this is clear enough…
 */
void Stock::generate_reference_point(const Point3D &point)
{
  // If the point is the coordinates of an existing AP, we don't
  // need to generate it
  if (is_ap_coordinate(point))
    return ;

  /* Get/create the reference point */
  const ReferencePoint &current_rp =
    find_create_reference_point(point) ;

  /* Create the timestamp */
  Timestamp time_sent ;
  time_sent.now() ;

  /* Create the measurement list */
  unordered_map<string, Measurement> measurements ;

  /* Prepare the virtual mobile */
  string vmob_mac(PosUtil::int_to_mac(nb_virtual_mobiles++)) ;
  // The gain and trx power of the mobile will be the average of
  // those of all the known APs (could be better, probably)
  double vmob_gain = 0 ;
  double vmob_pow = 0 ;

  for (unordered_map<string, AccessPoint>::const_iterator
       rx = aps.begin() ; rx != aps.end() ; ++rx)
    {
      /* Update the mobile's attributes */
      vmob_gain += rx->second.get_antenna_gain() / aps.size() ;
      vmob_pow += rx->second.get_trx_power() / aps.size() ;

      /* Choose the 2 nearest APs in angle */
      const Point3D &rx_coord =
        rx->second.get_coordinates() ;
      multimap<double,
               unordered_map<string, AccessPoint>::const_iterator>
               sorted_angles ;
      for (unordered_map<string, AccessPoint>::const_iterator
           ref = aps.begin() ; ref != aps.end() ; ++ref)
        {
          if (ref == rx)
            continue ;
          const Point3D &ref_coord =
            ref->second.get_coordinates() ;
          // Skip the AP if the associated reference point does
          // not exist yet:
          if (! reference_point_exists(ref_coord))
            continue ;
          double angle =
            rx_coord.angle(point, ref_coord) ;
          pair<double,
               unordered_map<string, AccessPoint>::const_iterator>
               angle_ap(angle, ref) ;
          sorted_angles.insert(angle_ap) ;
        }
      assert(sorted_angles.size() > 1) ;

      /* Compute the angle weight of the 2 reference APs */
      map<double,
          unordered_map<string, AccessPoint>::const_iterator>
          ::const_iterator s = sorted_angles.begin() ;
      // Angle REF1-RX-P
      double ref1_angle = s->first ;
      const AccessPoint &ref1 = s->second->second ;
      ++s ;
      const AccessPoint &ref2 = s->second->second ;

      // Angle REF1-RX-REF2
      const Point3D &ref1_coord = ref1.get_coordinates() ;
      const Point3D &ref2_coord = ref2.get_coordinates() ;
      double reference_angle =
        rx_coord.angle(ref1_coord, ref2_coord) ;
      double ref1_percent, ref2_percent ;
      if (reference_angle == 0)
        ref1_percent = ref2_percent = 50 ;
      else
        {
          ref1_percent = ref1_angle * 100 / reference_angle ;
          if (ref1_percent > 100)
            ref1_percent = 100 ;
          ref2_percent = 100 - ref1_percent ;
        }

      /* Compute the SS that would be received from a mobile at
       * distance of RX-P, in the direction of each reference AP */
      // Distance RX-P
      float point_dst =
        rx_coord.distance(point) ;
      // Constant term
      double common_ss = rx->second.friis_constant_term() ;
      // SS for REF1
      const ReferencePoint &ref1_rp =
        get_reference_point(ref1_coord) ;
      double friis_index =
        ref1_rp.friis_index_for_ap(rx->second.get_mac_addr()) ;
      double ref1_ss =
        common_ss +
        ref1.get_trx_power() +
        ref1.get_antenna_gain() -
        10 * friis_index * log10(point_dst) ;
      // SS for REF2
      const ReferencePoint &ref2_rp =
        get_reference_point(ref2_coord) ;
      friis_index =
        ref2_rp.friis_index_for_ap(rx->second.get_mac_addr()) ;
      double ref2_ss =
        common_ss +
        ref2.get_trx_power() +
        ref2.get_antenna_gain() -
        10 * friis_index * log10(point_dst) ;

      /* Compute the SS */
      ref1_ss = ref1_ss * ref1_percent / 100 ;
      ref2_ss = ref2_ss * ref2_percent / 100 ;
      double rx_ss = ref1_ss + ref2_ss ;

      /* Create the measurement, add it to the list */
      Measurement m(&rx->second) ;
      m.add_ss(rx_ss) ;
      measurements[rx->second.get_mac_addr()] = m ;
    }

  /* Create the virtual mobile */
  Mobile vmob("", vmob_mac, vmob_gain, vmob_pow) ;
  const Mobile &mobile = find_create_mobile(vmob) ;

  /* Create the calibration request */
  CalibrationRequest cr(OWL_REQUEST_GENERATED) ;
  cr.set_time_sent(time_sent) ;
  cr.set_mobile(&mobile) ;
  cr.set_measurements(measurements) ;
  cr.set_reference_point(&current_rp) ;

  store_calibration_request(cr) ;
}



/* *** 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
  unordered_set<CalibrationRequest>::const_iterator 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) ;
}


const CalibrationRequest& Stock::
find_create_calibration_request(const CalibrationRequest &request)
{
  pair<unordered_set<CalibrationRequest>::iterator, bool> 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_aps = Configuration::bool_value(
                      "positioning.radar-ignore-ap-reference-points") ;
  unordered_set<CalibrationRequest>::const_iterator i =
    calibration_requests.begin() ;

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

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

  float distance = i->ss_square_distance(request) ;
  unordered_set<CalibrationRequest>::const_iterator closest = i ;

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

      float tmp_distance = i->ss_square_distance(request) ;
      if (tmp_distance < distance)
        {
          distance = tmp_distance ;
          closest = i ;
        }
    }

  return *closest ;
}


void Stock::delete_non_ap_calibration_requests()
{
  unordered_set<ReferencePoint>::iterator 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 ;
    }
}