/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------1------------------------------------------- */

/**
 * @file testManifold.cpp
 * @date September 18, 2014
 * @author Frank Dellaert
 * @author Paul Furgale
 * @brief unit tests for Manifold type machinery
 */

#include <gtsam/base/Manifold.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>

#undef CHECK
#include <CppUnitLite/TestHarness.h>

#include <boost/assign/list_of.hpp>
using boost::assign::list_of;
using boost::assign::map_list_of;

using namespace std;
using namespace gtsam;

// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
typedef PinholeCamera<Cal3Bundler> Camera;

//******************************************************************************
TEST(Manifold, SomeManifoldsGTSAM) {
  //BOOST_CONCEPT_ASSERT((IsManifold<int>)); // integer is not a manifold
  BOOST_CONCEPT_ASSERT((IsManifold<Camera>));
  BOOST_CONCEPT_ASSERT((IsManifold<Cal3_S2>));
  BOOST_CONCEPT_ASSERT((IsManifold<Cal3Bundler>));
  BOOST_CONCEPT_ASSERT((IsManifold<Camera>));
}

//******************************************************************************
TEST(Manifold, SomeLieGroupsGTSAM) {
  BOOST_CONCEPT_ASSERT((IsLieGroup<Rot2>));
  BOOST_CONCEPT_ASSERT((IsLieGroup<Pose2>));
  BOOST_CONCEPT_ASSERT((IsLieGroup<Rot3>));
  BOOST_CONCEPT_ASSERT((IsLieGroup<Pose3>));
}

//******************************************************************************
TEST(Manifold, SomeVectorSpacesGTSAM) {
  BOOST_CONCEPT_ASSERT((IsVectorSpace<double>));
  BOOST_CONCEPT_ASSERT((IsVectorSpace<float>));
  BOOST_CONCEPT_ASSERT((IsVectorSpace<Point2>));
  BOOST_CONCEPT_ASSERT((IsVectorSpace<Matrix24>));
}

//******************************************************************************
// dimension
TEST(Manifold, _dimension) {
  //using namespace traits;
  EXPECT_LONGS_EQUAL(2, traits<Point2>::dimension);
  EXPECT_LONGS_EQUAL(8, traits<Matrix24>::dimension);
  EXPECT_LONGS_EQUAL(1, traits<double>::dimension);
}

//******************************************************************************
TEST(Manifold, Identity) {
  EXPECT_DOUBLES_EQUAL(0.0, traits<double>::Identity(), 0.0);
  EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits<Matrix24>::Identity())));
  EXPECT(assert_equal(Pose3(), traits<Pose3>::Identity()));
  EXPECT(assert_equal(Point2(0,0), traits<Point2>::Identity()));
}

//******************************************************************************
// charts
TEST(Manifold, DefaultChart) {

  //DefaultChart<Point2> chart1;
  EXPECT(traits<Point2>::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0));
  EXPECT(traits<Point2>::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0));

  Vector v2(2);
  v2 << 1, 0;
  //DefaultChart<Vector2> chart2;
  EXPECT(assert_equal(v2, traits<Vector2>::Local(Vector2(0, 0), Vector2(1, 0))));
  EXPECT(traits<Vector2>::Retract(Vector2(0, 0), v2) == Vector2(1, 0));

  {
    typedef Matrix2 ManifoldPoint;
    ManifoldPoint m;
    //DefaultChart<ManifoldPoint> chart;
    m << 1, 3,
         2, 4;
    // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)!
    EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
    EXPECT(traits<ManifoldPoint>::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m);
  }

  {
    typedef Eigen::Matrix<double, 1, 2> ManifoldPoint;
    ManifoldPoint m;
    //DefaultChart<ManifoldPoint> chart;
    m << 1, 2;
    EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
    EXPECT(traits<ManifoldPoint>::Retract(m, Vector2(1, 2)) == 2 * m);
  }

  {
    typedef Eigen::Matrix<double, 1, 1> ManifoldPoint;
    ManifoldPoint m;
    //DefaultChart<ManifoldPoint> chart;
    m << 1;
    EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
    EXPECT(traits<ManifoldPoint>::Retract(m, ManifoldPoint::Ones()) == 2 * m);
  }

  //DefaultChart<double> chart3;
  Vector v1(1);
  v1 << 1;
  EXPECT(assert_equal(v1, traits<double>::Local(0, 1)));
  EXPECT_DOUBLES_EQUAL(traits<double>::Retract(0, v1), 1, 1e-9);

  // Dynamic does not work yet !
  Vector z = Z_2x1, v(2);
  v << 1, 0;
  //DefaultChart<Vector> chart4;
//  EXPECT(assert_equal(traits<Vector>::Local(z, v), v));
//  EXPECT(assert_equal(traits<Vector>::Retract(z, v), v));

  Vector v3(3);
  v3 << 1, 1, 1;
  Rot3 I = Rot3::identity();
  Rot3 R = I.retract(v3);
  //DefaultChart<Rot3> chart5;
  EXPECT(assert_equal(v3, traits<Rot3>::Local(I, R)));
  EXPECT(assert_equal(traits<Rot3>::Retract(I, v3), R));
  // Check zero vector
  //DefaultChart<Rot3> chart6;
  EXPECT(assert_equal((Vector) Z_3x1, traits<Rot3>::Local(R, R)));
}

//******************************************************************************
typedef ProductManifold<Point2,Point2> MyPoint2Pair;

// Define any direct product group to be a model of the multiplicative Group concept
namespace gtsam {
template<> struct traits<MyPoint2Pair> : internal::ManifoldTraits<MyPoint2Pair> {
  static void Print(const MyPoint2Pair& m, const string& s = "") {
    cout << s << "(" << m.first << "," << m.second << ")" << endl;
  }
  static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2, double tol = 1e-8) {
    return m1 == m2;
  }
};
}

TEST(Manifold, ProductManifold) {
  BOOST_CONCEPT_ASSERT((IsManifold<MyPoint2Pair>));
  MyPoint2Pair pair1(Point2(0,0),Point2(0,0));
  Vector4 d;
  d << 1,2,3,4;
  MyPoint2Pair expected(Point2(1,2),Point2(3,4));
  MyPoint2Pair pair2 = pair1.retract(d);
  EXPECT(assert_equal(expected,pair2,1e-9));
  EXPECT(assert_equal(d, pair1.localCoordinates(pair2),1e-9));
}

//******************************************************************************
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
//******************************************************************************


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "boost::simd::constants SIMD real"

#include <boost/simd/include/constants/real.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

////////////////////////////////////////////////////////////////////////////////
// Test value of real constants for every base real types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL(real_values, BOOST_SIMD_SIMD_TYPES)
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t> vd_t;
  for(std::size_t i=0; i< boost::simd::meta::cardinal_of<vd_t>::value;++i)
  {
    NT2_TEST_EQUAL( (boost::simd::Mhalf<vd_t>())[i], boost::simd::Mhalf<T>() );
    NT2_TEST_EQUAL( (boost::simd::Mzero<vd_t>())[i], boost::simd::Mzero<T>() );
    NT2_TEST_EQUAL( (boost::simd::Half<vd_t>())[i], boost::simd::Half<T>() );
    NT2_TEST_EQUAL( (boost::simd::Third<vd_t>())[i], boost::simd::Third<T>() );
    NT2_TEST_EQUAL( (boost::simd::Quarter<vd_t>())[i], boost::simd::Quarter<T>() );
    NT2_TEST_EQUAL( (boost::simd::Twotom10<vd_t>())[i], boost::simd::Twotom10<T>() );
    NT2_TEST_EQUAL( (boost::simd::Pi<vd_t>())[i], boost::simd::Pi<T>() );
    NT2_TEST_EQUAL( (boost::simd::Sqrt_2<vd_t>())[i], boost::simd::Sqrt_2<T>() );
    NT2_TEST_EQUAL( (boost::simd::Sqrt_2o_2<vd_t>())[i], boost::simd::Sqrt_2o_2<T>() );
    NT2_TEST_EQUAL( (boost::simd::Gold<vd_t>())[i], boost::simd::Gold<T>() );
    NT2_TEST_EQUAL( (boost::simd::Cgold<vd_t>())[i], boost::simd::Cgold<T>() );
    NT2_TEST_EQUAL( (boost::simd::Twotonmb<vd_t>())[i] , boost::simd::Twotonmb<T>() );
    NT2_TEST_EQUAL( (boost::simd::Splitfactor<vd_t>())[i], boost::simd::Splitfactor<T>());
  }
}

template<class T>
struct type_dependant_const_value;

template<>
struct type_dependant_const_value<double>
{
  static const uint64_t value = 0x3FF3BE76C8B43958LL;
};

template<>
struct type_dependant_const_value<float>
{
  static const uint64_t value = 0x3F9DF3B6;
};

NT2_TEST_CASE_TPL(type_dependant_const, BOOST_SIMD_SIMD_REAL_TYPES)
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION      ext_t;
  typedef boost::simd::native<T,ext_t>      v_t;

  for(std::size_t i=0; i< boost::simd::meta::cardinal_of<v_t>::value;++i)
  {
    NT2_TEST_EQUAL( (boost::simd::Const<v_t, type_dependant_const_value<T>::value>())[i], T(1.234)   );
  }
}

////////////////////////////////////////////////////////////////////////////////
// Test real_constant for every base types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL(real_pattern, BOOST_SIMD_SIMD_REAL_TYPES)
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t> vd_t;
  for(std::size_t i=0; i< boost::simd::meta::cardinal_of<vd_t>::value;++i)
  {
    NT2_TEST_EQUAL( (boost::simd::real_constant < vd_t , 0x3FF3BE76C8B43958LL
                                            , 0x3F9DF3B6
                                        >()
                    )[i],
                    (boost::simd::real_constant < T , 0x3FF3BE76C8B43958LL
                                                            , 0x3F9DF3B6
                                                        >()
                                    ));
  }
}


/**
 * \file NearestNeighbor.hpp
 * \brief Header for GeographicLib::NearestNeighbor class
 *
 * Copyright (c) Charles Karney (2016-2019) <charles@karney.com> and licensed
 * under the MIT/X11 License.  For more information, see
 * https://geographiclib.sourceforge.io/
 **********************************************************************/

#if !defined(GEOGRAPHICLIB_NEARESTNEIGHBOR_HPP)
#define GEOGRAPHICLIB_NEARESTNEIGHBOR_HPP 1

#include <algorithm>            // for nth_element, max_element, etc.
#include <vector>
#include <queue>                // for priority_queue
#include <utility>              // for swap + pair
#include <cstring>
#include <limits>
#include <cmath>
#include <iostream>
#include <sstream>
// Only for GeographicLib::GeographicErr
#include <GeographicLib/Constants.hpp>

#if defined(GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION) && \
  GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/vector.hpp>
#endif

#if defined(_MSC_VER)
// Squelch warnings about constant conditional expressions
#  pragma warning (push)
#  pragma warning (disable: 4127)
#endif

namespace GeographicLib {

  /**
   * \brief Nearest-neighbor calculations
   *
   * This class solves the nearest-neighbor problm using a vantage-point tree
   * as described in \ref nearest.
   *
   * This class is templated so that it can handle arbitrary metric spaces as
   * follows:
   *
   * @tparam dist_t the type used for measuring distances; it can be a real or
   *   signed integer type; in typical geodetic applications, \e dist_t might
   *   be double.
   * @tparam pos_t the type for specifying the positions of points; geodetic
   *   application might bundled the latitude and longitude into a
   *   std::pair<dist_t, dist_t>.
   * @tparam distfun_t the type of a function object which takes takes two
   *   positions (of type \e pos_t) and returns the distance (of type \e
   *   dist_t); in geodetic applications, this might be a class which is
   *   constructed with a Geodesic object and which implements a member
   *   function with a signature dist_t operator() (const pos_t&, const
   *   pos_t&) const, which returns the geodesic distance between two
   *   points.
   *
   * \note The distance measure must satisfy the triangle inequality, \f$
   * d(a,c) \le d(a,b) + d(b,c) \f$ for all points \e a, \e b, \e c.  The
   * geodesic distance (given by Geodesic::Inverse) does, while the great
   * ellipse distance and the rhumb line distance do not.  If you use
   * the ordinary Euclidean distance, i.e., \f$ \sqrt{(x_a-x_b)^2 +
   * (y_a-y_b)^2} \f$ for two dimensions, don't be tempted to leave out the
   * square root in the interests of "efficiency"; the squared distance does
   * not satisfy the triangle inequality!
   *
   * \note This is a "header-only" implementation and, as such, depends in a
   * minimal way on the rest of GeographicLib (the only dependency is through
   * the use of GeographicLib::GeographicErr for handling compile-time and
   * run-time exceptions).  Therefore, it is easy to extract this class from
   * the rest of GeographicLib and use it as a stand-alone facility.
   *
   * The \e dist_t type must support numeric_limits queries (specifically:
   * is_signed, is_integer, max(), digits).
   *
   * The NearestNeighbor object is constructed with a vector of points (type \e
   * pos_t) and a distance function (type \e distfun_t).  However the object
   * does \e not store the points.  When querying the object with Search(),
   * it's necessary to supply the same vector of points and the same distance
   * function.
   *
   * There's no capability in this implementation to add or remove points from
   * the set.  Instead Initialize() should be called to re-initialize the
   * object with the modified vector of points.
   *
   * Because of the overhead in constructing a NearestNeighbor object for a
   * large set of points, functions Save() and Load() are provided to save the
   * object to an external file.  operator<<(), operator>>() and  Boost
   * serialization can also be used to save and restore a NearestNeighbor
   * object.  This is illustrated in the example.
   *
   * Example of use:
   * \include example-NearestNeighbor.cpp
   **********************************************************************/
  template <typename dist_t, typename pos_t, class distfun_t>
  class NearestNeighbor {
    // For tracking changes to the I/O format
    static const int version = 1;
    // This is what we get "free"; but if sizeof(dist_t) = 1 (unlikely), allow
    // 4 slots (and this accommodates the default value bucket = 4).
    static const int maxbucket =
      (2 + ((4 * sizeof(dist_t)) / sizeof(int) >= 2 ?
            (4 * sizeof(dist_t)) / sizeof(int) : 2));
  public:

    /**
     * Default constructor for NearestNeighbor.
     *
     * This is equivalent to specifying an empty set of points.
     **********************************************************************/
    NearestNeighbor() : _numpoints(0), _bucket(0), _cost(0) {}

    /**
     * Constructor for NearestNeighbor.
     *
     * @param[in] pts a vector of points to include in the set.
     * @param[in] dist the distance function object.
     * @param[in] bucket the size of the buckets at the leaf nodes; this must
     *   lie in [0, 2 + 4*sizeof(dist_t)/sizeof(int)] (default 4).
     * @exception GeographicErr if the value of \e bucket is out of bounds or
     *   the size of \e pts is too big for an int.
     * @exception std::bad_alloc if memory for the tree can't be allocated.
     *
     * \e pts may contain coincident points (i.e., the distance between them
     * vanishes); these are treated as distinct.
     *
     * The choice of \e bucket is a tradeoff between space and efficiency.  A
     * larger \e bucket decreases the size of the NearestNeighbor object which
     * scales as pts.size() / max(1, bucket) and reduces the number of distance
     * calculations to construct the object by log2(bucket) * pts.size().
     * However each search then requires about bucket additional distance
     * calculations.
     *
     * \warning The distances computed by \e dist must satisfy the standard
     * metric conditions.  If not, the results are undefined.  Neither the data
     * in \e pts nor the query points should contain NaNs or infinities because
     * such data violates the metric conditions.
     *
     * \warning The same arguments \e pts and \e dist must be provided
     * to the Search() function.
     **********************************************************************/
    NearestNeighbor(const std::vector<pos_t>& pts, const distfun_t& dist,
                    int bucket = 4) {
      Initialize(pts, dist, bucket);
    }

    /**
     * Initialize or re-initialize NearestNeighbor.
     *
     * @param[in] pts a vector of points to include in the tree.
     * @param[in] dist the distance function object.
     * @param[in] bucket the size of the buckets at the leaf nodes; this must
     *   lie in [0, 2 + 4*sizeof(dist_t)/sizeof(int)] (default 4).
     * @exception GeographicErr if the value of \e bucket is out of bounds or
     *   the size of \e pts is too big for an int.
     * @exception std::bad_alloc if memory for the tree can't be allocated.
     *
     * See also the documentation on the constructor.
     *
     * If an exception is thrown, the state of the NearestNeighbor is
     * unchanged.
     **********************************************************************/
    void Initialize(const std::vector<pos_t>& pts, const distfun_t& dist,
                    int bucket = 4) {
      static_assert(std::numeric_limits<dist_t>::is_signed,
                    "dist_t must be a signed type");
      if (!( 0 <= bucket && bucket <= maxbucket ))
        throw GeographicLib::GeographicErr
          ("bucket must lie in [0, 2 + 4*sizeof(dist_t)/sizeof(int)]");
      if (pts.size() > size_t(std::numeric_limits<int>::max()))
        throw GeographicLib::GeographicErr("pts array too big");
      // the pair contains distance+id
      std::vector<item> ids(pts.size());
      for (int k = int(ids.size()); k--;)
        ids[k] = std::make_pair(dist_t(0), k);
      int cost = 0;
      std::vector<Node> tree;
      init(pts, dist, bucket, tree, ids, cost,
           0, int(ids.size()), int(ids.size()/2));
      _tree.swap(tree);
      _numpoints = int(pts.size());
      _bucket = bucket;
      _mc = _sc = 0;
      _cost = cost; _c1 = _k = _cmax = 0;
      _cmin = std::numeric_limits<int>::max();
    }

    /**
     * Search the NearestNeighbor.
     *
     * @param[in] pts the vector of points used for initialization.
     * @param[in] dist the distance function object used for initialization.
     * @param[in] query the query point.
     * @param[out] ind a vector of indices to the closest points found.
     * @param[in] k the number of points to search for (default = 1).
     * @param[in] maxdist only return points with distances of \e maxdist or
     *   less from \e query (default is the maximum \e dist_t).
     * @param[in] mindist only return points with distances of more than
     *   \e mindist from \e query (default = −1).
     * @param[in] exhaustive whether to do an exhaustive search (default true).
     * @param[in] tol the tolerance on the results (default 0).
     * @return the distance to the closest point found (−1 if no points
     *   are found).
     * @exception GeographicErr if \e pts has a different size from that used
     *   to construct the object.
     *
     * The indices returned in \e ind are sorted by distance from \e query
     * (closest first).
     *
     * The simplest invocation is with just the 4 non-optional arguments.  This
     * returns the closest distance and the index to the closest point in
     * ind0.  If there are several points equally close, then
     * ind0 gives the index of an arbirary one of them.  If
     * there's no closest point (because the set of points is empty), then \e
     * ind is empty and −1 is returned.
     *
     * With \e exhaustive = true and \e tol = 0 (their default values), this
     * finds the indices of \e k closest neighbors to \e query whose distances
     * to \e query are in (\e mindist, \e maxdist].  If \e mindist and \e
     * maxdist have their default values, then these bounds have no effect.  If
     * \e query is one of the points in the tree, then set \e mindist = 0 to
     * prevent this point (and other coincident points) from being returned.
     *
     * If \e exhaustive = false, exit as soon as \e k results satisfying the
     * distance criteria are found.  If less than \e k results are returned
     * then the search was exhaustive even if \e exhaustive = false.
     *
     * If \e tol is positive, do an approximate search; in this case the
     * results are to be interpreted as follows: if the k'th distance is
     * \e dk, then all results with distances less than or equal \e dk −
     * \e tol are correct; all others are suspect — there may be other
     * closer results with distances greater or equal to \e dk − \e tol.
     * If less than \e k results are found, then the search is exact.
     *
     * \e mindist should be used to exclude a "small" neighborhood of the query
     * point (relative to the average spacing of the data).  If \e mindist is
     * large, the efficiency of the search deteriorates.
     *
     * \note Only the shortest distance is returned (as as the function value).
     * The distances to other points (indexed by indj
     * for \e j > 0) can be found by invoking \e dist again.
     *
     * \warning The arguments \e pts and \e dist must be identical to those
     * used to initialize the NearestNeighbor; if not, this function will
     * return some meaningless result (however, if the size of \e pts is wrong,
     * this function throw an exception).
     *
     * \warning The query point cannot be a NaN or infinite because then the
     * metric conditions are violated.
     **********************************************************************/
    dist_t Search(const std::vector<pos_t>& pts, const distfun_t& dist,
                  const pos_t& query,
                  std::vector<int>& ind,
                  int k = 1,
                  dist_t maxdist = std::numeric_limits<dist_t>::max(),
                  dist_t mindist = -1,
                  bool exhaustive = true,
                  dist_t tol = 0) const {
      if (_numpoints != int(pts.size()))
          throw GeographicLib::GeographicErr("pts array has wrong size");
      std::priority_queue<item> results;
      if (_numpoints > 0 && k > 0 && maxdist > mindist) {
        // distance to the kth closest point so far
        dist_t tau = maxdist;
        // first is negative of how far query is outside boundary of node
        // +1 if on boundary or inside
        // second is node index
        std::priority_queue<item> todo;
        todo.push(std::make_pair(dist_t(1), int(_tree.size()) - 1));
        int c = 0;
        while (!todo.empty()) {
          int n = todo.top().second;
          dist_t d = -todo.top().first;
          todo.pop();
          dist_t tau1 = tau - tol;
          // compare tau and d again since tau may have become smaller.
          if (!( n >= 0 && tau1 >= d )) continue;
          const Node& current = _tree[n];
          dist_t dst = 0;   // to suppress warning about uninitialized variable
          bool exitflag = false, leaf = current.index < 0;
          for (int i = 0; i < (leaf ? _bucket : 1); ++i) {
            int index = leaf ? current.leaves[i] : current.index;
            if (index < 0) break;
            dst = dist(pts[index], query);
            ++c;

            if (dst > mindist && dst <= tau) {
              if (int(results.size()) == k) results.pop();
              results.push(std::make_pair(dst, index));
              if (int(results.size()) == k) {
                if (exhaustive)
                  tau = results.top().first;
                else {
                  exitflag = true;
                  break;
                }
                if (tau <= tol) {
                  exitflag = true;
                  break;
                }
              }
            }
          }
          if (exitflag) break;

          if (current.index < 0) continue;
          tau1 = tau - tol;
          for (int l = 0; l < 2; ++l) {
            if (current.data.child[l] >= 0 &&
                dst + current.data.upper[l] >= mindist) {
              if (dst < current.data.lower[l]) {
                d = current.data.lower[l] - dst;
                if (tau1 >= d)
                  todo.push(std::make_pair(-d, current.data.child[l]));
              } else if (dst > current.data.upper[l]) {
                d = dst - current.data.upper[l];
                if (tau1 >= d)
                  todo.push(std::make_pair(-d, current.data.child[l]));
              } else
                todo.push(std::make_pair(dist_t(1), current.data.child[l]));
            }
          }
        }
        ++_k;
        _c1 += c;
        double omc = _mc;
        _mc += (c - omc) / _k;
        _sc += (c - omc) * (c - _mc);
        if (c > _cmax) _cmax = c;
        if (c < _cmin) _cmin = c;
      }

      dist_t d = -1;
      ind.resize(results.size());

      for (int i = int(ind.size()); i--;) {
        ind[i] = int(results.top().second);
        if (i == 0) d = results.top().first;
        results.pop();
      }
      return d;

    }

    /**
     * @return the total number of points in the set.
     **********************************************************************/
    int NumPoints() const { return _numpoints; }

    /**
     * Write the object to an I/O stream.
     *
     * @param[in,out] os the stream to write to.
     * @param[in] bin if true (the default) save in binary mode.
     * @exception std::bad_alloc if memory for the string representation of the
     *   object can't be allocated.
     *
     * The counters tracking the statistics of searches are not saved; however
     * the initializtion cost is saved.  The format of the binary saves is \e
     * not portable.
     *
     * \note
     * Boost serialization can also be used to save and restore a
     * NearestNeighbor object.  This requires that the
     * GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION macro be defined.
     **********************************************************************/
    void Save(std::ostream& os, bool bin = true) const {
      int realspec = std::numeric_limits<dist_t>::digits *
        (std::numeric_limits<dist_t>::is_integer ? -1 : 1);
      if (bin) {
        char id[] = "NearestNeighbor_";
        os.write(id, 16);
        int buf[6];
        buf[0] = version;
        buf[1] = realspec;
        buf[2] = _bucket;
        buf[3] = _numpoints;
        buf[4] = int(_tree.size());
        buf[5] = _cost;
        os.write(reinterpret_cast<const char *>(buf), 6 * sizeof(int));
        for (int i = 0; i < int(_tree.size()); ++i) {
          const Node& node = _tree[i];
          os.write(reinterpret_cast<const char *>(&node.index), sizeof(int));
          if (node.index >= 0) {
            os.write(reinterpret_cast<const char *>(node.data.lower),
                     2 * sizeof(dist_t));
            os.write(reinterpret_cast<const char *>(node.data.upper),
                     2 * sizeof(dist_t));
            os.write(reinterpret_cast<const char *>(node.data.child),
                     2 * sizeof(int));
          } else {
            os.write(reinterpret_cast<const char *>(node.leaves),
                     _bucket * sizeof(int));
          }
        }
      } else {
        std::stringstream ostring;
          // Ensure enough precision for type dist_t.  With C++11, max_digits10
          // can be used instead.
        if (!std::numeric_limits<dist_t>::is_integer) {
          static const int prec
            = int(std::ceil(std::numeric_limits<dist_t>::digits *
                            std::log10(2.0) + 1));
          ostring.precision(prec);
        }
        ostring << version << " " << realspec << " " << _bucket << " "
                << _numpoints << " " << _tree.size() << " " << _cost;
        for (int i = 0; i < int(_tree.size()); ++i) {
          const Node& node = _tree[i];
          ostring << "\n" << node.index;
          if (node.index >= 0) {
            for (int l = 0; l < 2; ++l)
              ostring << " " << node.data.lower[l] << " " << node.data.upper[l]
                      << " " << node.data.child[l];
          } else {
            for (int l = 0; l < _bucket; ++l)
              ostring << " " << node.leaves[l];
          }
        }
        os << ostring.str();
      }
    }

    /**
     * Read the object from an I/O stream.
     *
     * @param[in,out] is the stream to read from
     * @param[in] bin if true (the default) load in binary mode.
     * @exception GeographicErr if the state read from \e is is illegal.
     * @exception std::bad_alloc if memory for the tree can't be allocated.
     *
     * The counters tracking the statistics of searches are reset by this
     * operation.  Binary data must have been saved on a machine with the same
     * architecture.  If an exception is thrown, the state of the
     * NearestNeighbor is unchanged.
     *
     * \note
     * Boost serialization can also be used to save and restore a
     * NearestNeighbor object.  This requires that the
     * GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION macro be defined.
     *
     * \warning The same arguments \e pts and \e dist used for
     * initialization must be provided to the Search() function.
     **********************************************************************/
    void Load(std::istream& is, bool bin = true) {
      int version1, realspec, bucket, numpoints, treesize, cost;
      if (bin) {
        char id[17];
        is.read(id, 16);
        id[16] = '\0';
        if (!(std::strcmp(id, "NearestNeighbor_") == 0))
          throw GeographicLib::GeographicErr("Bad ID");
        is.read(reinterpret_cast<char *>(&version1), sizeof(int));
        is.read(reinterpret_cast<char *>(&realspec), sizeof(int));
        is.read(reinterpret_cast<char *>(&bucket), sizeof(int));
        is.read(reinterpret_cast<char *>(&numpoints), sizeof(int));
        is.read(reinterpret_cast<char *>(&treesize), sizeof(int));
        is.read(reinterpret_cast<char *>(&cost), sizeof(int));
      } else {
        if (!( is >> version1 >> realspec >> bucket >> numpoints >> treesize
               >> cost ))
          throw GeographicLib::GeographicErr("Bad header");
      }
      if (!( version1 == version ))
        throw GeographicLib::GeographicErr("Incompatible version");
      if (!( realspec == std::numeric_limits<dist_t>::digits *
             (std::numeric_limits<dist_t>::is_integer ? -1 : 1) ))
        throw GeographicLib::GeographicErr("Different dist_t types");
      if (!( 0 <= bucket && bucket <= maxbucket ))
        throw GeographicLib::GeographicErr("Bad bucket size");
      if (!( 0 <= treesize && treesize <= numpoints ))
        throw
          GeographicLib::GeographicErr("Bad number of points or tree size");
      if (!( 0 <= cost ))
        throw GeographicLib::GeographicErr("Bad value for cost");
      std::vector<Node> tree;
      tree.reserve(treesize);
      for (int i = 0; i < treesize; ++i) {
        Node node;
        if (bin) {
          is.read(reinterpret_cast<char *>(&node.index), sizeof(int));
          if (node.index >= 0) {
            is.read(reinterpret_cast<char *>(node.data.lower),
                    2 * sizeof(dist_t));
            is.read(reinterpret_cast<char *>(node.data.upper),
                    2 * sizeof(dist_t));
            is.read(reinterpret_cast<char *>(node.data.child),
                    2 * sizeof(int));
          } else {
            is.read(reinterpret_cast<char *>(node.leaves),
                    bucket * sizeof(int));
            for (int l = bucket; l < maxbucket; ++l)
              node.leaves[l] = 0;
          }
        } else {
          if (!( is >> node.index ))
            throw GeographicLib::GeographicErr("Bad index");
          if (node.index >= 0) {
            for (int l = 0; l < 2; ++l) {
              if (!( is >> node.data.lower[l] >> node.data.upper[l]
                     >> node.data.child[l] ))
                throw GeographicLib::GeographicErr("Bad node data");
            }
          } else {
            // Must be at least one valid leaf followed by a sequence end
            // markers (-1).
            for (int l = 0; l < bucket; ++l) {
              if (!( is >> node.leaves[l] ))
                throw GeographicLib::GeographicErr("Bad leaf data");
            }
            for (int l = bucket; l < maxbucket; ++l)
              node.leaves[l] = 0;
          }
        }
        node.Check(numpoints, treesize, bucket);
        tree.push_back(node);
      }
      _tree.swap(tree);
      _numpoints = numpoints;
      _bucket = bucket;
      _mc = _sc = 0;
      _cost = cost; _c1 = _k = _cmax = 0;
      _cmin = std::numeric_limits<int>::max();
    }

    /**
     * Write the object to stream \e os as text.
     *
     * @param[in,out] os the output stream.
     * @param[in] t the NearestNeighbor object to be saved.
     * @exception std::bad_alloc if memory for the string representation of the
     *   object can't be allocated.
     **********************************************************************/
    friend std::ostream& operator<<(std::ostream& os, const NearestNeighbor& t)
    { t.Save(os, false); return os; }

    /**
     * Read the object from stream \e is as text.
     *
     * @param[in,out] is the input stream.
     * @param[out] t the NearestNeighbor object to be loaded.
     * @exception GeographicErr if the state read from \e is is illegal.
     * @exception std::bad_alloc if memory for the tree can't be allocated.
     **********************************************************************/
    friend std::istream& operator>>(std::istream& is, NearestNeighbor& t)
    { t.Load(is, false); return is; }

    /**
     * Swap with another NearestNeighbor object.
     *
     * @param[in,out] t the NearestNeighbor object to swap with.
     **********************************************************************/
    void swap(NearestNeighbor& t) {
      std::swap(_numpoints, t._numpoints);
      std::swap(_bucket, t._bucket);
      std::swap(_cost, t._cost);
      _tree.swap(t._tree);
      std::swap(_mc, t._mc);
      std::swap(_sc, t._sc);
      std::swap(_c1, t._c1);
      std::swap(_k, t._k);
      std::swap(_cmin, t._cmin);
      std::swap(_cmax, t._cmax);
    }

    /**
     * The accumulated statistics on the searches so far.
     *
     * @param[out] setupcost the cost of initializing the NearestNeighbor.
     * @param[out] numsearches the number of calls to Search().
     * @param[out] searchcost the total cost of the calls to Search().
     * @param[out] mincost the minimum cost of a Search().
     * @param[out] maxcost the maximum cost of a Search().
     * @param[out] mean the mean cost of a Search().
     * @param[out] sd the standard deviation in the cost of a Search().
     *
     * Here "cost" measures the number of distance calculations needed.  Note
     * that the accumulation of statistics is \e not thread safe.
     **********************************************************************/
    void Statistics(int& setupcost, int& numsearches, int& searchcost,
                    int& mincost, int& maxcost,
                    double& mean, double& sd) const {
      setupcost = _cost; numsearches = _k; searchcost = _c1;
      mincost = _cmin; maxcost = _cmax;
      mean = _mc; sd = std::sqrt(_sc / (_k - 1));
    }

    /**
     * Reset the counters for the accumulated statistics on the searches so
     * far.
     **********************************************************************/
    void ResetStatistics() const {
      _mc = _sc = 0;
      _c1 = _k = _cmax = 0;
      _cmin = std::numeric_limits<int>::max();
    }

  private:
    // Package up a dist_t and an int.  We will want to sort on the dist_t so
    // put it first.
    typedef std::pair<dist_t, int> item;
    // \cond SKIP
    class Node {
    public:
      struct bounds {
        dist_t lower[2], upper[2]; // bounds on inner/outer distances
        int child[2];
      };
      union {
        bounds data;
        int leaves[maxbucket];
      };
      int index;

      Node()
        : index(-1)
      {
        for (int i = 0; i < 2; ++i) {
          data.lower[i] = data.upper[i] = 0;
          data.child[i] = -1;
        }
      }

      // Sanity check on a Node
      void Check(int numpoints, int treesize, int bucket) const {
        if (!( -1 <= index && index < numpoints ))
          throw GeographicLib::GeographicErr("Bad index");
        if (index >= 0) {
          if (!( -1 <= data.child[0] && data.child[0] < treesize &&
                 -1 <= data.child[1] && data.child[1] < treesize ))
            throw GeographicLib::GeographicErr("Bad child pointers");
          if (!( 0 <= data.lower[0] && data.lower[0] <= data.upper[0] &&
                 data.upper[0] <= data.lower[1] &&
                 data.lower[1] <= data.upper[1] ))
            throw GeographicLib::GeographicErr("Bad bounds");
        } else {
          // Must be at least one valid leaf followed by a sequence end markers
          // (-1).
          bool start = true;
          for (int l = 0; l < bucket; ++l) {
            if (!( (start ?
                    ((l == 0 ? 0 : -1) <= leaves[l] && leaves[l] < numpoints) :
                    leaves[l] == -1) ))
              throw GeographicLib::GeographicErr("Bad leaf data");
            start = leaves[l] >= 0;
          }
          for (int l = bucket; l < maxbucket; ++l) {
            if (leaves[l] != 0)
              throw GeographicLib::GeographicErr("Bad leaf data");
          }
        }
      }

#if defined(GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION) && \
  GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
      friend class boost::serialization::access;
      template<class Archive>
      void save(Archive& ar, const unsigned int) const {
        ar & boost::serialization::make_nvp("index", index);
        if (index < 0)
          ar & boost::serialization::make_nvp("leaves", leaves);
        else
          ar & boost::serialization::make_nvp("lower", data.lower)
            & boost::serialization::make_nvp("upper", data.upper)
            & boost::serialization::make_nvp("child", data.child);
      }
      template<class Archive>
      void load(Archive& ar, const unsigned int) {
        ar & boost::serialization::make_nvp("index", index);
        if (index < 0)
          ar & boost::serialization::make_nvp("leaves", leaves);
        else
          ar & boost::serialization::make_nvp("lower", data.lower)
            & boost::serialization::make_nvp("upper", data.upper)
            & boost::serialization::make_nvp("child", data.child);
      }
      template<class Archive>
      void serialize(Archive& ar, const unsigned int file_version)
      { boost::serialization::split_member(ar, *this, file_version); }
#endif
    };
    // \endcond
#if defined(GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION) && \
  GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
    friend class boost::serialization::access;
    template<class Archive> void save(Archive& ar, const unsigned) const {
      int realspec = std::numeric_limits<dist_t>::digits *
        (std::numeric_limits<dist_t>::is_integer ? -1 : 1);
      // Need to use version1, otherwise load error in debug mode on Linux:
      // undefined reference to GeographicLib::NearestNeighbor<...>::version.
      int version1 = version;
      ar & boost::serialization::make_nvp("version", version1)
        & boost::serialization::make_nvp("realspec", realspec)
        & boost::serialization::make_nvp("bucket", _bucket)
        & boost::serialization::make_nvp("numpoints", _numpoints)
        & boost::serialization::make_nvp("cost", _cost)
        & boost::serialization::make_nvp("tree", _tree);
    }
    template<class Archive> void load(Archive& ar, const unsigned) {
      int version1, realspec, bucket, numpoints, cost;
      ar & boost::serialization::make_nvp("version", version1);
      if (version1 != version)
        throw GeographicLib::GeographicErr("Incompatible version");
      std::vector<Node> tree;
      ar & boost::serialization::make_nvp("realspec", realspec);
      if (!( realspec == std::numeric_limits<dist_t>::digits *
             (std::numeric_limits<dist_t>::is_integer ? -1 : 1) ))
        throw GeographicLib::GeographicErr("Different dist_t types");
      ar & boost::serialization::make_nvp("bucket", bucket);
      if (!( 0 <= bucket && bucket <= maxbucket ))
        throw GeographicLib::GeographicErr("Bad bucket size");
      ar & boost::serialization::make_nvp("numpoints", numpoints)
        & boost::serialization::make_nvp("cost", cost)
        & boost::serialization::make_nvp("tree", tree);
      if (!( 0 <= int(tree.size()) && int(tree.size()) <= numpoints ))
        throw
          GeographicLib::GeographicErr("Bad number of points or tree size");
      for (int i = 0; i < int(tree.size()); ++i)
        tree[i].Check(numpoints, int(tree.size()), bucket);
      _tree.swap(tree);
      _numpoints = numpoints;
      _bucket = bucket;
      _mc = _sc = 0;
      _cost = cost; _c1 = _k = _cmax = 0;
      _cmin = std::numeric_limits<int>::max();
    }
    template<class Archive>
    void serialize(Archive& ar, const unsigned int file_version)
    { boost::serialization::split_member(ar, *this, file_version); }
#endif

    int _numpoints, _bucket, _cost;
    std::vector<Node> _tree;
    // Counters to track stastistics on the cost of searches
    mutable double _mc, _sc;
    mutable int _c1, _k, _cmin, _cmax;

    int init(const std::vector<pos_t>& pts, const distfun_t& dist, int bucket,
             std::vector<Node>& tree, std::vector<item>& ids, int& cost,
             int l, int u, int vp) {

      if (u == l)
        return -1;
      Node node;

      if (u - l > (bucket == 0 ? 1 : bucket)) {

        // choose a vantage point and move it to the start
        int i = vp;
        std::swap(ids[l], ids[i]);

        int m = (u + l + 1) / 2;

        for (int k = l + 1; k  l + 1) {        // node.child[0] is possibly empty
          typename std::vector<item>::iterator
            t = std::min_element(ids.begin() + l + 1, ids.begin() + m);
          node.data.lower[0] = t->first;
          t = std::max_element(ids.begin() + l + 1, ids.begin() + m);
          node.data.upper[0] = t->first;
          // Use point with max distance as vantage point; this point act as a
          // "corner" point and leads to a good partition.
          node.data.child[0] = init(pts, dist, bucket, tree, ids, cost,
                                    l + 1, m, int(t - ids.begin()));
        }
        typename std::vector<item>::iterator
          t = std::max_element(ids.begin() + m, ids.begin() + u);
        node.data.lower[1] = ids[m].first;
        node.data.upper[1] = t->first;
        // Use point with max distance as vantage point here too
        node.data.child[1] = init(pts, dist, bucket, tree, ids, cost,
                                  m, u, int(t - ids.begin()));
      } else {
        if (bucket == 0)
          node.index = ids[l].second;
        else {
          node.index = -1;
          // Sort the bucket entries so that the tree is independent of the
          // implementation of nth_element.
          std::sort(ids.begin() + l, ids.begin() + u);
          for (int i = l; i < u; ++i)
            node.leaves[i-l] = ids[i].second;
          for (int i = u - l; i < bucket; ++i)
            node.leaves[i] = -1;
          for (int i = bucket; i < maxbucket; ++i)
            node.leaves[i] = 0;
        }
      }

      tree.push_back(node);
      return int(tree.size()) - 1;
    }

  };

} // namespace GeographicLib

namespace std {

  /**
   * Swap two GeographicLib::NearestNeighbor objects.
   *
   * @tparam dist_t the type used for measuring distances.
   * @tparam pos_t the type for specifying the positions of points.
   * @tparam distfun_t the type for a function object which calculates
   *   distances between points.
   * @param[in,out] a the first GeographicLib::NearestNeighbor to swap.
   * @param[in,out] b the second GeographicLib::NearestNeighbor to swap.
   **********************************************************************/
  template <typename dist_t, typename pos_t, class distfun_t>
  void swap(GeographicLib::NearestNeighbor<dist_t, pos_t, distfun_t>& a,
            GeographicLib::NearestNeighbor<dist_t, pos_t, distfun_t>& b) {
    a.swap(b);
  }

} // namespace std

#if defined(_MSC_VER)
#  pragma warning (pop)
#endif

#endif  // GEOGRAPHICLIB_NEARESTNEIGHBOR_HPP


// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Copyright (c) 2006 Johan Rade
// Copyright (c) 2011 Paul A. Bristow

/*!
\file
\brief Tests of nonfinite signaling NaN loopback.

\detail  nonfinite signaling NaN
test outputs using nonfinite facets
(output and input) and reads back in, and checks if loopback OK.

Not expected to work on all platforms (if any).  But shows that on MSVC,
this legacy locale can ensure a consistent quiet NaN input from representations
"1.#QNAN", "1.#SNAN" and "1.#IND"

*/

#ifdef _MSC_VER
#   pragma warning(disable : 4702)
#endif

#include <boost/math/special_functions/nonfinite_num_facets.hpp>
using boost::math::nonfinite_num_get;
using boost::math::nonfinite_num_put;

#include <iostream>
using std::cout;
using std::endl;

#include <locale>
using std::locale;

#include <string>
using std::string;

#include <sstream>
  using std::stringstream;
  using std::istringstream;

#include <limits>
using std::numeric_limits;

int main()
{
    if((std::numeric_limits<double>::has_infinity == false) || (std::numeric_limits<double>::infinity() == 0))
  {
    std::cout << "Infinity not supported on this platform." << std::endl;
    return 0;
  }

  if((std::numeric_limits<double>::has_quiet_NaN == false) || (std::numeric_limits<double>::quiet_NaN() == 0))
  {
    std::cout << "NaN not supported on this platform." << std::endl;
    return 0;
  }

  locale default_locale; // Current global locale.
  // Try to use the default locale first.
  // On MSVC this doesn't work.

  { // Try Quiet NaN
    stringstream ss; // Both input and output.
    ss.imbue(default_locale); // Redundant, of course.
    string infs;
    if(numeric_limits<double>::has_quiet_NaN)
    {  // Make sure quiet NaN is specialised for type double.
      double qnan = numeric_limits<double>::quiet_NaN();
      ss << qnan; // Output quiet_NaN.
      infs = ss.str();  //
    }
    else
    { // Need to provide a suitable string for quiet NaN.
     infs =  "1.#QNAN";
      ss << infs;
    }
    double r;
    ss >> r; // Read back in.

    cout << "quiet_NaN output was " << infs << endl; // "1.#QNAN"
    cout << "quiet_NaN input was " << r << endl; // "1"
  }

#if (!defined __BORLANDC__ && !defined __CODEGEARC__)
  // These compilers trap when trying to create a signaling_NaN!
  { // Try Signaling NaN
    stringstream ss; // Both input and output.
    ss.imbue(default_locale); // Redundant, of course.
    string infs;
    if(numeric_limits<double>::has_signaling_NaN)
    {  // Make sure signaling NaN is specialised for type double.
      double qnan = numeric_limits<double>::signaling_NaN();
      ss << qnan; // Output signaling_NaN.
      infs = ss.str();  //
    }
    else
    { // Need to provide a suitable string for signaling NaN.
     infs =  "1.#SNAN";
      ss << infs;
    }
    double r;
    ss >> r; // Read back in.

    cout << "signaling_NaN output was " << infs << endl; // "1.#QNAN" (or "1.#SNAN"?)
    cout << "signaling_NaN input was " << r << endl; // "1"
  }
#endif // Not Borland or CodeGear.

  // Create legacy_locale and store the nonfinite_num_get facet (with legacy flag) in it.
  locale legacy_locale(default_locale, new nonfinite_num_get<char>(boost::math::legacy));
  // Note that the legacy flag has no effect on the nonfinite_num_put output facet.

  cout << "Use legacy locale." << endl;

  { // Try infinity.
    stringstream ss; // Both input and output.
    ss.imbue(legacy_locale);
    string infs;
    if(numeric_limits<double>::has_infinity)
    {  // Make sure infinity is specialised for type double.
      double inf = numeric_limits<double>::infinity();
      ss << inf; // Output infinity.
      infs = ss.str();  //
    }
    else
    { // Need to provide a suitable string for infinity.
     infs =  "1.#INF";
      ss << infs;
    }
    double r;
    ss >> r; // Read back in.

    cout << "infinity output was " << infs << endl; // "1.#INF"
    cout << "infinity input was " << r << endl; // "1.#INF"
  }

  { // Try input of "1.#SNAN".
    //double inf = numeric_limits<double>::signaling_NaN(); // Assigns "1.#QNAN" on MSVC.
    // So must use explicit string "1.#SNAN" instead.
    stringstream ss; // Both input and output.
    ss.imbue(legacy_locale);
    string s = "1.#SNAN";

    ss << s; // Write out.
    double r;

    ss >> r; // Read back in.

    cout << "SNAN output was " << s << endl; // "1.#SNAN"
    cout << "SNAN input was " << r << endl;  // "1.#QNAN"
  }

  { // Try input of "1.#IND" .
    stringstream ss; // Both input and output.
    ss.imbue(legacy_locale);
    string s = "1.#IND";
    ss << s; // Write out.
    double r;
    ss >> r; // Read back in.

    cout << "IND output was " << s << endl; // "1.#IND"
    cout << "IND input was " << r << endl;  // "1.#QNAN"
  }

} // int main()

/*

Output:
  nonfinite_signaling_NaN.vcxproj -> J:\Cpp\fp_facet\fp_facet\Debug\nonfinite_signaling_NaN.exe

  quiet_NaN output was 1.#QNAN
  quiet_NaN input was 1
  signaling_NaN output was 1.#QNAN
  signaling_NaN input was 1
  Use legacy locale.
  infinity output was 1.#INF
  infinity input was 1.#INF
  SNAN output was 1.#SNAN
  SNAN input was 1.#QNAN
  IND output was 1.#IND
  IND input was 1.#QNAN

*/


#define CGAL_data_type float
#define CGAL_GL_data_type GL_FLOAT
#include "Scene_points_with_normal_item.h"
#include <CGAL/Polygon_mesh_processing/compute_normal.h>

#include <CGAL/Point_set_3/IO.h>
#include <CGAL/Timer.h>
#include <CGAL/Memory_sizer.h>
#include <CGAL/compute_average_spacing.h>

#include <CGAL/Three/Viewer_interface.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>
#include <CGAL/linear_least_squares_fitting_3.h>

#include <QObject>
#include <QApplication>
#include <QMenu>
#include <QSlider>
#include <QWidgetAction>
#include <QGLViewer/manipulatedCameraFrame.h>

#include <set>
#include <stack>
#include <algorithm>
#include <boost/array.hpp>

#include <CGAL/boost/graph/properties_Surface_mesh.h>
#include "Polyhedron_type.h"

const std::size_t limit_fast_drawing = 300000; //arbitraty large value

struct Scene_points_with_normal_item_priv
{
  void init_values(Scene_points_with_normal_item* parent)
  {
    item = parent;
    nb_points = 0;
    nb_selected_points = 0;
    nb_lines = 0;
    is_point_slider_moving = false;
    normal_Slider = new QSlider(Qt::Horizontal);
    normal_Slider->setValue(20);
    point_Slider = new QSlider(Qt::Horizontal);
    point_Slider->setValue(2);
    point_Slider->setMinimum(1);
    point_Slider->setMaximum(25);
  }
  Scene_points_with_normal_item_priv(Scene_points_with_normal_item* parent)
    :m_points(new Point_set)
  {
    init_values(parent);
  }
  Scene_points_with_normal_item_priv(const Scene_points_with_normal_item& toCopy, Scene_points_with_normal_item* parent)
    : m_points(new Point_set(*toCopy.d->m_points))
  {
    init_values(parent);
  }

  Scene_points_with_normal_item_priv(const SMesh& input_mesh, Scene_points_with_normal_item* parent)
    : m_points(new Point_set)
  {
   init_values(parent);
   boost::graph_traits<SMesh>::vertex_iterator v;
    m_points->add_normal_map();
    for (v = const_cast<SMesh&>(input_mesh).vertices_begin();
         v != const_cast<SMesh&>(input_mesh).vertices_end(); v++)
    {
      boost::graph_traits<SMesh>::vertex_descriptor vd(*v);
      const Kernel::Point_3& p = input_mesh.point(vd);
      Kernel::Vector_3 n =
        CGAL::Polygon_mesh_processing::compute_vertex_normal(vd, input_mesh);
      m_points->insert(p,n);
    }
  }

  Scene_points_with_normal_item_priv(const Polyhedron& input_mesh, Scene_points_with_normal_item* parent)
     : m_points(new Point_set)
   {
    init_values(parent);
     Polyhedron::Vertex_iterator v;
     m_points->add_normal_map();
     for (v = const_cast<Polyhedron&>(input_mesh).vertices_begin();
          v != const_cast<Polyhedron&>(input_mesh).vertices_end(); v++)
     {
       const Kernel::Point_3& p = v->point();
       Kernel::Vector_3 n =
         CGAL::Polygon_mesh_processing::compute_vertex_normal(v, input_mesh);
       m_points->insert(p,n);
     }
   }

  ~Scene_points_with_normal_item_priv()
  {
    if(m_points)
    {
      delete m_points;
      m_points = NULL;
    }
    delete normal_Slider;
    delete point_Slider;
  }
  bool isPointSliderMoving() { return is_point_slider_moving; }
  void initializeBuffers(CGAL::Three::Viewer_interface *viewer) const;
  void compute_normals_and_vertices() const;
  enum VAOs {
      Edges=0,
      ThePoints,
      TheShadedPoints,
      Selected_points,
      Selected_shaded_points,
      NbOfVaos
  };
  enum VBOs {
      Edges_vertices = 0,
      Points_vertices,
      Points_normals,
      Points_colors,
      Selected_points_vertices,
      Selected_points_normals,
      NbOfVbos
  };
  Point_set* m_points;
  QAction* actionDeleteSelection;
  QAction* actionResetSelection;
  QAction* actionSelectDuplicatedPoints;
  QSlider* normal_Slider;
  QSlider* point_Slider;
  mutable bool is_point_slider_moving;
  mutable std::vector<CGAL_data_type> positions_lines;
  mutable std::vector<CGAL_data_type> normals;
  mutable std::vector<CGAL_data_type> positions_normals;
  mutable std::vector<CGAL_data_type> positions_selected_normals;
  mutable std::vector<CGAL_data_type> colors_points;
  mutable std::size_t nb_points;
  mutable std::size_t nb_selected_points;
  mutable std::size_t nb_lines;
  mutable QOpenGLShaderProgram *program;

  Scene_points_with_normal_item* item;
};

Scene_points_with_normal_item::Scene_points_with_normal_item()
    : Scene_item(Scene_points_with_normal_item_priv::NbOfVbos,Scene_points_with_normal_item_priv::NbOfVaos)
{
    setRenderingMode(Points);
    is_selected = true;
    d = new Scene_points_with_normal_item_priv(this);
}

// Copy constructor
Scene_points_with_normal_item::Scene_points_with_normal_item(const Scene_points_with_normal_item& toCopy)
    :Scene_item(Scene_points_with_normal_item_priv::NbOfVbos,Scene_points_with_normal_item_priv::NbOfVaos)
{

  d = new Scene_points_with_normal_item_priv(toCopy, this);
  if (has_normals())
    {
        setRenderingMode(PointsPlusNormals);
        is_selected = true;
    }
  else
    {
        setRenderingMode(Points);
        is_selected = true;
    }

    invalidateOpenGLBuffers();
}

// Converts polyhedron to point set

Scene_points_with_normal_item::Scene_points_with_normal_item(const SMesh& input_mesh)
    : Scene_item(Scene_points_with_normal_item_priv::NbOfVbos,Scene_points_with_normal_item_priv::NbOfVaos)
{
  // Converts Polyhedron vertices to point set.
  // Computes vertices normal from connectivity.
  d = new Scene_points_with_normal_item_priv(input_mesh, this);
  setRenderingMode(PointsPlusNormals);
  is_selected = true;
  invalidateOpenGLBuffers();
}

Scene_points_with_normal_item::Scene_points_with_normal_item(const Polyhedron& input_mesh)
    : Scene_item(Scene_points_with_normal_item_priv::NbOfVbos,Scene_points_with_normal_item_priv::NbOfVaos)
{
  // Converts Polyhedron vertices to point set.
  // Computes vertices normal from connectivity.
  d = new Scene_points_with_normal_item_priv(input_mesh, this);
  setRenderingMode(PointsPlusNormals);
  is_selected = true;
  invalidateOpenGLBuffers();
}

Scene_points_with_normal_item::~Scene_points_with_normal_item()
{
  delete d;
}

void Scene_points_with_normal_item_priv::initializeBuffers(CGAL::Three::Viewer_interface *viewer) const
{
    compute_normals_and_vertices();
    //vao for the edges
    if(item->has_normals())
    {
        program = item->getShaderProgram(Scene_points_with_normal_item::PROGRAM_NO_SELECTION, viewer);
        program->bind();

        item->vaos[Edges]->bind();
        item->buffers[Edges_vertices].bind();
        item->buffers[Edges_vertices].allocate(positions_lines.data(),
                            static_cast<int>(positions_lines.size()*sizeof(CGAL_data_type)));
        program->enableAttributeArray("vertex");
        program->setAttributeBuffer("vertex",CGAL_GL_data_type,0,3);
        item->buffers[Edges_vertices].release();

        if (!(colors_points.empty()))
        {
          item->buffers[Points_colors].bind();
          item->buffers[Points_colors].allocate (colors_points.data(),
                                                 static_cast<int>(colors_points.size()*sizeof(CGAL_data_type)));
          program->enableAttributeArray("colors");
          program->setAttributeBuffer("colors",CGAL_GL_data_type,0,3);
          item->buffers[Points_colors].release();
        }
        else
          program->disableAttributeArray("colors");

        item->vaos[Edges]->release();

        nb_lines = positions_lines.size();
        program->release();
    }
    //vao for the points
    {
        program = item->getShaderProgram(Scene_points_with_normal_item::PROGRAM_NO_SELECTION, viewer);
        program->bind();

        item->vaos[ThePoints]->bind();
        item->buffers[Edges_vertices].bind();
        if(!item->has_normals()) {
          item->buffers[Edges_vertices].allocate(positions_lines.data(),
                              static_cast<int>(positions_lines.size()*sizeof(CGAL_data_type)));
        }
        program->enableAttributeArray("vertex");
        if(item->has_normals())
          program->setAttributeBuffer("vertex",CGAL_GL_data_type,0,3,
                                      static_cast<int>(6*sizeof(CGAL_GL_data_type)));
        else
          program->setAttributeBuffer("vertex",CGAL_GL_data_type,0,3);
        item->buffers[Edges_vertices].release();
        if (!(colors_points.empty()))
        {
          item->buffers[Points_colors].bind();
          item->buffers[Points_colors].allocate (colors_points.data(),
                                                 static_cast<int>(colors_points.size()*sizeof(CGAL_data_type)));
          program->enableAttributeArray("colors");
          program->setAttributeBuffer("colors",CGAL_GL_data_type,0,3,6*sizeof(CGAL_data_type));
          item->buffers[Points_colors].release();
          colors_points.resize(0);
          std::vector<CGAL_data_type>(colors_points).swap(colors_points);
        }
        else
          program->disableAttributeArray("colors");
        item->vaos[ThePoints]->release();
        program->release();

        if(item->has_normals())
        {
          program = item->getShaderProgram(Scene_points_with_normal_item::PROGRAM_WITH_LIGHT, viewer);
          program->bind();
          item->vaos[TheShadedPoints]->bind();
          item->buffers[Edges_vertices].bind();
          program->enableAttributeArray("vertex");
          program->setAttributeBuffer("vertex",CGAL_GL_data_type,0,3,
                                      static_cast<int>(6*sizeof(CGAL_GL_data_type)));
          item->buffers[Edges_vertices].release();
          item->buffers[Points_normals].bind();
          item->buffers[Points_normals].allocate(positions_normals.data(),
                                                 static_cast<int>(positions_normals.size()*sizeof(CGAL_data_type)));
          program->enableAttributeArray("normals");
          program->setAttributeBuffer("normals",CGAL_GL_data_type,0,3);
          item->buffers[Points_normals].release();
          positions_normals.resize(0);
          std::vector<CGAL_data_type>(positions_normals).swap(positions_normals);
          item->vaos[TheShadedPoints]->release();
          program->release();
        }

        if(item->has_normals())
          nb_points = positions_lines.size()/2;
        else
          nb_points = positions_lines.size();
    }
    //vao for the selected points
    {
      program = item->getShaderProgram(Scene_points_with_normal_item::PROGRAM_NO_SELECTION, viewer);
      program->bind();

      item->vaos[Selected_points]->bind();
      item->buffers[Edges_vertices].bind();
      program->enableAttributeArray("vertex");
      if(!item->has_normals())
      {
        program->setAttributeBuffer("vertex",CGAL_GL_data_type,
                                    static_cast<int>( 3*(m_points->size()-m_points->nb_selected_points())*sizeof(CGAL_data_type) ),
                                    3,
                                    0);
      }
      else
      {
        program->setAttributeBuffer("vertex",CGAL_GL_data_type,
                                    static_cast<int>( 6*(m_points->size()-m_points->nb_selected_points())*sizeof(CGAL_data_type) ),
                                    3,
                                    static_cast<int>(6*sizeof(CGAL_GL_data_type)));
      }
      item->buffers[Edges_vertices].release();
      program->disableAttributeArray("colors");
      item->vaos[Selected_points]->release();
      program->release();
      if(item->has_normals())
      {
        program = item->getShaderProgram(Scene_points_with_normal_item::PROGRAM_WITH_LIGHT, viewer);
        program->bind();
        item->vaos[Selected_shaded_points]->bind();
        item->buffers[Edges_vertices].bind();
        program->enableAttributeArray("vertex");
        program->setAttributeBuffer("vertex",CGAL_GL_data_type,
                                    static_cast<int>( 6*(m_points->size()-m_points->nb_selected_points())*sizeof(CGAL_data_type) ),
                                    3,
                                    static_cast<int>(6*sizeof(CGAL_GL_data_type)));

        item->buffers[Edges_vertices].release();
        program->disableAttributeArray("colors");
        item->buffers[Selected_points_normals].bind();
        item->buffers[Selected_points_normals].allocate(positions_selected_normals.data(),
                                                        static_cast<int>(positions_selected_normals.size()*sizeof(CGAL_data_type)));
        program->enableAttributeArray("normals");
        program->setAttributeBuffer("normals",CGAL_GL_data_type,0,3);
        item->buffers[Selected_points_normals].release();
        positions_selected_normals.resize(0);
        std::vector<CGAL_data_type>(positions_selected_normals).swap(positions_selected_normals);

        item->vaos[Selected_shaded_points]->release();
        program->release();
      }
      nb_selected_points = 3*m_points->nb_selected_points();
    }
    positions_lines.resize(0);
    positions_lines.shrink_to_fit();
    item->are_buffers_filled = true;
}

void Scene_points_with_normal_item_priv::compute_normals_and_vertices() const
{
    const qglviewer::Vec offset = static_cast<CGAL::Three::Viewer_interface*>(QGLViewer::QGLViewerPool().first())->offset();
    QApplication::setOverrideCursor(Qt::WaitCursor);
    positions_lines.resize(0);
    normals.resize(0);
    positions_normals.resize(0);
    positions_selected_normals.resize(0);
    normals.resize(0);
    colors_points.resize(0);

    //Shuffle container to allow quick display random points
    std::random_shuffle (m_points->begin(), m_points->first_selected());
    if (m_points->nb_selected_points() != 0)
      std::random_shuffle (m_points->first_selected(), m_points->end());
    //if item has normals, points will be one point out of two in the lines data.
    //else points will be lines and lines discarded.
    double average_spacing = 0;
    double normal_length =0;
    double length_factor =0;
    if (item->has_normals())
    {

#ifdef LINK_WITH_TBB
      typedef CGAL::Parallel_tag Concurrency_tag;
#else
      typedef CGAL::Sequential_tag Concurrency_tag;
#endif
    // Store normals
    Kernel::Sphere_3 region_of_interest = m_points->region_of_interest();
      positions_lines.reserve(m_points->size() * 6);
      positions_normals.reserve((m_points->size() - m_points->nb_selected_points()) * 3);
      positions_selected_normals.reserve(m_points->nb_selected_points() * 3);
      average_spacing = CGAL::compute_average_spacing<Concurrency_tag>(
            m_points->begin(), m_points->end(), m_points->point_map(),
            6);
      normal_length = (std::min)(average_spacing, std::sqrt(region_of_interest.squared_radius() / 1000.));
      length_factor = 5.0/100*normal_Slider->value();
    }
    else
    {
      positions_lines.reserve(m_points->size() * 3);
    }

    for (Point_set_3<Kernel>::const_iterator it = m_points->begin(); it != m_points->first_selected(); ++it)
    {
      const Kernel::Point_3& p = m_points->point(*it);
      positions_lines.push_back(p.x()+offset.x);
      positions_lines.push_back(p.y()+offset.y);
      positions_lines.push_back(p.z()+offset.z);
      if(item->has_normals())
      {
        const Kernel::Vector_3& n = m_points->normal(*it);
        Point_set_3<Kernel>::Point q = p + normal_length * length_factor* n;
        positions_lines.push_back(q.x()+offset.x);
        positions_lines.push_back(q.y()+offset.y);
        positions_lines.push_back(q.z()+offset.z);

        positions_normals.push_back(n.x());
        positions_normals.push_back(n.y());
        positions_normals.push_back(n.z());
      }
    }
    for (Point_set_3<Kernel>::const_iterator it = m_points->first_selected(); it != m_points->end(); ++it)
    {
      const Kernel::Point_3& p = m_points->point(*it);
      positions_lines.push_back(p.x()+offset.x);
      positions_lines.push_back(p.y()+offset.y);
      positions_lines.push_back(p.z()+offset.z);
      if(item->has_normals())
      {
        const Kernel::Vector_3& n = m_points->normal(*it);
        Point_set_3<Kernel>::Point q = p + normal_length * length_factor* n;
        positions_lines.push_back(q.x()+offset.x);
        positions_lines.push_back(q.y()+offset.y);
        positions_lines.push_back(q.z()+offset.z);

        positions_selected_normals.push_back(n.x());
        positions_selected_normals.push_back(n.y());
        positions_selected_normals.push_back(n.z());
      }
    }
    //The colors
    if (m_points->has_colors())
    {
        colors_points.reserve((m_points->size() - m_points->nb_selected_points()) * 6);

        for (Point_set_3<Kernel>::const_iterator it = m_points->begin(); it != m_points->end(); ++it)
	  {
            colors_points.push_back (m_points->red(*it));
            colors_points.push_back (m_points->green(*it));
            colors_points.push_back (m_points->blue(*it));
            colors_points.push_back (m_points->red(*it));
            colors_points.push_back (m_points->green(*it));
            colors_points.push_back (m_points->blue(*it));
	  }
    }

    QApplication::restoreOverrideCursor();
}

// Duplicates scene item
Scene_points_with_normal_item*
Scene_points_with_normal_item::clone() const
{
  return new Scene_points_with_normal_item(*this);
}

// Is selection empty?
bool Scene_points_with_normal_item::isSelectionEmpty() const
{
  return (d->m_points->nb_selected_points() == 0);
}

// Delete selection
void Scene_points_with_normal_item::deleteSelection()
{
  CGAL::Timer task_timer; task_timer.start();
  std::cerr << "Delete " << d->m_points->nb_selected_points() << " points...";

  // Delete selected points
  d->m_points->delete_selection();

  std::size_t memory = CGAL::Memory_sizer().virtual_size();
  std::cerr << "done: " << task_timer.time() << " seconds, "
                        << (memory>>20) << " Mb allocated"
                        << std::endl;
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}

// Invert selection
void Scene_points_with_normal_item::invertSelection()
{
  d->m_points->invert_selection();
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}

// Select everything
void Scene_points_with_normal_item::selectAll()
{
  d->m_points->select_all();
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}
// Reset selection mark
void Scene_points_with_normal_item::resetSelection()
{
  // Un-select all points
  d->m_points->unselect_all();
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}
  //Select duplicated points
void Scene_points_with_normal_item::selectDuplicates()
{
  std::set<Kernel::Point_3> unique_points;
  std::vector<Point_set::Index> unselected, selected;
  for (Point_set::iterator ptit = d->m_points->begin(); ptit!= d->m_points->end(); ++ ptit)
    if ( !unique_points.insert(d->m_points->point(*ptit)).second)
      selected.push_back (*ptit);
    else
      unselected.push_back (*ptit);

  for (std::size_t i = 0; i < unselected.size(); ++ i)
    *(d->m_points->begin() + i) = unselected[i];
  for (std::size_t i = 0; i < selected.size(); ++ i)
    *(d->m_points->begin() + (unselected.size() + i)) = selected[i];

  if (selected.empty ())
  {
    d->m_points->unselect_all();
  }
  else
  {
    d->m_points->set_first_selected
      (d->m_points->begin() + unselected.size());
  }

  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}

#if !defined(CGAL_CFG_NO_CPP0X_RVALUE_REFERENCE) && !defined(CGAL_CFG_NO_CPP0X_VARIADIC_TEMPLATES)
#ifdef CGAL_LINKED_WITH_LASLIB
// Loads point set from .LAS file
bool Scene_points_with_normal_item::read_las_point_set(std::istream& stream)
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->clear();

  bool ok = stream &&
    CGAL::read_las_point_set (stream, *(d->m_points)) &&
            !isEmpty();

  std::cerr << d->m_points->info();

  if (d->m_points->has_normal_map())
    setRenderingMode(PointsPlusNormals);
  if (d->m_points->check_colors())
    std::cerr << "-> Point set has colors" << std::endl;

  invalidateOpenGLBuffers();
  return ok;
}

// Write point set to .LAS file
bool Scene_points_with_normal_item::write_las_point_set(std::ostream& stream) const
{
  Q_ASSERT(d->m_points != NULL);

  return stream &&
    CGAL::write_las_point_set (stream, *(d->m_points));
}

#endif // LAS

// Loads point set from .PLY file
bool Scene_points_with_normal_item::read_ply_point_set(std::istream& stream)
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->clear();

  bool ok = stream &&
    CGAL::read_ply_point_set (stream, *(d->m_points)) &&
            !isEmpty();

  std::cerr << d->m_points->info();

  if (d->m_points->has_normal_map())
    setRenderingMode(PointsPlusNormals);
  if (d->m_points->check_colors())
    std::cerr << "-> Point set has colors" << std::endl;

  invalidateOpenGLBuffers();
  return ok;
}

// Write point set to .PLY file
bool Scene_points_with_normal_item::write_ply_point_set(std::ostream& stream, bool binary) const
{
  Q_ASSERT(d->m_points != NULL);

  if (!stream)
    return false;

  if (binary)
    CGAL::set_binary_mode (stream);
  stream << *(d->m_points);

  return true;
}

#endif // CXX11

// Loads point set from .OFF file
bool Scene_points_with_normal_item::read_off_point_set(std::istream& stream)
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->clear();
  bool ok = stream &&
    CGAL::read_off_point_set(stream, *(d->m_points)) &&
            !isEmpty();
  invalidateOpenGLBuffers();
  return ok;
}

// Write point set to .OFF file
bool Scene_points_with_normal_item::write_off_point_set(std::ostream& stream) const
{
  Q_ASSERT(d->m_points != NULL);

  return stream &&
    CGAL::write_off_point_set (stream, *(d->m_points));
}

// Loads point set from .XYZ file
bool Scene_points_with_normal_item::read_xyz_point_set(std::istream& stream)
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->clear();

  bool ok = stream &&
    CGAL::read_xyz_point_set (stream, *(d->m_points)) &&
    !isEmpty();

  invalidateOpenGLBuffers();
  return ok;
}

// Write point set to .XYZ file
bool Scene_points_with_normal_item::write_xyz_point_set(std::ostream& stream) const
{
  Q_ASSERT(d->m_points != NULL);

  return stream &&
    CGAL::write_xyz_point_set (stream, *(d->m_points));
}

QString
Scene_points_with_normal_item::toolTip() const
{
  Q_ASSERT(d->m_points != NULL);

  return QObject::tr("<p><b>%1</b> (color: %4)<br />"
                     "<i>Point_set_3</i></p>"
                     "<p>Number of points: %2</p>")
    .arg(name())
    .arg(d->m_points->size())
    .arg(color().name());
}

bool Scene_points_with_normal_item::supportsRenderingMode(RenderingMode m) const
{
  switch ( m )
  {
  case Points:
    return true;
  case ShadedPoints:
  case PointsPlusNormals:
  case Splatting:
    return has_normals();

  default:
    return false;
  }
}

void Scene_points_with_normal_item::drawSplats(CGAL::Three::Viewer_interface* viewer) const
{
  const qglviewer::Vec v_offset = static_cast<CGAL::Three::Viewer_interface*>(QGLViewer::QGLViewerPool().first())->offset();
 Kernel::Vector_3 offset(v_offset.x, v_offset.y, v_offset.z);

   // TODO add support for selection
   viewer->glBegin(GL_POINTS);
   if (d->m_points->has_colors())
     for ( Point_set_3<Kernel>::const_iterator it = d->m_points->begin(); it != d->m_points->end(); it++)
       {
         Point_set::Point p = d->m_points->point (*it) + offset;
         const Point_set::Vector& n = d->m_points->normal (*it);
         viewer->glColor4d(d->m_points->red(*it),
                           d->m_points->green(*it),
                           d->m_points->blue(*it),
                           1.0);
         viewer->glNormal3dv(&n.x());
         viewer->glMultiTexCoord1d(GL_TEXTURE2, d->m_points->radius(*it));
         viewer->glVertex3dv(&p.x());

       }
   else
     for ( Point_set_3<Kernel>::const_iterator it = d->m_points->begin(); it != d->m_points->end(); it++)
       {
         const Point_set::Point p = d->m_points->point (*it) + offset;
         const Point_set::Vector& n = d->m_points->normal (*it);
         viewer->glNormal3dv(&n.x());
         viewer->glMultiTexCoord1d(GL_TEXTURE2, d->m_points->radius(*it));
         viewer->glVertex3dv(&p.x());

       }

   viewer->glEnd();

}

void Scene_points_with_normal_item::drawEdges(CGAL::Three::Viewer_interface* viewer) const
{
    double ratio_displayed = 1.0;
    if (viewer->inFastDrawing () &&
        (d->nb_lines/6 > limit_fast_drawing)) // arbitrary large value
      ratio_displayed = 6 * limit_fast_drawing / (double)(d->nb_lines);

    if(!are_buffers_filled)
        d->initializeBuffers(viewer);
    vaos[Scene_points_with_normal_item_priv::Edges]->bind();
    d->program=getShaderProgram(PROGRAM_NO_SELECTION);
    attribBuffers(viewer,PROGRAM_NO_SELECTION);
    d->program->bind();
    d->program->setAttributeValue("colors", this->color());
    viewer->glDrawArrays(GL_LINES, 0,
                         static_cast<GLsizei>(((std::size_t)(ratio_displayed * d->nb_lines)/3)));
    vaos[Scene_points_with_normal_item_priv::Edges]->release();
    d->program->release();
}
void Scene_points_with_normal_item::drawPoints(CGAL::Three::Viewer_interface* viewer) const
{
    if(!are_buffers_filled)
        d->initializeBuffers(viewer);
    GLfloat point_size;
    viewer->glGetFloatv(GL_POINT_SIZE, &point_size);
    viewer->glPointSize(d->point_Slider->value());
    double ratio_displayed = 1.0;
    if ((viewer->inFastDrawing () || d->isPointSliderMoving())
        &&((d->nb_points )/3 > limit_fast_drawing)) // arbitrary large value
      ratio_displayed = 3 * limit_fast_drawing / (double)(d->nb_points);

    // POINTS
    if(has_normals() && renderingMode() == ShadedPoints)
    {
      vaos[Scene_points_with_normal_item_priv::TheShadedPoints]->bind();
      d->program=getShaderProgram(PROGRAM_WITH_LIGHT);
      attribBuffers(viewer,PROGRAM_WITH_LIGHT);
    }
    else
    {
      vaos[Scene_points_with_normal_item_priv::ThePoints]->bind();
      d->program=getShaderProgram(PROGRAM_NO_SELECTION);
      attribBuffers(viewer,PROGRAM_NO_SELECTION);
    }
    d->program->bind();
    if (!(d->m_points->has_colors()) || renderingMode() == ShadedPoints)
      d->program->setAttributeValue("colors", this->color());
    viewer->glDrawArrays(GL_POINTS, 0,
                         static_cast<GLsizei>(((std::size_t)(ratio_displayed * (d->nb_points - d->nb_selected_points))/3)));

    if(has_normals() && renderingMode() == ShadedPoints)
      vaos[Scene_points_with_normal_item_priv::TheShadedPoints]->release();
    else
      vaos[Scene_points_with_normal_item_priv::ThePoints]->release();
    d->program->release();

    // SELECTED POINTS
    if(has_normals() && renderingMode() == ShadedPoints)
    {
      vaos[Scene_points_with_normal_item_priv::Selected_shaded_points]->bind();
      d->program=getShaderProgram(PROGRAM_WITH_LIGHT);
      attribBuffers(viewer,PROGRAM_WITH_LIGHT);
    }
    else
    {
      vaos[Scene_points_with_normal_item_priv::Selected_points]->bind();
      d->program=getShaderProgram(PROGRAM_NO_SELECTION);
      attribBuffers(viewer,PROGRAM_NO_SELECTION);
    }
    d->program->bind();
    d->program->setAttributeValue("colors", QColor(Qt::red));
    viewer->glDrawArrays(GL_POINTS, 0,
                         static_cast<GLsizei>(((std::size_t)(ratio_displayed * d->nb_selected_points)/3)));

    if(has_normals() && renderingMode() == ShadedPoints)
      vaos[Scene_points_with_normal_item_priv::Selected_shaded_points]->release();
    else
      vaos[Scene_points_with_normal_item_priv::Selected_points]->release();
    d->program->release();
    viewer->glPointSize(point_size);
}
// Gets wrapped point set
Point_set* Scene_points_with_normal_item::point_set()
{
  Q_ASSERT(d->m_points != NULL);
  return d->m_points;
}
const Point_set* Scene_points_with_normal_item::point_set() const
{
  Q_ASSERT(d->m_points != NULL);
  return d->m_points;
}

bool
Scene_points_with_normal_item::isEmpty() const
{
  Q_ASSERT(d->m_points != NULL);
  return d->m_points->empty();
}

void
Scene_points_with_normal_item::compute_bbox() const
{
  Q_ASSERT(d->m_points != NULL);

  Kernel::Iso_cuboid_3 bbox = d->m_points->bounding_box();
  _bbox = Bbox(bbox.xmin(),bbox.ymin(),bbox.zmin(),
              bbox.xmax(),bbox.ymax(),bbox.zmax());
}

void Scene_points_with_normal_item::computes_local_spacing(int k)
{
  typedef Kernel Geom_traits;

  typedef CGAL::Search_traits_3<Geom_traits> SearchTraits_3;
  typedef CGAL::Search_traits_adapter <Point_set::Index, Point_set::Point_map, SearchTraits_3> Search_traits;
  typedef CGAL::Orthogonal_k_neighbor_search<Search_traits> Neighbor_search;
  typedef Neighbor_search::Tree Tree;
  typedef Neighbor_search::Distance Distance;

  // build kdtree
  Tree tree(d->m_points->begin(),
            d->m_points->end(),
            Tree::Splitter(),
            Search_traits (d->m_points->point_map())
            );
  Distance tr_dist(d->m_points->point_map());

  if (!(d->m_points->has_property_map<double> ("radius")))
    d->m_points->add_radius();

  // Compute the radius of each point = (distance max to k nearest neighbors)/2.
  {
    int i=0;
    for (Point_set::iterator it=d->m_points->begin(); it!=d->m_points->end(); ++it, ++i)
    {
      Neighbor_search search(tree, d->m_points->point(*it), k+1, 0, true, tr_dist);
      double maxdist2 = (--search.end())->second; // squared distance to furthest neighbor
      d->m_points->radius(*it) = sqrt(maxdist2)/2.;
    }
  }

  d->m_points->set_radii_uptodate(true);
}

QMenu* Scene_points_with_normal_item::contextMenu()
{
    const char* prop_name = "Menu modified by Scene_points_with_normal_item.";

    QMenu* menu = Scene_item::contextMenu();

    //add a slider to modify the normals length
    // Use dynamic properties:
    // http://doc.qt.io/qt-5/qobject.html#property
    bool menuChanged = menu->property(prop_name).toBool();

    if(!menuChanged) {
      if(has_normals())
      {
        QMenu *container = new QMenu(tr("Normals Length"));
        QWidgetAction *sliderAction = new QWidgetAction(0);
        if((d->nb_points)/3 <= limit_fast_drawing)
        {
          connect(d->normal_Slider, &QSlider::valueChanged, this, &Scene_points_with_normal_item::invalidateOpenGLBuffers);
          connect(d->normal_Slider, &QSlider::valueChanged, this, &Scene_points_with_normal_item::itemChanged);
        }
        else
        {
          connect(d->normal_Slider, &QSlider::sliderReleased, this, &Scene_points_with_normal_item::invalidateOpenGLBuffers);
          connect(d->normal_Slider, &QSlider::sliderReleased, this, &Scene_points_with_normal_item::itemChanged);
        }
        sliderAction->setDefaultWidget(d->normal_Slider);

        container->addAction(sliderAction);
        menu->addMenu(container);
      }
        QMenu *container = new QMenu(tr("Points Size"));
        QWidgetAction *sliderAction = new QWidgetAction(0);
        connect(d->point_Slider, &QSlider::sliderPressed, this, &Scene_points_with_normal_item::pointSliderPressed);
        connect(d->point_Slider, &QSlider::sliderReleased, this, &Scene_points_with_normal_item::pointSliderReleased);
        connect(d->point_Slider, &QSlider::valueChanged, this, &Scene_points_with_normal_item::itemChanged);

        sliderAction->setDefaultWidget(d->point_Slider);

        container->addAction(sliderAction);
        menu->addMenu(container);

        d->actionDeleteSelection = menu->addAction(tr("Delete Selection"));
        d->actionDeleteSelection->setObjectName("actionDeleteSelection");
        connect(d->actionDeleteSelection, SIGNAL(triggered()),this, SLOT(deleteSelection()));

        d->actionResetSelection = menu->addAction(tr("Reset Selection"));
        d->actionResetSelection->setObjectName("actionResetSelection");
        connect(d->actionResetSelection, SIGNAL(triggered()),this, SLOT(resetSelection()));

        d->actionSelectDuplicatedPoints = menu->addAction(tr("Select duplicated points"));
        d->actionSelectDuplicatedPoints->setObjectName("actionSelectDuplicatedPoints");
        connect(d->actionSelectDuplicatedPoints, SIGNAL(triggered()),this, SLOT(selectDuplicates()));

        menu->setProperty(prop_name, true);
    }

    if (isSelectionEmpty())
    {
        d->actionDeleteSelection->setDisabled(true);
        d->actionResetSelection->setDisabled(true);
    }
    else
    {
        d->actionDeleteSelection->setDisabled(false);
        d->actionResetSelection->setDisabled(false);
    }

    return menu;
}

void Scene_points_with_normal_item::setRenderingMode(RenderingMode m)
{
    Scene_item::setRenderingMode(m);
    if (rendering_mode==Splatting && (!d->m_points->are_radii_uptodate()))
    {
        computes_local_spacing(6); // default value = small
    }
}

bool Scene_points_with_normal_item::has_normals() const { return d->m_points->has_normal_map(); }

void Scene_points_with_normal_item::invalidateOpenGLBuffers()
{
    are_buffers_filled = false;
    compute_bbox();
}

void Scene_points_with_normal_item::pointSliderPressed()
{
  d->is_point_slider_moving = true;
}

void Scene_points_with_normal_item::pointSliderReleased()
{
  d->is_point_slider_moving = false;
}

void Scene_points_with_normal_item::copyProperties(Scene_item *item)
{
  Scene_points_with_normal_item* point_item = qobject_cast<Scene_points_with_normal_item*>(item);
  if(!point_item)
    return;
  d->point_Slider->setValue(point_item->getPointSliderValue());
  if(has_normals())
    d->normal_Slider->setValue(point_item->getNormalSliderValue());
}

int Scene_points_with_normal_item::getNormalSliderValue()
{
  return d->normal_Slider->value();
}

int Scene_points_with_normal_item::getPointSliderValue()
{
  return d->point_Slider->value();
}

void Scene_points_with_normal_item::itemAboutToBeDestroyed(Scene_item *item)
{
  Scene_item::itemAboutToBeDestroyed(item);
  if(d && d->m_points && item == this)
  {
    delete d->m_points;
    d->m_points = NULL;
  }
}

void Scene_points_with_normal_item::
zoomToPosition(const QPoint &, CGAL::Three::Viewer_interface *viewer) const
{
  if (point_set()->nb_selected_points() == 0)
    return;
  const qglviewer::Vec offset = static_cast<CGAL::Three::Viewer_interface*>(QGLViewer::QGLViewerPool().first())->offset();
  // fit plane to triangles
  Point_set points;
  Bbox selected_points_bbox;
  for(Point_set::const_iterator it = point_set()->first_selected();
      it != point_set()->end();
      ++it)
  {
    points.insert(point_set()->point(*it));
    selected_points_bbox += point_set()->point(*it).bbox();
  }
  Kernel::Plane_3 plane;
  Kernel::Point_3 center_of_mass;
  CGAL::linear_least_squares_fitting_3
      (points.points().begin(),points.points().end(),plane, center_of_mass,
       CGAL::Dimension_tag<0>());

  Kernel::Vector_3 plane_normal= plane.orthogonal_vector();
  plane_normal = plane_normal/(CGAL::sqrt(plane_normal.squared_length()));
  Kernel::Point_3 centroid(center_of_mass.x() + offset.x,
                           center_of_mass.y() + offset.y,
                           center_of_mass.z() + offset.z);

  qglviewer::Quaternion new_orientation(qglviewer::Vec(0,0,-1),
                                        qglviewer::Vec(-plane_normal.x(), -plane_normal.y(), -plane_normal.z()));
  double max_side = (std::max)((std::max)(selected_points_bbox.xmax() - selected_points_bbox.xmin(),
                                          selected_points_bbox.ymax() - selected_points_bbox.ymin()),
                               selected_points_bbox.zmax() - selected_points_bbox.zmin());
  //put the camera in way we are sure the longest side is entirely visible on the screen
  //See openGL's frustum definition
  double factor = max_side/(tan(viewer->camera()->aspectRatio()/
                                  (viewer->camera()->fieldOfView()/2)));

  Kernel::Point_3 new_pos = centroid + factor*plane_normal ;
  viewer->camera()->setSceneCenter(qglviewer::Vec(centroid.x(),
                                                  centroid.y(),
                                                  centroid.z()));
  viewer->moveCameraToCoordinates(QString("%1 %2 %3 %4 %5 %6 %7").arg(new_pos.x())
                                                                 .arg(new_pos.y())
                                                                 .arg(new_pos.z())
                                                                 .arg(new_orientation[0])
                                                                 .arg(new_orientation[1])
                                                                 .arg(new_orientation[2])
                                                                 .arg(new_orientation[3]));

}


/**
 * @file
 * @author University of Warwick
 * @version 1.0
 *
 * @section LICENSE
 *
 * @section DESCRIPTION
 *
 * Tests for the MeshStructGenSource Class
 */

#define BOOST_TEST_MODULE GradientKernels
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>

#include <stdexcept>
#include <string>

#include "GradientKernels.h"
#include "MeshConfig.h"
#include "MeshSourceStructGenConfig.h"
#include "CupCfdAoSMesh.h"
#include "EuclideanVector.h"

#include "PartitionerNaiveConfig.h"
#include "PartitionerConfig.h"

#include <cstdlib>

#include <iostream>

using namespace cupcfd::fvm;

namespace utf = boost::unit_test;
namespace euc = cupcfd::geometry::euclidean;
namespace meshgeo = cupcfd::geometry::mesh;

// Setup
BOOST_AUTO_TEST_CASE(setup)
{
    int argc = boost::unit_test::framework::master_test_suite().argc;
    char ** argv = boost::unit_test::framework::master_test_suite().argv;

    MPI_Init(&argc, &argv);
}

// === GradientPhiGaussDolfyn ===
// Test 1: Test it runs without error on a small mesh
BOOST_AUTO_TEST_CASE(GradientPhiGaussDolfyn_test1)
{
	cupcfd::error::eCodes status;
    cupcfd::comm::Communicator comm(MPI_COMM_WORLD);

	// Create a small test mesh
    cupcfd::partitioner::PartitionerNaiveConfig<int,int> partConfig;

	// Setup the source config
    meshgeo::MeshSourceStructGenConfig<int, double> meshSourceConfig(5, 5, 5, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0);

	// Setup the config to use for building
    meshgeo::MeshConfig<int,double,int> meshConfig(partConfig, meshSourceConfig);

    meshgeo::CupCfdAoSMesh<int,double,int> * mesh;
	status = meshConfig.buildUnstructuredMesh(&mesh, comm);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	// Setup
	double * phiCell = (double *) malloc(sizeof(double) * mesh->properties.lTCells);
	double * phiBoundary = (double *) malloc(sizeof(double) * mesh->properties.lTCells);
	euc::EuclideanVector<double,3> * dPhidxCell = (euc::EuclideanVector<double,3> *) malloc(sizeof(euc::EuclideanVector<double,3>) * mesh->properties.lTCells);
	euc::EuclideanVector<double,3> * dPhidxoCell  = (euc::EuclideanVector<double,3> *) malloc(sizeof(euc::EuclideanVector<double,3>) * mesh->properties.lTCells);

	// Won't bother populating the data arrays for now

	status = GradientPhiGaussDolfyn(*mesh, 1, phiCell, mesh->properties.lTCells,
			phiBoundary, mesh->properties.lTCells,
			dPhidxCell, mesh->properties.lTCells,
			dPhidxoCell, mesh->properties.lTCells);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	free(phiCell);
	free(phiBoundary);
	free(dPhidxCell);
	free(dPhidxoCell);
	delete(mesh);
}

BOOST_AUTO_TEST_CASE(cleanup)
{
    MPI_Finalize();
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2014-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <rokko/rokko.hpp>
#include <rokko/collective.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>
#define BOOST_TEST_MODULE test_product_v
#ifndef BOOST_TEST_DYN_LINK
# include <boost/test/included/unit_test.hpp>
#else
# include <boost/test/unit_test.hpp>
#endif

BOOST_AUTO_TEST_CASE(test_product_v) {
  MPI_Init(&boost::unit_test::framework::master_test_suite().argc,
           &boost::unit_test::framework::master_test_suite().argv);
  MPI_Comm comm = MPI_COMM_WORLD;
  int rank;
  MPI_Comm_rank(comm, &rank);

  int dim = 100;
  if (boost::unit_test::framework::master_test_suite().argc > 1) {
    dim = boost::lexical_cast<int>(boost::unit_test::framework::master_test_suite().argv[1]);
  }

  boost::mt19937 eng;
  boost::variate_generator<boost::mt19937&, boost::uniform_real<> >
    rng(eng, boost::uniform_real<>());

  if (rank == 0) std::cout << "dimension = " << dim << std::endl;
  rokko::parallel_dense_solver solver(rokko::parallel_dense_solver::default_solver());
  rokko::grid g(comm);
  rokko::distributed_matrix<double, rokko::matrix_col_major> matA(dim, dim, g, solver);
  rokko::distributed_matrix<double, rokko::matrix_col_major> vecX(dim, 1, g, solver);
  rokko::distributed_matrix<double, rokko::matrix_col_major> vecY(dim, 1, g, solver);
  rokko::localized_matrix<double, rokko::matrix_col_major> locA(dim, dim);
  rokko::localized_matrix<double, rokko::matrix_col_major> locX(dim, 1);
  rokko::localized_matrix<double, rokko::matrix_col_major> locY(dim, 1);
  for (int j = 0; j < dim; ++j) for (int i = 0; i < dim; ++i) locA(i, j) = rng();
  for (int i = 0; i < dim; ++i) locX(i, 0) = rng();
  for (int i = 0; i < dim; ++i) locY(i, 0) = rng();
  rokko::scatter(locA, matA, 0);
  rokko::scatter(locX, vecX, 0);
  rokko::scatter(locY, vecY, 0);

  // local calculation
  for (int i = 0; i < dim; ++i)
    for (int j = 0; j < dim; ++j)
      locY(i, 0) += locA(i, j) * locX(j, 0);

  // global calculation
  rokko::product_v(1.0, matA, false, vecX, false, 0, 1, vecY, false, 0);

  int success_local = 1;
  for (int i = 0; i < dim; ++i) {
    if (vecY.is_gindex(i, 0) && std::abs(vecY.get_global(i, 0) - locY(i, 0)) >  10e-12)
      success_local = 0;
  }
  int success;
  MPI_Allreduce(&success_local, &success, 1, MPI_INT, MPI_PROD, comm);
  BOOST_CHECK_EQUAL(success, true);
  MPI_Finalize();
}