/*
 * This file is part of the Owl Positioning System (OwlPS).
 * OwlPS is a project of the University of Franche-Comte
 * (Université de Franche-Comté), France.
 *
 * Copyright © Université de Franche-Comté 2007-2012.
 *
 * Corresponding author: Matteo Cypriani <mcy@lm7.fr>
 *
 ***********************************************************************
 *
 * This software is governed by the CeCILL license under French law and
 * abiding by the rules of distribution of free software. You can use,
 * modify and/or redistribute the software under the terms of the CeCILL
 * license as circulated by CEA, CNRS and INRIA at the following URL:
 * http://www.cecill.info
 *
 * As a counterpart to the access to the source code and rights to copy,
 * modify and redistribute granted by the license, users are provided
 * only with a limited warranty and the software's authors, the holder
 * of the economic rights, and the successive licensors have only
 * limited liability.
 *
 * In this respect, the user's attention is drawn to the risks
 * associated with loading, using, modifying and/or developing or
 * reproducing the software by the user in light of its specific status
 * of free software, that may mean that it is complicated to manipulate,
 * and that also therefore means that it is reserved for developers and
 * experienced professionals having in-depth computer knowledge. Users
 * are therefore encouraged to load and test the software's suitability
 * as regards their requirements in conditions enabling the security of
 * their systems and/or data to be ensured and, more generally, to use
 * and operate it in the same conditions as regards security.
 *
 * The fact that you are presently reading this means that you have had
 * knowledge of the CeCILL license and that you accept its terms.
 *
 ***********************************************************************
 */


#include "minmax.hh"
#include "accesspoint.hh"

using std::tr1::unordered_map ;



Point3D MinMax::trilaterate(
  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::trilaterate_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) ;
    }
}