/*
 * 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 "autocalibrationmesh.hh"

#include "point3d.hh"
#include "capturepoint.hh"
#include "stock.hh"
#include "configuration.hh"
#include "posexcept.hh"

#include <map>
#include <string>
#include <iostream>
#include <sstream>

using namespace std ;



/* *** Operations *** */


void AutocalibrationMesh::generate_ss()
{
  /* Compute the origin angle P-RX-O */
  const Point3D &rx_coord = rx->second.get_coordinates() ;
  Point3D origin(rx_coord) ;
  origin.set_x(rx_coord.get_x() + 10) ;
  origin_angle = rx_coord.angle_2d(point, origin) ;
  if (point.get_y() > rx_coord.get_y())
    origin_angle = -origin_angle ;

  /* Choose the nearest CP(s) in angle */
  sort_reference_cps() ;
  // We need at least one reference CP:
  if (sorted_negative_angles.empty() && sorted_positive_angles.empty())
    throw autocalibration_error("Not enough CPs in coverage!") ;

  /* Compute the angle weight of the reference CPs */
  weight_cps() ;

  /* Compute the SS that would be received from a mobile at a
   * distance of RX-P, in the direction of each reference CP */
  compute_ss() ;
}


/**
 * The reference (transmitter) CPs are first filtered (floor, coverage),
 * then sorted according to their angles with RX and P.
 */
void AutocalibrationMesh::sort_reference_cps()
{
  const Point3D &rx_coord = rx->second.get_coordinates() ;

  sorted_negative_angles.clear() ;
  sorted_positive_angles.clear() ;

  for (auto ref = Stock::cps.begin() ; ref != Stock::cps.end() ; ++ref)
    {
      if (ref == rx)
        continue ;

      /* Skip the CP if it is not at the same floor than the
       * receiver CP */
      const Point3D &ref_coord = ref->second.get_coordinates() ;
      if (ref_coord.get_z() != rx_coord.get_z())
        continue ;

      /* Skip the CP if it is not in coverage with the receiver CP */
      float coverage =
        rx->second.received_calibration_from_cp(ref->second) ;
      if (coverage < 1) // Less than 1% coverage is ridiculous!
        continue ;

      /* Angle P-RX-REF */
      double angle = rx_coord.angle_2d(point, ref_coord) ;

      /* Weight in the CPs' list according to coverage and angle */
      double weight = angle / coverage ;
      /* Note: this weight is used only to sort the CPs, it has nothing
         to do with the angle weight (see weight_cps()) used to compute
         the SSs. */

      /* Create the list entry */
      pair<double,
           unordered_map<string, CapturePoint>::const_iterator>
           angle_cp(angle, ref) ;
      pair<double, pair<double,
           unordered_map<string, CapturePoint>::const_iterator> >
           weight_angle_cp(weight, angle_cp) ;

      /* Rotate the CP's coordinates to know the angle direction */
      Point3D ref_coord_r(ref_coord) ;
      ref_coord_r.rotate_2d(rx_coord, origin_angle) ;
      // Insert the CP in the right list, according to the direction:
      if (ref_coord_r.get_y() < rx_coord.get_y())
        sorted_negative_angles.insert(weight_angle_cp) ;
      else
        sorted_positive_angles.insert(weight_angle_cp) ;
    }
}


void AutocalibrationMesh::weight_cps()
{
  /* Retrieve the reference CPs & angles */
  ref_cps.clear() ;
  map<double, pair<double,
      unordered_map<string, CapturePoint>::const_iterator> >
      ::const_iterator s ;
  s = sorted_negative_angles.begin() ;
  if (s != sorted_negative_angles.end())
    init_cp(s) ;
  s = sorted_positive_angles.begin() ;
  if (s != sorted_positive_angles.end())
    init_cp(s) ;

  /* Weight the CPs */
  if (ref_cps.size() == 1)
    ref_cps[0].weight = 100 ;
  else if (ref_cps.size() == 2)
    weight_2_cps() ;
  else
    {
      ostringstream oss ;
      oss << "We should not be here... error when sorting the reference"
          << " CPs? (" << ref_cps.size() << " CPs sorted)" ;
      throw autocalibration_error(oss.str()) ;
    }
}


void AutocalibrationMesh::init_cp(
  const map<double, pair<double, unordered_map<
  string, CapturePoint>::const_iterator> >::const_iterator s)
{
  struct cp ref ;
  ref.cp = &s->second.second->second ;
  ref.angle = s->second.first ; // Angle P-REF-RX
  ref_cps.push_back(ref) ;
}


void AutocalibrationMesh::weight_2_cps()
{
  assert(ref_cps.size() > 1) ;

  /* Compute the angle REF1-RX-REF2 */
  double reference_angle = ref_cps[0].angle + ref_cps[1].angle ;
  /* Note: the reference angle must be the sum of the two angles
     P-RX-REF1 and P-RX-REF2 instead of simply REF1-RX-REF2, to allow
     the angle to be within [0;360[° instead of [0;180[°. This is needed
     if P-RX-REF1 or P-RX-REF2 are greater than 90°, otherwise the
     weights would be wrong. */

  /* Weight the two CPs */
  if (reference_angle == 0)
    ref_cps[0].weight = ref_cps[1].weight = 50 ;
  else
    {
      ref_cps[0].weight = ref_cps[0].angle * 100 / reference_angle ;
      if (ref_cps[0].weight > 100)
        ref_cps[0].weight = 100 ;
      ref_cps[1].weight = 100 - ref_cps[0].weight ;
    }
}


void AutocalibrationMesh::compute_multi_packet_ss()
{
  const cp &ref1 = ref_cps[0] ;
  const ReferencePoint &ref1_rp =
    Stock::get_reference_point(ref1.cp->get_coordinates()) ;
  const vector<CalibrationRequest*> &ref1_cr = ref1_rp.get_requests() ;

  /* Search for the first measurement made by RX in ref1_rp */
  const Measurement *rx_measurement = nullptr ;
  vector<CalibrationRequest*>::const_iterator cr ;
  const string &rx_mac = rx->second.get_mac_addr() ;
  for (cr = ref1_cr.begin() ; cr != ref1_cr.end() ; ++cr)
    {
      rx_measurement = (*cr)->get_measurement(rx_mac) ;
      if (rx_measurement)
        break ;
    }

  // The selected reference CPs are in coverage of RX, therefore
  // we should always find a measurement of RX in ref1_rp
  assert(rx_measurement) ;

  /* Generate the SS for each packet */
  bool packet_generated = false ;
  for (pkt_id_t pkt_id = 1 ; pkt_id <= (*cr)->get_nb_packets() ;
       ++pkt_id)
    if (compute_single_packet_ss(pkt_id))
      packet_generated = true ;

  if (! packet_generated)
    {
      if (Configuration::is_configured("verbose"))
        cerr << "No packet has been generated for RX CP " << rx_mac
             << "... failing back to single packet request. Calibration"
             << " request involved : " << **cr << "\n" ;
      compute_single_packet_ss(0) ;
    }
}


/**
 * @param pkt_id The number of the packet to create. Must be 0 if all
 * the generated reference points contain only one packet.
 */
bool AutocalibrationMesh::compute_single_packet_ss(pkt_id_t pkt_id)
{
  /* Distance RX-P */
  const Point3D &rx_coord = rx->second.get_coordinates() ;
  float point_dst = rx_coord.distance(point) ;

  /* Compute the consolidated SS */
  double rx_ss = 0 ;
  // Reminder: ref_cps.size() can be 1 or 2
  for (unsigned int i = 0 ; i < ref_cps.size() ; ++i)
    {
      double tmp_ss = compute_weighted_ss(i, pkt_id, point_dst) ;
      if (tmp_ss > 127) // error
        return false ;
      rx_ss += tmp_ss ;
    }

  /* Add the SS to the measurements' list */
  const string &rx_mac = rx->second.get_mac_addr() ;
  // 0 is not a valid packet ID, so we use 1 if we generate only one
  // packet per reference point:
  if (pkt_id == 0)
    pkt_id = 1 ;
  // The Measurement measurements[rx_mac] is created if needed:
  measurements[rx_mac].add_ss(pkt_id, rx_ss) ;
  // Useful only if the measurement was just created, but it is not
  // worth a test:
  measurements[rx_mac].set_cp(&rx->second) ;

  return true ;
}


/**
 * @param cp_idx The index of the CP in #ref_cps.
 * @param pkt_id The number of the packet to generate, or 0 if we
 * generate only one packet per reference point.
 * @param point_dst The distance RX-P.
 *
 * @returns The weighted SS value in dBm, or 128 in case of error.
 */
double AutocalibrationMesh::compute_weighted_ss(
  const unsigned int cp_idx, const pkt_id_t pkt_id, const float point_dst)
{
  const cp &ref = ref_cps[cp_idx] ;
  const ReferencePoint &rp =
    Stock::get_reference_point(ref.cp->get_coordinates()) ;

  /* Friis index */
  const string &rx_mac = rx->second.get_mac_addr() ;
  float friis_idx ;
  if (pkt_id == 0)
    friis_idx = rp.friis_index_for_cp(rx_mac) ;
  else
    {
      friis_idx = rp.friis_index_for_cp(rx_mac, pkt_id) ;
      if (friis_idx == 0)
        return 128 ; // The packet pkt_id does not exist in rp
    }

  /* SS */
  // Constant term
  double common_ss = rx->second.friis_constant_term() ;
  double ss =
    common_ss +
    ref.cp->get_trx_power() +
    ref.cp->get_antenna_gain() -
    10 * friis_idx * log10(point_dst) ;

  return ss * ref.weight / 100 ;
}