/*
 * 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 "minmax.hh"
#include "accesspoint.hh"

using std::tr1::unordered_map ;



Point3D MinMax::multilaterate(
  const unordered_map<AccessPoint*, float> &_ap_distances)
{
  min = INFINITE ;
  centre = start ;
  ap_distances = &_ap_distances ;

  for (float x = start.get_x() ; x <= stop.get_x() ; x += step)
    for (float y = start.get_y() ; y <= stop.get_y() ; y += step)
      for (float z = start.get_z() ; z <= stop.get_z() ; z += step)
        iterate(x, y, z) ;

  return centre ;
}


Point3D MinMax::multilaterate_2d(
  const unordered_map<AccessPoint*, float> &_ap_distances, float z)
{
  min = INFINITE ;
  centre = start ;
  ap_distances = &_ap_distances ;

  for (float x = start.get_x() ; x <= stop.get_x() ; x += step)
    for (float y = start.get_y() ; y <= stop.get_y() ; y += step)
      iterate(x, y, z) ;

  return centre ;
}


void MinMax::iterate(float x, float y, float z)
{
  float d_max = 0 ;
  for (unordered_map<AccessPoint*, float>::const_iterator i =
         ap_distances->begin() ; i != ap_distances->end() ; ++i)
    {
      float dist =
        Point3D(x, y, z).distance_to_sphere(
          i->first->get_coordinates(), i->second) ;
      if (dist > d_max)
        d_max = dist ;
    }
  if (d_max <= min)
    {
      min = d_max ;
      centre.set_coordinates(x, y, z) ;
    }
}