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

using namespace std ;



Point3D MinMax::trilaterate(
  const unordered_map<const CapturePoint*, float> &_cp_distances)
{
  min = INFINITE ;
  centre = start ;
  cp_distances = &_cp_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<const CapturePoint*, float> &_cp_distances, float z)
{
  min = INFINITE ;
  centre = start ;
  cp_distances = &_cp_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(const float x, const float y, const float z)
{
  float d_max = 0 ;
  for (auto i = cp_distances->begin() ; i != cp_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) ;
    }
}