// ===============================================================================================================
// Copyright (c) 2019, Cornell University. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that
// the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright otice, this list of conditions and
//       the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
//       the following disclaimer in the documentation and/or other materials provided with the distribution.
//
//     * Neither the name of Cornell University nor the names of its contributors may be used to endorse or
//       promote products derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
// OF SUCH DAMAGE.
//
// Author: Kai Zhang (kz298@cornell.edu)
//
// The research is based upon work supported by the Office of the Director of National Intelligence (ODNI),
// Intelligence Advanced Research Projects Activity (IARPA), via DOI/IBC Contract Number D17PC00287.
// The U.S. Government is authorized to reproduce and distribute copies of this work for Governmental purposes.
// ===============================================================================================================

#include "ceres/ceres.h"
#include "glog/logging.h"
#include <Eigen/Dense>

#include <iostream>
#include <vector>
#include <sstream>
#include <fstream>
#include <iomanip>
#include <limits>

using Eigen::MatrixXd;

using ceres::AutoDiffCostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

using std::cout;
using std::vector;
using std::string;
using std::istringstream;
using std::ifstream;
using std::ofstream;
using std::stod;
using std::runtime_error;
using std::setprecision;

typedef std::numeric_limits<double> dbl;
#define INIT_VAL -1e10

struct RPCCamera {
    RPCCamera() {}

    // RPC camera parameters
    double col_numera[20] = {INIT_VAL};
    double col_denomi[20] = {INIT_VAL};
    double row_numera[20] = {INIT_VAL};
    double row_denomi[20] = {INIT_VAL};
    double lat_off = INIT_VAL, lat_scale = INIT_VAL;
    double lon_off = INIT_VAL, lon_scale = INIT_VAL;
    double alt_off = INIT_VAL, alt_scale = INIT_VAL;
    double row_off = INIT_VAL, row_scale = INIT_VAL;
    double col_off = INIT_VAL, col_scale = INIT_VAL;

    // affine approximation of the RPC camera
    // [col, row]^T = M * [lat, lon, alt]^T
    double M11;
    double M12;
    double M13;
    double M14;

    double M21;
    double M22;
    double M23;
    double M24;
};

struct Observation {
    Observation(RPCCamera* cam, double col, double row): cam(cam), col(col), row(row) {}

    RPCCamera* cam = NULL;
    double col = INIT_VAL;
    double row = INIT_VAL;
};

struct ReprojResidual {
    ReprojResidual(Observation* pixel): pixel(pixel) {}

    template <typename T>
    bool operator() (const T* const lat, const T* const lon, const T* const alt,
                     T* residuals) const {
        RPCCamera& cam = *(this->pixel->cam);

        T lat_normed = (lat[0] - T(cam.lat_off)) / T(cam.lat_scale);
        T lon_normed = (lon[0] - T(cam.lon_off)) / T(cam.lon_scale);
        T alt_normed = (alt[0] - T(cam.alt_off)) / T(cam.alt_scale);

        T row_numera = this->apply_poly(cam.row_numera, lat_normed, lon_normed, alt_normed);
        T row_denomi = this->apply_poly(cam.row_denomi, lat_normed, lon_normed, alt_normed);

        T predict_row = row_numera / row_denomi * T(cam.row_scale) + T(cam.row_off);

        T col_numera = this->apply_poly(cam.col_numera, lat_normed, lon_normed, alt_normed);
        T col_denomi = this->apply_poly(cam.col_denomi, lat_normed, lon_normed, alt_normed);

        T predict_col = col_numera / col_denomi * T(cam.col_scale) + T(cam.col_off);

        residuals[0] = predict_row - T(this->pixel->row);
        residuals[1] = predict_col - T(this->pixel->col);
        return true;
    }

private:
    template <typename T>
    T apply_poly(const double* const poly, T x, T y, T z) const {
        T out = T(poly[0]);
        out += poly[1]*y + poly[2]*x + poly[3]*z;
        out += poly[4]*y*x + poly[5]*y*z +poly[6]*x*z;
        out += poly[7]*y*y + poly[8]*x*x + poly[9]*z*z;
        out += poly[10]*x*y*z;
        out += poly[11]*y*y*y;
        out += poly[12]*y*x*x + poly[13]*y*z*z + poly[14]*y*y*x;
        out += poly[15]*x*x*x;
        out += poly[16]*x*z*z + poly[17]*y*y*z + poly[18]*x*x*z;
        out += poly[19]*z*z*z;

        return out;
    }

private:
    Observation* pixel = NULL;
};

void solve_initial(const vector<Observation*>& pixels, vector<double>& initial) {
    assert (pixels.size() >= 2 && initial.size() == 3);

    MatrixXd A(2 * pixels.size(), 3);  // one observation contributes to two equations
    MatrixXd b(2 * pixels.size(), 1);
    for (int i=0; i < pixels.size(); ++i) {
        A(i * 2, 0) = pixels[i]->cam->M11;
        A(i * 2, 1) = pixels[i]->cam->M12;
        A(i * 2, 2) = pixels[i]->cam->M13;
        b(i * 2, 0) = pixels[i]->col - pixels[i]->cam->M14;

        A(i * 2 + 1, 0) = pixels[i]->cam->M21;
        A(i * 2 + 1, 1) = pixels[i]->cam->M22;
        A(i * 2 + 1, 2) = pixels[i]->cam->M23;
        b(i * 2 + 1, 0) = pixels[i]->row - pixels[i]->cam->M24;
    }

    MatrixXd x = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    for (int i=0; i < 3; ++i) {
        initial[i] = x(i, 0);
    }
}

void refine_initial(const vector<Observation*>& pixels, const vector<double>& initial, vector <double>& final, vector <double>& reproj_error) {
    assert (initial.size() == 3 && final.size() == 3 && reproj_error.size() == 2);

    double lat = initial[0];
    double lon = initial[1];
    double alt = initial[2];

    Problem problem;
    for (int i=0; i < pixels.size(); ++i) {
        problem.AddResidualBlock(
                                 new AutoDiffCostFunction<ReprojResidual, 2, 1, 1, 1>(new ReprojResidual(pixels[i])),
                                 NULL, &lat, &lon, &alt);
    }

    Solver::Options options;
    options.max_num_iterations = 100;
    // options.function_tolerance = 1e-10;
    options.linear_solver_type = ceres::DENSE_QR;
    // set the following options to true for debugging
    options.minimizer_progress_to_stdout = false;

    Solver::Summary summary;
    Solve(options, &problem, &summary);

    double init_error = sqrt(summary.initial_cost * 2 / pixels.size());
    double final_error = sqrt(summary.final_cost * 2 / pixels.size());

    // for debugging
    // cout << summary.BriefReport() << "\n";
    // cout << "\ninitial Point: (" << initial[0] << "," << initial[1] << "," << initial[2] << "), reproj_error: " << init_error << " pixels\n";
    // cout << "final Point:  (" << lat << "," << lon << "," << alt << "), reproj_error: " << final_error << " pixels\n";

    // output
    final[0] = lat;
    final[1] = lon;
    final[2] = alt;
    reproj_error[0] = init_error;
    reproj_error[1] = final_error;
}

void read_rpc_cameras(const string& fname, vector<RPCCamera*>& rpc_cameras) {
    // number of rpc cameras
    // camera_id
    // 20 column numerator coefficients
    // 20 column denominator coefficients
    // 20 row numerator coefficients
    // 20 row denominator coefficients
    // 10 normalization constants: lat off, lat scale, lon off, lon scale, alt off, alt scale, col off, col scale
    // 8 affine approximation coefficients: M11, M12, M13, M14, M21, M22, M23, M24
    // ...

    ifstream infile;
    infile.open(fname);
    if (!infile) {
        throw runtime_error("unable to open " + fname);
    }

    int cnt;
    infile >> cnt;
    for (int i=0; i<cnt; ++i) {
        int cam_id;
        // read camera id
        infile >> cam_id;
        assert(cam_id == i);

        rpc_cameras.push_back(new RPCCamera());
        RPCCamera *cam = rpc_cameras.back();

        // read rpc camera parameters
        for (int i = 0; i < 20; ++i) {
            infile >> cam->col_numera[i];
        }
        for (int i = 0; i < 20; ++i) {
            infile >> cam->col_denomi[i];
        }
        for (int i = 0; i < 20; ++i) {
            infile >> cam->row_numera[i];
        }
        for (int i = 0; i < 20; ++i) {
            infile >> cam->row_denomi[i];
        }
        infile >> cam->lat_off >> cam->lat_scale;
        infile >> cam->lon_off >> cam->lon_scale;
        infile >> cam->alt_off >> cam->alt_scale;
        infile >> cam->col_off >> cam->col_scale;
        infile >> cam->row_off >> cam->row_scale;

        // read affine approximation parameters
        infile >> cam->M11 >> cam->M12 >> cam->M13 >> cam->M14;
        infile >> cam->M21 >> cam->M22 >> cam->M23 >> cam->M24;
    }

    infile.close();
}

void triangulate_tracks(const string& cameras_fname, const string& tracks_fname, const string& results_fname) {
    vector<RPCCamera*> rpc_cameras;
    read_rpc_cameras(cameras_fname, rpc_cameras);

    ifstream infile;
    infile.open(tracks_fname);
    if (!infile) {
        throw runtime_error("unable to open " + tracks_fname);
    }

    ofstream outfile;
    outfile.open(results_fname);
    if (!outfile) {
        throw runtime_error("unable to open " + results_fname);
    }
    outfile << setprecision(dbl::max_digits10);

    int cnt;
    infile >> cnt;
    outfile << cnt << '\n';
    for (int i=0; i<cnt; ++i) {
        // read feature track length
        int len;
        infile >> len;
        // read all the observations for this track
        vector<Observation*> pixels;
        for (int j=0; j < len; ++j) {
            int cam_id;
            double col, row;
            infile >> cam_id >> col >> row;

            assert(cam_id < rpc_cameras.size());
            pixels.push_back(new Observation(rpc_cameras[cam_id], col, row));
        }

        // solve for (lat, lon, alt)
        vector<double> initial(3, INIT_VAL);
        vector<double> final(3, INIT_VAL);
        vector<double> reproj_error(2, INIT_VAL);
        solve_initial(pixels, initial);
        refine_initial(pixels, initial, final, reproj_error);

        // write results to file
        // each line is "intial lat, initial lon, initial alt, initial reproj err, final lat, final lon, final alt, final reproj err"
        outfile << initial[0] << " " << initial[1] << " " << initial[2] << " " << reproj_error[0] << " ";
        outfile << final[0] << " " << final[1] << " " << final[2] << " " << reproj_error[1] << "\n";

        // free memory
        for (int i = 0; i < pixels.size(); ++i) {
            delete pixels[i];
        }
    }

    // close file
    infile.close();
    outfile.close();

    // free memory
    for (int i = 0; i < rpc_cameras.size(); ++i) {
        delete rpc_cameras[i];
    }
}

int main(int argc, char** argv) {
    // program name, cameras_fname, tracks_fname, results_fname
    assert(argc == 4);
    google::InitGoogleLogging(argv[0]);
    string cameras_fname = string(argv[1]);
    string tracks_fname = string(argv[2]);
    string results_fname = string(argv[3]);

    triangulate_tracks(cameras_fname, tracks_fname, results_fname);
    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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
//==============================================================================
#ifndef NT2_HYPERBOLIC_FUNCTIONS_SIMD_COMMON_SINHC_HPP_INCLUDED
#define NT2_HYPERBOLIC_FUNCTIONS_SIMD_COMMON_SINHC_HPP_INCLUDED
#include <nt2/hyperbolic/functions/sinhc.hpp>
#include <nt2/hyperbolic/functions/details/sinhc_kernel.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/log_2.hpp>
#include <nt2/include/constants/maxlog.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/average.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/exp.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/inbtrue.hpp>
#include <nt2/include/functions/simd/is_greater.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/rec.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( sinhc_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 1 sinhc is computed using a polynomial(float)
      // respectively rational(double) approx inspired from cephes sinh approx.
      // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
      // computed
      // * in the first case sinh is ((e-rec(e))/2)/x
      // * in the second     sinh is (e/2/x)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2
      //////////////////////////////////////////////////////////////////////////////
      typedef typename meta::as_logical<A0>::type bA0;
      result_type x = nt2::abs(a0);
      bA0 lt1= lt(x, One<A0>());
      std::size_t nb = inbtrue(lt1);
      A0 z = Zero<A0>();
      if( nb > 0)
      {
        z = details::sinhc_kernel<A0>::compute(sqr(x));
        if(nb >= meta::cardinal_of<A0>::value) return z;
      }
      bA0 test1 = gt(x, Maxlog<A0>()-Log_2<A0>());
      A0 fac = if_else(test1, Half<A0>(), One<A0>());
      A0 tmp = exp(x*fac);
      A0 tmp1 = (Half<A0>()*tmp)/x;
      A0 r =  if_else(test1, tmp1*tmp, average(tmp, -rec(tmp))/x);
      #ifndef BOOST_SIMD_NO_INFINITIES
      r = if_else(eq(x, Inf<A0>()), x, r);
      #endif
      return if_else(lt1, z, r);
    }
  };
} }
#endif


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_VELOCITYSCREW6D_HPP
#define RW_MATH_VELOCITYSCREW6D_HPP

/**
 * @file VelocityScrew6D.hpp
 */
#if !defined(SWIG)
#include "EAA.hpp"
#include "Math.hpp"
#include "Transform3D.hpp"
#include "Vector3D.hpp"

#include <rw/common/Serializable.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief Class for representing 6 degrees of freedom velocity screws.
     *
     * \f[
     * \mathbf{\nu} =
     * \left[
     *  \begin{array}{c}
     *  v_x\\
     *  v_y\\
     *  v_z\\
     *  \omega_x\\
     *  \omega_y\\
     *  \omega_z
     *  \end{array}
     * \right]
     * \f]
     *
     * A VelocityScrew is the description of a frames linear and rotational velocity
     * with respect to some reference frame.
     *
     */
    template< class T = double > class VelocityScrew6D
    {
      private:
        T _screw[6];

      public:
        /**
         * @brief Constructs a 6 degrees of freedom velocity screw
         *
         * @param vx [in] @f$ v_x @f$
         * @param vy [in] @f$ v_y @f$
         * @param vz [in] @f$ v_z @f$
         * @param wx [in] @f$ \omega_x @f$
         * @param wy [in] @f$ \omega_y @f$
         * @param wz [in] @f$ \omega_z @f$
         */
        VelocityScrew6D (T vx, T vy, T vz, T wx, T wy, T wz);

        /**
         * @brief Construct from Eigen vector representation.
         * @param v [in] Eigen matrix with either one row or one column.
         */
        template< class R > VelocityScrew6D (const Eigen::MatrixBase< R >& v)
        {
            if (v.cols () != 1 || v.rows () != 6)
                RW_THROW ("Unable to initialize VectorND with " << v.rows () << " x " << v.cols ()
                                                                << " matrix");
            /* For some reason the following does not WORK AT ALL (JIMMY)
            _screw[0] = v(0,0);
            _screw[1] = v(1,0);
            _screw[2] = v(2,0);
            _screw[3] = v(3,0);
            _screw[4] = v(4,0);
            _screw[5] = v(5,0);
            */ // instead use
            _screw[0] = v.row (0) (0);
            _screw[1] = v.row (1) (0);
            _screw[2] = v.row (2) (0);
            _screw[3] = v.row (3) (0);
            _screw[4] = v.row (4) (0);
            _screw[5] = v.row (5) (0);
        }

        /**
         * @brief Default Constructor. Initialized the velocity to 0
         */
        VelocityScrew6D ()
        {
            _screw[0] = _screw[1] = _screw[2] = _screw[3] = _screw[4] = _screw[5] = 0;
        }

        /**
         * @brief Copy Constructor
         * @param vs [in] the velocityscrew6D to copy
         */
        VelocityScrew6D (const VelocityScrew6D& vs)
        {
            for(size_t i = 0; i < vs.size(); i++){
                this->_screw[i]=vs[i];
            }
        }

        /**
         * @brief Constructs a velocity screw in frame @f$ a @f$ from a
         * transform @f$\robabx{a}{b}{\mathbf{T}} @f$.
         *
         * @param transform [in] the corresponding transform.
         */
        explicit VelocityScrew6D (const rw::math::Transform3D< T >& transform);

        /**
         * @brief Constructs a velocity screw from a linear and angular velocity
         *
         * @param linear [in] linear velocity
         * @param angular [in] angular velocity
         */
        VelocityScrew6D (const Vector3D< T >& linear, const EAA< T >& angular);

        /**
         * @brief Extracts the linear velocity
         *
         * @return the linear velocity
         */
        const Vector3D< T > linear () const
        {
            return Vector3D< T > (_screw[0], _screw[1], _screw[2]);
        }

        /**
         * @brief Extracts the angular velocity and represents it using an
         * equivalent-angle-axis as @f$ \dot{\Theta}\mathbf{k} @f$
         *
         * @return the angular velocity
         */
        const EAA< T > angular () const { return EAA< T > (_screw[3], _screw[4], _screw[5]); }

        /**
         * @brief get the size of the underlying vector
         */
        size_t size() const {return 6;}

#if !defined(SWIG)
        /**
         * @brief Returns reference to velocity screw element
         *
         * @param index [in] index in the screw, index must be @f$ < 6 @f$.
         *
         * @return reference to velocity screw element
         */
        T& operator() (std::size_t index)
        {
            assert (index < 6);
            return _screw[index];
        }

        /**
         * @brief Returns const reference to velocity screw element
         *
         * @param index [in] index in the screw, index must be @f$ < 6 @f$.
         *
         * @return const reference to velocity screw element
         */
        const T& operator() (std::size_t index) const
        {
            assert (index < 6);
            return _screw[index];
        }

        /**
         * @brief Returns const reference to velocity screw element
         *
         * @param i [in] index in the screw, index must be @f$ < 6 @f$.
         *
         * @return const reference to velocity screw element
         */
        const T& operator[] (size_t i) const { return (*this) (i); }

        /**
         * @brief Returns const reference to velocity screw element
         *
         * @param i [in] index in the screw, index must be @f$ < 6 @f$.
         *
         * @return const reference to velocity screw element
         */
        T& operator[] (size_t i) { return (*this) (i); }
#else
        ARRAYOPERATOR (T);
#endif

        /**
         * @brief Adds the velocity screw given as a parameter to the velocity screw.
         *
         * @param screw [in] Velocity screw to add
         *
         * @return reference to the VelocityScrew6D to support additional
         * assignments.
         */
        VelocityScrew6D< T >& operator+= (const VelocityScrew6D< T >& screw)
        {
            _screw[0] += screw (0);
            _screw[1] += screw (1);
            _screw[2] += screw (2);
            _screw[3] += screw (3);
            _screw[4] += screw (4);
            _screw[5] += screw (5);
            return *this;
        }

        /**
         * @brief Subtracts the velocity screw given as a parameter from the
         * velocity screw.
         *
         * @param screw [in] Velocity screw to subtract
         *
         * @return reference to the VelocityScrew6D to support additional
         * assignments.
         */
        VelocityScrew6D< T >& operator-= (const VelocityScrew6D< T >& screw)
        {
            _screw[0] -= screw (0);
            _screw[1] -= screw (1);
            _screw[2] -= screw (2);
            _screw[3] -= screw (3);
            _screw[4] -= screw (4);
            _screw[5] -= screw (5);
            return *this;
        }

        /**
         * @brief Scales velocity screw with s
         *
         * @param s [in] scaling value
         *
         * @return reference to the VelocityScrew6D to support additional
         * assigments
         */
        VelocityScrew6D< T >& operator*= (T s)
        {
            _screw[0] *= s;
            _screw[1] *= s;
            _screw[2] *= s;
            _screw[3] *= s;
            _screw[4] *= s;
            _screw[5] *= s;
            return *this;
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true only if all elements are equal.
         *
         * @param rhs [in] VelocityScrew6D to compare with
         * @return True if equal.
         */
        bool operator== (const VelocityScrew6D< T >& rhs) const
        {
            for (int i = 0; i < 6; ++i) {
                if (!(_screw[i] == rhs (i))) {
                    return false;
                }
            }
            return true;
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true if any of the elements are different.
         *
         * @param rhs [in] VelocityScrew6D to compare with
         * @return True if not equal.
         */
        bool operator!= (const VelocityScrew6D< T >& rhs) const { return !(*this == rhs); }
#if !defined(SWIGPYTHON)
        /**
         * @brief Scales velocity screw and returns scaled version
         *
         * @param s [in] scaling value
         * @param screw [in] Screw to scale
         * @return Scales screw
         */
        friend const VelocityScrew6D< T > operator* (T s, const VelocityScrew6D& screw)
        {
            VelocityScrew6D result = screw;
            result *= s;
            return result;
        }
#endif
        /**
         * @brief Scales velocity screw and returns scaled version
         * @param s [in] scaling value
         * @return Scales screw
         */
        const VelocityScrew6D< T > operator* (T s) const
        {
            VelocityScrew6D result = *this;
            result *= s;
            return result;
        }

#if !defined(SWIG)
        /**
         * @brief Changes frame of reference and velocity referencepoint of
         * velocityscrew: @f$ \robabx{b}{b}{\mathbf{\nu}}\to
         * \robabx{a}{a}{\mathbf{\nu}} @f$
         *
         * The frames @f$ \mathcal{F}_a @f$ and @f$ \mathcal{F}_b @f$ are
         * rigidly connected.
         *
         * @param aTb [in] the location of frame @f$ \mathcal{F}_b @f$ wrt.
         * frame @f$ \mathcal{F}_a @f$: @f$ \robabx{a}{b}{\mathbf{T}} @f$
         *
         * @param bV [in] velocity screw wrt. frame @f$ \mathcal{F}_b @f$: @f$
         * \robabx{b}{b}{\mathbf{\nu}} @f$
         *
         * @return the velocity screw wrt. frame @f$ \mathcal{F}_a @f$: @f$
         * \robabx{a}{a}{\mathbf{\nu}} @f$
         *
         * Transformation of both the velocity reference point and of the base to
         * which the VelocityScrew is expressed
         *
         * \f[
         * \robabx{a}{a}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *  \robabx{a}{a}{\mathbf{v}} \\
         *  \robabx{a}{a}{\mathbf{\omega}}
         *  \end{array}
         * \right] =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & S(\robabx{a}{b}{\mathbf{p}})
         *    \robabx{a}{b}{\mathbf{R}} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \robabx{b}{b}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{b}{\mathbf{v}} +
         *    \robabx{a}{b}{\mathbf{p}} \times \robabx{a}{b}{\mathbf{R}}
         *    \robabx{b}{b}{\mathbf{\omega}}\\
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{b}{\mathbf{\omega}}
         *  \end{array}
         * \right]
         * \f]
         *
         */
        friend const VelocityScrew6D< T > operator* (const rw::math::Transform3D< T >& aTb,
                                                     const VelocityScrew6D< T >& bV)
        {
            const Vector3D< T >& bv = bV.linear ();
            const EAA< T >& bw      = bV.angular ();
            const EAA< T >& aw      = aTb.R () * bw;
            const Vector3D< T >& av = aTb.R () * bv + cross (aTb.P (), aw);
            return VelocityScrew6D< T > (av, aw);
        }
#endif
#if !defined(SWIG)
        /**
         * @brief Changes velocity referencepoint of
         * velocityscrew: @f$ \robabx{a}{q}{\mathbf{\nu}}\to
         * \robabx{a}{p}{\mathbf{\nu}} @f$
         *
         * The vector should describe a translation from the current
         * velocity reference point q to the wanted/new velocity reference point p
         * seen from frame @f$ \mathcal{F}_a @f$
         *
         * @param aPqTop [in] the translation from point q to point p seen in
         * frame @f$ \mathcal{F}_a @f$
         *
         * @param bV [in] velocity screw wrt. frame @f$ \mathcal{F}_a @f$: @f$
         * \robabx{a}{q}{\mathbf{\nu}} @f$
         *
         * @return the velocity screw wrt. frame @f$ \mathcal{F}_a @f$: @f$
         * \robabx{a}{p}{\mathbf{\nu}} @f$
         *
         * Transformation of the velocity reference point
         *
         * \f[
         * \robabx{a}{p}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *  \robabx{a}{p}{\mathbf{v}} \\
         *  \robabx{a}{p}{\mathbf{\omega}}
         *  \end{array}
         * \right] =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & S(\robabx{a}{b}{\mathbf{p}})
         *    \robabx{a}{b}{\mathbf{R}} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \robabx{a}{p}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *    \robabx{a}{p}{\mathbf{v}} +
         *    \robabx{a}{qTop}{\mathbf{p}}
         *    \robabx{b}{b}{\mathbf{\omega}}\\
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{b}{\mathbf{\omega}}
         *  \end{array}
         * \right]
         * \f]
         *
         */
        friend const VelocityScrew6D< T > operator* (const Vector3D< T >& aPqTop,
                                                     const VelocityScrew6D< T >& bV)
        {
            const Vector3D< T >& bv = bV.linear ();
            const EAA< T >& bw      = bV.angular ();
            const Vector3D< T >& av = bv + cross (aPqTop, bw);
            return VelocityScrew6D< T > (av, bw);
        }
#endif
#if !defined(SWIG)
        /**
         * @brief Changes frame of reference for velocityscrew: @f$
         * \robabx{b}{i}{\mathbf{\nu}}\to \robabx{a}{i}{\mathbf{\nu}}
         * @f$
         *
         * @param aRb [in] the change in orientation between frame
         * @f$ \mathcal{F}_a @f$ and frame
         * @f$ \mathcal{F}_b @f$: @f$ \robabx{a}{b}{\mathbf{R}} @f$
         *
         * @param bV [in] velocity screw wrt. frame
         * @f$ \mathcal{F}_b @f$: @f$ \robabx{b}{i}{\mathbf{\nu}} @f$
         *
         * @return the velocity screw wrt. frame @f$ \mathcal{F}_a @f$:
         * @f$ \robabx{a}{i}{\mathbf{\nu}} @f$
         *
         * Transformation of the base to which the VelocityScrew is expressed. The velocity
         * reference point is left intact
         *
         * \f[
         * \robabx{a}{i}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *  \robabx{a}{i}{\mathbf{v}} \\
         *  \robabx{a}{i}{\mathbf{\omega}}
         *  \end{array}
         * \right] =
         * \left[
         *  \begin{array}{cc}
         *    \robabx{a}{b}{\mathbf{R}} & \mathbf{0}^{3x3} \\
         *    \mathbf{0}^{3x3} & \robabx{a}{b}{\mathbf{R}}
         *  \end{array}
         * \right]
         * \robabx{b}{i}{\mathbf{\nu}} =
         * \left[
         *  \begin{array}{c}
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{i}{\mathbf{v}} \\
         *    \robabx{a}{b}{\mathbf{R}} \robabx{b}{i}{\mathbf{\omega}}
         *  \end{array}
         * \right]
         * \f]
         */
        friend const VelocityScrew6D< T > operator* (const Rotation3D< T >& aRb,
                                                     const VelocityScrew6D< T >& bV)
        {
            Vector3D< T > bv = bV.linear ();
            EAA< T > bw      = bV.angular ();

            return VelocityScrew6D< T > (aRb * bv, aRb * bw);
        }
#endif

        /**
         * @brief Adds two velocity screws together @f$
         * \mathbf{\nu}_{12}=\mathbf{\nu}_1+\mathbf{\nu}_2 @f$
         *
         * @param screw2 [in] @f$ \mathbf{\nu}_2 @f$
         *
         * @return the velocity screw @f$ \mathbf{\nu}_{12} @f$
         */
        const VelocityScrew6D< T > operator+ (const VelocityScrew6D< T >& screw2) const
        {
            return VelocityScrew6D< T > (_screw[0] + screw2._screw[0],
                                         _screw[1] + screw2._screw[1],
                                         _screw[2] + screw2._screw[2],
                                         _screw[3] + screw2._screw[3],
                                         _screw[4] + screw2._screw[4],
                                         _screw[5] + screw2._screw[5]);
        }

        /**
         * @brief Subtracts two velocity screws
         * \f$\mathbf{\nu}_{12}=\mathbf{\nu}_1-\mathbf{\nu}_2\f$
         *
         * \param screw2 [in] \f$\mathbf{\nu}_2\f$
         * \return the velocity screw \f$\mathbf{\nu}_{12} \f$
         */
        const VelocityScrew6D< T > operator- (const VelocityScrew6D< T >& screw2) const
        {
            return VelocityScrew6D< T > (_screw[0] - screw2._screw[0],
                                         _screw[1] - screw2._screw[1],
                                         _screw[2] - screw2._screw[2],
                                         _screw[3] - screw2._screw[3],
                                         _screw[4] - screw2._screw[4],
                                         _screw[5] - screw2._screw[5]);
        }
#if !defined(SWIG)
        /**
         * @brief Ouputs velocity screw to stream
         *
         * @param os [in/out] stream to use
         * @param screw [in] velocity screw
         * @return the resulting stream
         */
        friend std::ostream& operator<< (std::ostream& os, const VelocityScrew6D< T >& screw)
        {
            return os << "{{" << screw (0) << "," << screw (1) << "," << screw (2) << "},{"
                      << screw (3) << "," << screw (4) << "," << screw (5) << "}}";
        }
#else
        TOSTRING (rw::math::VelocityScrew6D< T >);
#endif
        /**
         * @brief Takes the 1-norm of the velocity screw. All elements both
         * angular and linear are given the same weight.
         *
         * @return the 1-norm
         */
        T norm1 () const
        {
            return fabs (_screw[0]) + fabs (_screw[1]) + fabs (_screw[2]) + fabs (_screw[3]) +
                   fabs (_screw[4]) + fabs (_screw[5]);
        }

        /**
         * @brief Takes the 2-norm of the velocity screw. All elements both
         * angular and linear are given the same weight
         *
         * @return the 2-norm
         */
        T norm2 () const
        {
            return std::sqrt (Math::sqr (_screw[0]) + Math::sqr (_screw[1]) +
                              Math::sqr (_screw[2]) + Math::sqr (_screw[3]) +
                              Math::sqr (_screw[4]) + Math::sqr (_screw[5]));
        }

        /**
         * @brief Takes the infinite norm of the velocity screw. All elements
         * both angular and linear are given the same weight.
         *
         * @return the infinite norm
         */
        T normInf () const
        {
            return std::max (
                fabs (_screw[0]),
                std::max (fabs (_screw[1]),
                          std::max (fabs (_screw[2]),
                                    std::max (fabs (_screw[3]),
                                              std::max (fabs (_screw[4]), fabs (_screw[5]))))));
        }

        /**
           @brief Converter to Eigen vector
         */
        Eigen::Matrix< T, 6, 1 > e () const
        {
            Eigen::Matrix< T, 6, 1 > res;
            for (size_t i = 0; i < 6; i++)
                res (i) = _screw[i];
            return res;
        }
    };

    /**
     * @brief Takes the 1-norm of the velocity screw. All elements both
     * angular and linear are given the same weight.
     *
     * @param screw [in] the velocity screw
     * @return the 1-norm
     */
    template< class T > T norm1 (const VelocityScrew6D< T >& screw) { return screw.norm1 (); }

    /**
     * @brief Takes the 2-norm of the velocity screw. All elements both
     * angular and linear are given the same weight
     *
     * @param screw [in] the velocity screw
     * @return the 2-norm
     */
    template< class T > T norm2 (const VelocityScrew6D< T >& screw) { return screw.norm2 (); }

    /**
     * @brief Takes the infinite norm of the velocity screw. All elements
     * both angular and linear are given the same weight.
     *
     * @param screw [in] the velocity screw
     *
     * @return the infinite norm
     */
    template< class T > T normInf (const VelocityScrew6D< T >& screw) { return screw.normInf (); }

    /**
     * @brief Casts VelocityScrew6D<T> to VelocityScrew6D<Q>
     *
     * @param vs [in] VelocityScrew6D with type T
     *
     * @return VelocityScrew6D with type Q
     */
    template< class Q, class T > const VelocityScrew6D< Q > cast (const VelocityScrew6D< T >& vs)
    {
        return VelocityScrew6D< Q > (static_cast< Q > (vs (0)),
                                     static_cast< Q > (vs (1)),
                                     static_cast< Q > (vs (2)),
                                     static_cast< Q > (vs (3)),
                                     static_cast< Q > (vs (4)),
                                     static_cast< Q > (vs (5)));
    }
#if !defined(SWIG)
    extern template class rw::math::VelocityScrew6D< double >;
    extern template class rw::math::VelocityScrew6D< float >;
#else
    SWIG_DECLARE_TEMPLATE (VelocityScrew6Dd, rw::math::VelocityScrew6D< double >);
    SWIG_DECLARE_TEMPLATE (VelocityScrew6Df, rw::math::VelocityScrew6D< float >);
#endif
    using VelocityScrew6Dd = VelocityScrew6D< double >;
    using VelocityScrew6Df = VelocityScrew6D< float >;

    /*@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::VelocityScrew6D
         */
        template<>
        void write (const rw::math::VelocityScrew6D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::VelocityScrew6D
         */
        template<>
        void write (const rw::math::VelocityScrew6D< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::VelocityScrew6D
         */
        template<>
        void read (rw::math::VelocityScrew6D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::VelocityScrew6D
         */
        template<>
        void read (rw::math::VelocityScrew6D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Simple_cartesian.h>

// Graphs
#include <CGAL/Polyhedron_3.h>
#include <CGAL/IO/Polyhedron_iostream.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/graph_traits_Regular_triangulation_2.h>
#include <CGAL/boost/graph/properties_Regular_triangulation_2.h>

#include <CGAL/Polygon_mesh_processing/locate.h>

#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/Bbox_3.h>
#include <CGAL/boost/graph/io.h>
#include <CGAL/boost/graph/named_params_helper.h>
#include <CGAL/boost/graph/generators.h>
#include <CGAL/boost/graph/helpers.h>
#include <CGAL/Dimension.h>
#include <CGAL/Kernel_traits.h>
#include <CGAL/Origin.h>
#include <CGAL/property_map.h>
#include <CGAL/Random.h>
#include <CGAL/Unique_hash_map.h>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/optional.hpp>

#include <fstream>
#include <iostream>
#include <limits>
#include <map>
#include <set>
#include <type_traits>
#include <utility>
#include <vector>

namespace PMP = CGAL::Polygon_mesh_processing;

typedef CGAL::Exact_predicates_inexact_constructions_kernel    EPICK;
typedef CGAL::Exact_predicates_exact_constructions_kernel      EPECK;

typedef CGAL::Simple_cartesian<typename CGAL::internal::Exact_field_selector<double>::Type> Exact_kernel;

template<typename AABB_tree>
typename CGAL::Kernel_traits<typename AABB_tree::AABB_traits::Point_3>::type::Ray_2
random_2D_ray(const AABB_tree& aabb_tree, CGAL::Random& rnd)
{
  typedef typename AABB_tree::AABB_traits::Point_3           Point_3;
  typedef typename CGAL::Kernel_traits<Point_3>::type        Kernel;
  typedef typename Kernel::FT                                FT;
  typedef typename Kernel::Point_2                           Point_2;
  typedef typename Kernel::Ray_2                             Ray_2;

  const CGAL::Bbox_3& bbox = aabb_tree.bbox();

  FT px = (bbox.xmin() == bbox.xmax()) ? bbox.xmin() : rnd.get_double(bbox.xmin(), bbox.xmax());
  FT py = (bbox.ymin() == bbox.ymax()) ? bbox.ymin() : rnd.get_double(bbox.ymin(), bbox.ymax());

  FT qx = (bbox.xmin() == bbox.xmax()) ? bbox.xmin() : rnd.get_double(bbox.xmin(), bbox.xmax());
  FT qy = (bbox.ymin() == bbox.ymax()) ? bbox.ymin() : rnd.get_double(bbox.ymin(), bbox.ymax());

  return Ray_2(Point_2(px, py), Point_2(qx, qy));
}

template<typename AABB_tree>
typename CGAL::Kernel_traits<typename AABB_tree::AABB_traits::Point_3>::type::Ray_3
random_3D_ray(const AABB_tree& aabb_tree, CGAL::Random& rnd)
{
  typedef typename AABB_tree::AABB_traits::Point_3           Point_3;
  typedef typename CGAL::Kernel_traits<Point_3>::type        Kernel;
  typedef typename Kernel::FT                                FT;
  typedef typename Kernel::Ray_3                             Ray_3;

  const CGAL::Bbox_3& bbox = aabb_tree.bbox();

  FT px = (bbox.xmin() == bbox.xmax()) ? bbox.xmin() : rnd.get_double(bbox.xmin(), bbox.xmax());
  FT py = (bbox.ymin() == bbox.ymax()) ? bbox.ymin() : rnd.get_double(bbox.ymin(), bbox.ymax());
  FT pz = (bbox.zmin() == bbox.zmax()) ? bbox.zmin() : rnd.get_double(bbox.zmin(), bbox.zmax());

  FT qx = (bbox.xmin() == bbox.xmax()) ? bbox.xmin() : rnd.get_double(bbox.xmin(), bbox.xmax());
  FT qy = (bbox.ymin() == bbox.ymax()) ? bbox.ymin() : rnd.get_double(bbox.ymin(), bbox.ymax());
  FT qz = (bbox.zmin() == bbox.zmax()) ? bbox.zmin() : rnd.get_double(bbox.zmin(), bbox.zmax());

  return Ray_3(Point_3(px, py, pz), Point_3(qx, qy, qz));
}

template<typename FT>
bool is_equal(const FT& a, const FT& b)
{
  if(std::is_floating_point<FT>::value)
    return (CGAL::abs(a - b) <= 1e-7); // numeric_limits' epsilon is too restrictive...
  else
    return (a == b);
}

template<typename K, typename G>
void test_snappers(const G& g)
{
  std::cout << "  test snappers..." << std::endl;

  typedef typename K::FT                                              FT;

  PMP::Barycentric_coordinates<FT> coords = CGAL::make_array(FT(1e-6), FT(0.9999999999999999999), FT(1e-7));
  PMP::Face_location<G, FT> loc = std::make_pair(*(faces(g).first), coords);

  // ---------------------------------------------------------------------------
  PMP::internal::snap_coordinates_to_border<FT>(coords); // uses numeric_limits' epsilon()
  assert(coords[0] == FT(1e-6) && coords[1] == FT(1) && coords[2] == FT(1e-7));

  PMP::internal::snap_coordinates_to_border(coords, FT(1e-5));
  assert(coords[0] == FT(0) && coords[1] == FT(1) && coords[2] == FT(0));

  // ---------------------------------------------------------------------------
  PMP::internal::snap_location_to_border<FT>(loc, g); // uses numeric_limits' epsilon()
  assert(!PMP::is_on_face_border(loc, g));

  PMP::internal::snap_location_to_border(loc, g, FT(1e-7));
  assert(PMP::is_on_face_border(loc, g));
}

template <typename K, int>
struct Point_to_bare_point
{
  typedef typename K::Point_2 type;
};

template <typename K>
struct Point_to_bare_point<K, 3>
{
  typedef typename K::Point_3 type;
};

template<typename K, typename G, typename VPM>
void test_constructions(const G& g,
                        const VPM vpm,
                        CGAL::Random& rnd)
{
  std::cout << "  test constructions..." << std::endl;

  typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
  typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
  typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;
  typedef typename PMP::descriptor_variant<G>                                descriptor_variant;

  typedef typename boost::property_traits<VPM>::value_type                        Point;
  typedef typename boost::property_traits<VPM>::reference                         Point_reference;
  typedef typename K::FT                                                          FT;
  typedef typename Point_to_bare_point<K, Point::Ambient_dimension::value>::type  Bare_point;

  typedef typename PMP::Barycentric_coordinates<FT>                          Barycentric_coordinates;
  typedef typename PMP::Face_location<G, FT>                                 Face_location;

  face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
  halfedge_descriptor h = halfedge(f, g);
  vertex_descriptor v = source(h, g);

  Point_reference p = get(vpm, v);
  Point_reference q = get(vpm, target(h, g));
  Point_reference r = get(vpm, target(next(h, g), g));

  const Bare_point bp(p);
  const Bare_point bq(q);
  const Bare_point br(r);

  Barycentric_coordinates bar;
  Face_location loc;
  loc.first = f;

  // ---------------------------------------------------------------------------
  bar = PMP::barycentric_coordinates(p, q, r, p, K());
  assert(is_equal(bar[0], FT(1)) && is_equal(bar[1], FT(0)) && is_equal(bar[2], FT(0)));
  bar = PMP::barycentric_coordinates(p, q, r, q, K());
  assert(is_equal(bar[0], FT(0)) && is_equal(bar[1], FT(1)) && is_equal(bar[2], FT(0)));
  bar = PMP::barycentric_coordinates(p, q, r, r, K());
  assert(is_equal(bar[0], FT(0)) && is_equal(bar[1], FT(0)) && is_equal(bar[2], FT(1)));

  Point mp = Point(CGAL::midpoint(bp, bq));
  bar = PMP::barycentric_coordinates(p, q, r, mp);
  assert(is_equal(bar[0], FT(0.5)) && is_equal(bar[1], FT(0.5)) && is_equal(bar[2], FT(0)));

  int n = 100;
  while(n --> 0) // :)
  {
    const FT a = rnd.get_double(-1., 1.);
    const FT b = rnd.get_double(-1., 1.);
    const FT c = 1. - a - b;

    // Point to location and inversely
    Bare_point barycentric_pt = CGAL::barycenter(bp, a, bq, b, br, c);
    bar = PMP::barycentric_coordinates(p, q, r, Point(barycentric_pt));
    assert(is_equal(bar[0], a) && is_equal(bar[1], b) && is_equal(bar[2], c));

    loc.second = bar;
    const Bare_point barycentric_pt_2 =
      Bare_point(PMP::construct_point(loc, g,
                                      CGAL::parameters::vertex_point_map(vpm)
                                                       .geom_traits(K())));

    const FT sq_dist = CGAL::squared_distance(barycentric_pt, barycentric_pt_2);
    assert(is_equal(sq_dist, FT(0)));
  }

  // ---------------------------------------------------------------------------
  loc = std::make_pair(f, CGAL::make_array(FT(0.3), FT(0.4), FT(0.3)));
  descriptor_variant dv = PMP::get_descriptor_from_location(loc, g);
  const face_descriptor* fd = boost::get<face_descriptor>(&dv);
  assert(fd);

  loc = std::make_pair(f, CGAL::make_array(FT(0.5), FT(0.5), FT(0)));
  dv = PMP::get_descriptor_from_location(loc, g);
  const halfedge_descriptor* hd = boost::get<halfedge_descriptor>(&dv);
  assert(hd);

  loc = std::make_pair(f, CGAL::make_array(FT(1), FT(0), FT(0)));
  assert(PMP::is_on_vertex(loc, source(halfedge(f, g), g), g));
  dv = PMP::get_descriptor_from_location(loc, g);
  if(const vertex_descriptor* v = boost::get<vertex_descriptor>(&dv)) { } else { assert(false); }

  // ---------------------------------------------------------------------------
  // just to check the API
  PMP::construct_point(loc, g);
  PMP::construct_point(loc, g, CGAL::parameters::default_values());
}

template<typename K, typename G>
void test_random_entities(const G& g, CGAL::Random& rnd)
{
  std::cout << "  test random entities..." << std::endl;

  typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
  typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

  typedef typename K::FT                                                     FT;
  typedef typename PMP::Face_location<G, FT>                                 Face_location;

  // ---------------------------------------------------------------------------
  Face_location loc;

  halfedge_descriptor h = CGAL::internal::random_halfedge_in_mesh(g, rnd);
  if(is_border(h, g))
    h = opposite(h, g);
  face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);

  int nn = 100;
  while(nn --> 0) // the infamous 'go to zero' operator
  {
    loc = PMP::random_location_on_mesh<FT>(g, rnd);
    assert(loc.first != boost::graph_traits<G>::null_face());
    assert(loc.second[0] >= FT(0) && loc.second[0] <= FT(1) &&
           loc.second[1] >= FT(0) && loc.second[1] <= FT(1) &&
           loc.second[2] >= FT(0) && loc.second[2] <= FT(1));

    loc = PMP::random_location_on_face<FT>(f, g, rnd);
    assert(loc.first == f);
    assert(loc.second[0] >= FT(0) && loc.second[0] <= FT(1) &&
           loc.second[1] >= FT(0) && loc.second[1] <= FT(1) &&
           loc.second[2] >= FT(0) && loc.second[2] <= FT(1));

    loc = PMP::random_location_on_halfedge<FT>(h, g, rnd);
    assert(loc.first == face(h, g));
    assert(loc.second[0] >= FT(0) && loc.second[0] <= FT(1) &&
           loc.second[1] >= FT(0) && loc.second[1] <= FT(1) &&
           loc.second[2] >= FT(0) && loc.second[2] <= FT(1));
    int h_id = CGAL::halfedge_index_in_face(h, g);
    assert(loc.second[(h_id+2)%3] == FT(0));
  }
}

template<typename K, typename G>
void test_helpers(const G& g, CGAL::Random& rnd)
{
  std::cout << "  test helpers..." << std::endl;

  typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
  typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
  typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

  typedef typename K::FT                                                     FT;
  typedef typename PMP::Face_location<G, FT>                                 Face_location;

  face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
  halfedge_descriptor h = halfedge(f, g);
  vertex_descriptor v = source(h, g);

  // ---------------------------------------------------------------------------
  // Local index
  int pos = CGAL::vertex_index_in_face(v, f, g);
  assert(pos == 0);
  pos = CGAL::vertex_index_in_face(target(h, g), f, g);
  assert(pos == 1);
  pos = CGAL::vertex_index_in_face(target(next(h, g), g), f, g);
  assert(pos == 2);

  pos = CGAL::halfedge_index_in_face(h, g);
  assert(pos == 0);
  pos = CGAL::halfedge_index_in_face(next(h, g), g);
  assert(pos == 1);
  pos = CGAL::halfedge_index_in_face(prev(h, g), g);
  assert(pos == 2);

  // ---------------------------------------------------------------------------
  // Incident faces
  Face_location loc = PMP::random_location_on_face<FT>(f, g, rnd);
  std::set<face_descriptor> s;
  PMP::internal::incident_faces(loc, g, std::inserter(s, s.begin()));
  assert(PMP::is_on_face_border(loc, g) || s.size() == 1);

  loc = PMP::random_location_on_halfedge<FT>(h, g, rnd);
  std::vector<face_descriptor> vec;
  PMP::internal::incident_faces(loc, g, std::back_inserter(vec));
  assert(PMP::is_on_vertex(loc, source(h, g), g) ||
         PMP::is_on_vertex(loc, target(h, g), g) ||
         vec.size() == 2);
}

template<typename K, typename G>
void test_predicates(const G& g, CGAL::Random& rnd)
{
  std::cout << "  test predicates..." << std::endl;

  typedef typename K::FT                                                     FT;

  typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
  typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
  typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

  typedef typename PMP::Face_location<G, FT>                                 Face_location;

  face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
  halfedge_descriptor h = halfedge(f, g);
  vertex_descriptor v = source(h, g);

  // ---------------------------------------------------------------------------
  Face_location loc(f, CGAL::make_array(FT(1), FT(0), FT(0)));
  assert(PMP::is_on_vertex<FT>(loc, v, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(1), FT(0)));
  assert(PMP::is_on_vertex<FT>(loc, target(h, g), g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(0), FT(1)));
  assert(PMP::is_on_vertex<FT>(loc, target(next(h, g), g), g));
  loc = Face_location(f, CGAL::make_array(FT(-1), FT(1), FT(1)));
  assert(!PMP::is_on_vertex<FT>(loc, target(next(h, g), g), g));

  // ---------------------------------------------------------------------------
  loc = Face_location(f, CGAL::make_array(FT(0.5), FT(0.5), FT(0)));
  assert(PMP::is_on_halfedge<FT>(loc, h, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(0.5), FT(0.5)));
  assert(PMP::is_on_halfedge<FT>(loc, next(h, g), g));
  loc = Face_location(f, CGAL::make_array(FT(-0.5), FT(1.5), FT(0)));
  assert(!PMP::is_on_halfedge<FT>(loc, h, g));
  loc = Face_location(f, CGAL::make_array(FT(0.1), FT(-0.6), FT(1.5)));
  assert(!PMP::is_on_halfedge<FT>(loc, h, g));

  // ---------------------------------------------------------------------------
  loc = Face_location(f, CGAL::make_array(FT(0.3), FT(0.3), FT(0.4)));
  assert(PMP::is_in_face<FT>(loc, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(0), FT(1)));
  assert(PMP::is_in_face<FT>(loc, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(2), FT(-1)));
  assert(!PMP::is_in_face<FT>(loc, g));

  // ---------------------------------------------------------------------------
  loc = Face_location(f, CGAL::make_array(FT(0.3), FT(0.3), FT(0.4)));
  assert(!PMP::is_on_face_border<FT>(loc, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(0.6), FT(0.4)));
  assert(PMP::is_on_face_border<FT>(loc, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(0), FT(1)));
  assert(PMP::is_on_face_border<FT>(loc, g));
  loc = Face_location(f, CGAL::make_array(FT(-0.2), FT(0), FT(1.2)));
  assert(!PMP::is_on_face_border(loc, g));

  // ---------------------------------------------------------------------------
  int max = 1000, counter = 0;
  typename boost::graph_traits<G>::halfedge_iterator hit, hend;
  boost::tie(hit, hend) = halfedges(g);
  for(; hit!=hend; ++hit)
  {
    const halfedge_descriptor h = *hit;
    if(face(h, g) == boost::graph_traits<G>::null_face())
      continue;

    const int id_of_h = CGAL::halfedge_index_in_face(h, g);
    const face_descriptor f = face(h, g);
    loc.first = f;

    loc.second[id_of_h] = FT(1);
    loc.second[(id_of_h+1)%3] = FT(0);
    loc.second[(id_of_h+2)%3] = FT(0);
    boost::optional<halfedge_descriptor> opt_hd = CGAL::is_border(source(h, g), g);
    assert(PMP::is_on_mesh_border<FT>(loc, g) == (opt_hd != boost::none));

    loc.second[id_of_h] = FT(0.5);
    loc.second[(id_of_h+1)%3] = FT(0.5);
    assert(PMP::is_on_mesh_border<FT>(loc, g) == CGAL::is_border(edge(h, g), g));

    // Even if the point does lie on the border of the mesh, 'false' is returned because
    // another face descriptor should be used.
    loc.second[id_of_h] = -0.5;
    loc.second[(id_of_h+1)%3] = 1.5;
    assert(!PMP::is_on_mesh_border<FT>(loc, g));

    if(++counter > max)
      break;
  }
}

template<typename K, typename G, typename VPM>
void test_locate_in_face(const G& g,
                         const VPM vpm,
                         CGAL::Random& rnd)
{
  std::cout << "  test locate_in_face()..." << std::endl;

  typedef typename boost::property_traits<VPM>::reference                    Point_reference;
  typedef typename K::FT                                                     FT;

  typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
  typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
  typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

  typedef typename PMP::Face_location<G, FT>                                 Face_location;

  const face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
  const halfedge_descriptor h = halfedge(f, g);
  const vertex_descriptor v = target(h, g);

  Face_location loc;
  FT a = 0.1;
  Point_reference p = get(vpm, v);

  loc = PMP::locate_vertex<FT>(v, g);
  assert(is_equal(loc.second[CGAL::vertex_index_in_face(v, loc.first, g)], FT(1)));
  assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+1)%3], FT(0)));
  assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+2)%3], FT(0)));

  loc = PMP::locate_vertex<FT>(v, f, g);
  assert(loc.first == f);
  assert(is_equal(loc.second[0], FT(0)) && is_equal(loc.second[1], FT(1)) && is_equal(loc.second[2], FT(0)));

  loc = PMP::locate_on_halfedge<FT>(h, a, g);
  const int h_id = CGAL::halfedge_index_in_face(h, g);
  assert(loc.first == f && is_equal(loc.second[(h_id+2)%3], FT(0)));

  loc = PMP::locate_in_face(p, f, g, CGAL::parameters::vertex_point_map(vpm).geom_traits(K()));
  int v_id = CGAL::vertex_index_in_face(v, f, g);
  assert(loc.first == f && is_equal(loc.second[v_id], FT(1)));

  // Internal vertex point pmap
  typedef typename boost::property_map_value<G, CGAL::vertex_point_t>::type     Point;

  Point p2 = get(CGAL::vertex_point, g, v);
  PMP::locate_in_face(p2, f, g);
  assert(loc.first == f && is_equal(loc.second[v_id], FT(1)));

  // ---------------------------------------------------------------------------
  loc.second[0] = FT(0.2);
  loc.second[1] = FT(0.8);
  loc.second[2] = FT(0);

  halfedge_descriptor neigh_hd = opposite(halfedge(f, g), g);
  face_descriptor neigh_f = face(neigh_hd, g);

  // Want to check good correspondence seen from one side and the other. If unfortunately
  // we have selected a border face, can't do anything!
  if(neigh_f != boost::graph_traits<G>::null_face())
  {
    int neigh_hd_id = CGAL::halfedge_index_in_face(neigh_hd, g);
    Face_location neigh_loc;
    neigh_loc.first = neigh_f;
    neigh_loc.second[neigh_hd_id] = FT(0.3);
    neigh_loc.second[(neigh_hd_id+1)%3] = FT(0.7);
    neigh_loc.second[(neigh_hd_id+2)%3] = FT(0);

    PMP::locate_in_adjacent_face(loc, neigh_f, g);

    assert(PMP::locate_in_common_face<FT>(loc, neigh_loc, g));

    assert(PMP::locate_in_common_face<FT>(loc, p, neigh_loc, g, CGAL::parameters::vertex_point_map(vpm).geom_traits(K())));
    assert(PMP::locate_in_common_face<FT>(loc, p, neigh_loc, g, CGAL::parameters::vertex_point_map(vpm).geom_traits(K()), 1e-7));
  }
}

template <typename K, typename VPM,
          int dim = CGAL::Ambient_dimension<typename boost::property_traits<VPM>::value_type>::value>
struct Locate_with_AABB_tree_Tester // 2D case
{
  template <typename G>
  void test(const G& g, const VPM vpm, CGAL::Random& rnd) const
  {
    std::cout << "  test locate_with_AABB_tree (2D)..." << std::endl;

    typedef typename boost::property_traits<VPM>::reference                    Point_reference;

    typedef typename K::FT                                                     FT;
    typedef typename K::Ray_2                                                  Ray_2;
    typedef typename K::Ray_3                                                  Ray_3;
    typedef typename K::Point_3                                                Point_3;

    typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
    typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
    typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

    typedef typename PMP::Face_location<G, FT>                                 Face_location;

    face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
    halfedge_descriptor h = halfedge(f, g);
    vertex_descriptor v = target(h, g);

    // ---------------------------------------------------------------------------
    typedef typename boost::property_traits<VPM>::value_type                   Intrinsic_point;
    typedef PMP::internal::Point_to_Point_3<G, Intrinsic_point>                Intrinsic_point_to_Point_3;
    typedef PMP::internal::Point_to_Point_3_VPM<G, VPM>                        WrappedVPM;
    typedef CGAL::AABB_face_graph_triangle_primitive<G, WrappedVPM>            AABB_face_graph_primitive;
    typedef CGAL::AABB_traits<K, AABB_face_graph_primitive>                    AABB_face_graph_traits;

    CGAL_static_assertion((std::is_same<typename AABB_face_graph_traits::Point_3, Point_3>::value));

    Intrinsic_point_to_Point_3 to_p3;

    CGAL::AABB_tree<AABB_face_graph_traits> tree_a;
    Point_reference p_a = get(vpm, v);
    const Point_3& p3_a = to_p3(p_a);

    CGAL::AABB_tree<AABB_face_graph_traits> tree_b;
    WrappedVPM vpm_b(vpm);
    // ---------------------------------------------------------------------------

    PMP::build_AABB_tree(g, tree_a, CGAL::parameters::vertex_point_map(vpm));
    PMP::build_AABB_tree(g, tree_b, CGAL::parameters::vertex_point_map(vpm_b));
    assert(tree_b.size() == num_faces(g));

    Face_location loc = PMP::locate_with_AABB_tree(p_a, tree_a, g, CGAL::parameters::vertex_point_map(vpm));

    // sanitize otherwise some test platforms fail
    PMP::internal::snap_location_to_border(loc, g, FT(1e-7));

    assert(PMP::is_on_vertex(loc, v, g)); // might fail du to precision issues...
    assert(is_equal(loc.second[CGAL::vertex_index_in_face(v, loc.first, g)], FT(1)));
    assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+1)%3], FT(0)));
    assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+2)%3], FT(0)));
    assert(is_equal(CGAL::squared_distance(to_p3(
      PMP::construct_point<FT>(loc, g, CGAL::parameters::vertex_point_map(vpm))), p3_a), FT(0)));

    loc = PMP::locate_with_AABB_tree(p_a, tree_a, g, CGAL::parameters::vertex_point_map(vpm));
    assert(is_equal(CGAL::squared_distance(to_p3(
      PMP::construct_point<FT>(loc, g, CGAL::parameters::vertex_point_map(vpm))), p3_a), FT(0)));

    // ---------------------------------------------------------------------------
    loc = PMP::locate(p_a, g, CGAL::parameters::vertex_point_map(vpm));
    assert(is_equal(CGAL::squared_distance(to_p3(
      PMP::construct_point(loc, g, CGAL::parameters::vertex_point_map(vpm))), p3_a), FT(0)));
    assert(PMP::is_in_face(loc, g));

    loc = PMP::locate_with_AABB_tree(CGAL::ORIGIN, tree_b, g, CGAL::parameters::vertex_point_map(vpm_b));
    assert(PMP::is_in_face(loc, g));

    loc = PMP::locate(CGAL::ORIGIN, g, CGAL::parameters::vertex_point_map(vpm_b));
    assert(PMP::is_in_face(loc, g));

    // ---------------------------------------------------------------------------
    Ray_2 r2 = random_2D_ray<CGAL::AABB_tree<AABB_face_graph_traits> >(tree_a, rnd);
    loc = PMP::locate_with_AABB_tree(r2, tree_a, g, CGAL::parameters::vertex_point_map(vpm));
    if(loc.first != boost::graph_traits<G>::null_face())
      assert(PMP::is_in_face(loc, g));

    Ray_3 r3 = random_3D_ray<CGAL::AABB_tree<AABB_face_graph_traits> >(tree_b, rnd);
    loc = PMP::locate_with_AABB_tree(r3, tree_b, g, CGAL::parameters::vertex_point_map(vpm_b)
                                                                     .geom_traits(K()));
  }
};

template <typename K>
struct My_3D_Point
{
  typedef typename K::FT FT;

  typedef K R; // so that we can use Kernel_traits
  typedef CGAL::Dimension_tag<3>  Ambient_dimension;
  typedef CGAL::Dimension_tag<0>  Feature_dimension;

  My_3D_Point() { }
  My_3D_Point(const CGAL::Origin& /*o*/) : cx(0), cy(0), cz(0) { }
  My_3D_Point(const FT x, const FT y, const FT z) : cx(x), cy(y), cz(z) { }

  FT x() const { return cx; }
  FT y() const { return cy; }
  FT z() const { return cz; }

private:
  FT cx, cy, cz;
};

template <typename K, typename VPM>
struct Locate_with_AABB_tree_Tester<K, VPM, 3> // 3D
{
  template <typename G>
  void test(const G& g, const VPM vpm, CGAL::Random& rnd) const
  {
    std::cout << "  test locate_with_AABB_tree (3D)..." << std::endl;

    typedef typename boost::property_traits<VPM>::reference                    Point_reference;

    typedef typename K::FT                                                     FT;
    typedef typename K::Ray_3                                                  Ray_3;

    typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
    typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
    typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

    typedef typename PMP::Face_location<G, FT>                                 Face_location;

    face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
    halfedge_descriptor h = halfedge(f, g);
    vertex_descriptor v = target(h, g);

    // ---------------------------------------------------------------------------
    typedef CGAL::AABB_face_graph_triangle_primitive<G, VPM>                   AABB_face_graph_primitive;
    typedef CGAL::AABB_traits<K, AABB_face_graph_primitive>                    AABB_face_graph_traits;

    typedef typename K::Point_3                            Point_3;
    CGAL_static_assertion((std::is_same<typename AABB_face_graph_traits::Point_3, Point_3>::value));

    CGAL::AABB_tree<AABB_face_graph_traits> tree_a;
    Point_reference p3_a = get(vpm, v);

    // below tests the case where the value type of the VPM is not Kernel::Point_3
    typedef My_3D_Point<K>                                                     Custom_point;
    typedef std::map<vertex_descriptor, Custom_point>                          Custom_map;
    typedef boost::associative_property_map<Custom_map>                        Custom_VPM;
    typedef PMP::internal::Point_to_Point_3_VPM<G, Custom_VPM>                 WrappedVPM;
    typedef CGAL::AABB_face_graph_triangle_primitive<G, WrappedVPM>            AABB_face_graph_primitive_with_WVPM;
    typedef CGAL::AABB_traits<K, AABB_face_graph_primitive_with_WVPM>          AABB_face_graph_traits_with_WVPM;

    CGAL::AABB_tree<AABB_face_graph_traits_with_WVPM> tree_b;
    Custom_map custom_map;
    for(vertex_descriptor vd : vertices(g))
    {
      const Point_reference p = get(vpm, vd);
      custom_map[vd] = Custom_point(p.x(), p.y(), p.z());
    }

    Custom_VPM custom_vpm(custom_map);
    WrappedVPM custom_vpm_3D(custom_vpm);
    // ---------------------------------------------------------------------------

    PMP::build_AABB_tree(g, tree_a); // just for the API
    assert(tree_a.size() == num_faces(g));

    PMP::build_AABB_tree(g, tree_a, CGAL::parameters::vertex_point_map(vpm));
    PMP::build_AABB_tree(g, tree_b, CGAL::parameters::vertex_point_map(custom_vpm_3D));
    assert(tree_b.size() == num_faces(g));

    Face_location loc = PMP::locate_with_AABB_tree(p3_a, tree_a, g, CGAL::parameters::vertex_point_map(vpm));
    assert(is_equal(loc.second[CGAL::vertex_index_in_face(v, loc.first, g)], FT(1)));
    assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+1)%3], FT(0)));
    assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+2)%3], FT(0)));
    assert(is_equal(CGAL::squared_distance(PMP::construct_point(loc, g), p3_a), FT(0)));

    loc = PMP::locate_with_AABB_tree(p3_a, tree_a, g, CGAL::parameters::vertex_point_map(vpm));
    assert(is_equal(CGAL::squared_distance(PMP::construct_point(loc, g), p3_a), FT(0)));

    // ---------------------------------------------------------------------------
    loc = PMP::locate(p3_a, g, CGAL::parameters::snapping_tolerance(1e-7));
    assert(is_equal(CGAL::squared_distance(PMP::construct_point(loc, g), p3_a), FT(0)));
    assert(PMP::is_in_face(loc, g));

    loc = PMP::locate_with_AABB_tree(CGAL::ORIGIN, tree_b, g, CGAL::parameters::vertex_point_map(custom_vpm_3D));
    assert(PMP::is_in_face(loc, g));

    // Doesn't necessarily have to wrap with a P_to_P3: it can be done automatically internally
    loc = PMP::locate(CGAL::ORIGIN, g, CGAL::parameters::vertex_point_map(custom_vpm));
    assert(PMP::is_in_face(loc, g));

    // ---------------------------------------------------------------------------
    Ray_3 r3 = random_3D_ray<CGAL::AABB_tree<AABB_face_graph_traits_with_WVPM> >(tree_b, rnd);
    loc = PMP::locate_with_AABB_tree(r3, tree_b, g, CGAL::parameters::vertex_point_map(custom_vpm_3D));
  }
};

template<typename K, typename G, typename VPM>
void test_locate(const G& g,
                 const VPM vpm,
                 CGAL::Random& rnd)
{
  assert(num_vertices(g) != 0 && num_faces(g) != 0);

  test_snappers<K>(g);
  test_constructions<K>(g, vpm, rnd);
  test_random_entities<K>(g, rnd);
  test_helpers<K>(g, rnd);
  test_predicates<K>(g, rnd);
  test_locate_in_face<K>(g, vpm, rnd);

  // This test has slight syntax changes between 2D and 3D (e.g. testing ray_2 in 3D makes no sense)
  Locate_with_AABB_tree_Tester<K, VPM> AABB_tester;
  AABB_tester.test(g, vpm, rnd);
}

template<typename K, typename G>
void test_locate(const G& g, CGAL::Random& rnd)
{
  return test_locate<K>(g, CGAL::get_const_property_map(boost::vertex_point, g), rnd);
}

template<typename K>
void test_2D_triangulation(const std::string fname, CGAL::Random& rnd)
{
  typedef CGAL::Regular_triangulation_2<K>                    RT;

  std::cout << "Testing Regular_triangulation_2 " << fname;

  std::ifstream in(fname);
  assert(in.good());

  RT tr;
  double x, y;
  while(in >> x >> y)
    tr.insert(typename RT::Point(x, y));

  std::ofstream out("triangulation.off");
  out << "OFF\n";
  out << tr.number_of_vertices() << " " << std::distance(tr.finite_faces_begin(), tr.finite_faces_end()) << " 0\n";

  std::size_t counter = 0;
  std::map<typename RT::Point_2, std::size_t> ids;
  for(const auto& v : CGAL::make_range(tr.finite_vertices_begin(), tr.finite_vertices_end()))
  {
    out << v.point().point() << " 0\n";
    if(ids.insert(std::make_pair(v.point().point(), counter)).second)
      ++counter;
  }

  for(const auto& fd : CGAL::make_range(tr.finite_faces_begin(), tr.finite_faces_end()))
  {
    out << "3 " << ids[fd.vertex(0)->point().point()] << " " << ids[fd.vertex(1)->point().point()] << " " << ids[fd.vertex(2)->point().point()] << "\n";
  }

  out.close();

  std::cout << " (" << tr.number_of_vertices() << " vertices)..." << std::endl;
  std::cout << "Kernel: " << typeid(K()).name() << std::endl;

  test_locate<K>(tr, rnd);
}

template<typename K>
void test_2D_surface_mesh(const std::string fname, CGAL::Random& rnd)
{
  typedef typename K::Point_2                                 Point;
  typedef CGAL::Surface_mesh<Point>                           Mesh;

  std::cout << "Testing Surface_mesh " << fname << "..." << std::endl;
  std::cout << "Kernel: " << typeid(K()).name() << std::endl;

  std::ifstream input(fname);
  assert(input.good());

  Mesh tm;
  if(!input || !(input >> tm))
  {
    std::cerr << "Error: cannot read file.";
    return;
  }

  test_locate<K>(tm, rnd);
}

template<typename K>
void test_surface_mesh_3D(const std::string fname, CGAL::Random& rnd)
{
  typedef typename K::Point_3                                 Point;
  typedef CGAL::Surface_mesh<Point>                           Mesh;

  std::cout << "Testing (3D) Surface_mesh " << fname << "..." << std::endl;
  std::cout << "Kernel: " << typeid(K()).name() << std::endl;

  std::ifstream input(fname);
  Mesh tm;
  if(!input || !(input >> tm))
  {
    std::cerr << "Error: cannot read file.";
    return;
  }

  typedef typename boost::property_map<Mesh, CGAL::vertex_point_t>::const_type  VertexPointMap;
  VertexPointMap vpm = CGAL::get_const_property_map(boost::vertex_point, tm);

  test_locate<K>(tm, vpm, rnd);
}

template<typename K>
void test_surface_mesh_projection(const std::string fname, CGAL::Random& rnd)
{
  typedef typename K::Point_3                                       Point;
  typedef CGAL::Surface_mesh<Point>                                 Mesh;
  typedef typename K::Point_2                                       Projected_point;

  typedef typename boost::graph_traits<Mesh>::vertex_descriptor     vertex_descriptor;

  std::cout << "Testing Projected Surface_mesh " << fname << "..." << std::endl;
  std::cout << "Kernel: " << typeid(K()).name() << std::endl;

  std::ifstream input(fname);
  Mesh tm;
  if(!input || !(input >> tm))
  {
    std::cerr << "Error: cannot read file.";
    return;
  }

  const auto& proj_vpm = tm.template add_property_map<typename Mesh::Vertex_index,
                                                      Projected_point>("P2", Projected_point()).first;

  for(vertex_descriptor v : vertices(tm))
  {
    const Point& p = tm.point(v);
    put(proj_vpm, v, Projected_point(p.x(), p.y()));
  }

  test_locate<K>(tm, proj_vpm, rnd);
}

template<typename K>
void test_polyhedron(const std::string fname, CGAL::Random& rnd)
{
  typedef CGAL::Polyhedron_3<K>                               Polyhedron;

  std::cout << "Testing Polyhedron_3 " << fname << "..." << std::endl;
  std::cout << "Kernel: " << typeid(K()).name() << std::endl;

  std::ifstream input(fname);
  Polyhedron poly;
  if(!input || !(input >> poly))
  {
    std::cerr << "Error: cannot read file.";
    return;
  }

  test_locate<K>(poly, rnd);
}

template <typename K>
void test(CGAL::Random& rnd)
{
  test_2D_triangulation<K>("data/stair.xy", rnd);
//  test_2D_surface_mesh<K>("data/blobby_2D.off", rnd); // temporarily disabled, until Surface_mesh's IO is "fixed"
  test_surface_mesh_3D<K>(CGAL::data_file_path("meshes/mech-holes-shark.off"), rnd);
  test_surface_mesh_projection<K>("data/unit-grid.off", rnd);
  test_polyhedron<K>("data-coref/elephant_split_2.off", rnd);
}

int main()
{
  CGAL::Set_ieee_double_precision pfr;

  std::cout.precision(17);
  std::cout << std::fixed;

//  CGAL::Random rnd(1557332474); // if needed to debug with a fixed seed
  CGAL::Random rnd(CGAL::get_default_random());

  std::cout << "The seed is " << rnd.get_seed() << std::endl;

  test<EPICK>(rnd);
  test<Exact_kernel>(rnd);

  return EXIT_SUCCESS;
}


/*
 * EigenvalueCovariance.cpp
 *
 *  Created on: May 6, 2015
 *      Author: dbazazian
 *
 *  Modified by: Milos Prokop, August 2019
 */
#include <iostream>
#include <Eigen/Eigenvalues>
#include <Eigen/Dense>
#include <vector>
#include <math.h>
#include <cmath>
#include <fstream>
#include <string>
#include <vector>
//#include <cstdlib>
#include <pcl/io/io.h>
//#include <pcl/io/pcd_io.h>
//#include <pcl/point_types.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
//#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/features/shot_omp.h>
#include "pcl/features/fpfh.h"
#include <pcl/io/ply_io.h>
#include <pcl/search/impl/kdtree.hpp>
#include <pcl/kdtree/impl/kdtree_flann.hpp>

#include <pcl/filters/statistical_outlier_removal.h>

#include "Difference_Eigenvalues.h"

#define KNN 20

using namespace std;
using namespace Eigen;

//Sigma threshold
//float threshold = 0.05;

//int main (int argc, char*argv[]){eeeeeeeeeeee
pcl::PointCloud<pcl::PointXYZ>::Ptr extract_edges(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const std::string& outputname, bool writeFile ){

    pcl::PointCloud<pcl::PointXYZ>::Ptr Normals (new pcl::PointCloud<pcl::PointXYZ>);
    Normals->resize(cloud->size());

    // K nearest neighbor search
    int KNumbersNeighbor = 10; // numbers of neighbors 7 , 120
    std::vector<int> NeighborsKNSearch(KNumbersNeighbor);
    std::vector<float> NeighborsKNSquaredDistance(KNumbersNeighbor);

    int* NumbersNeighbor = new  int [cloud ->points.size ()];
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud (cloud);
    pcl::PointXYZ searchPoint;

    double* SmallestEigen = new  double [cloud->points.size() ];
    double* MiddleEigen = new  double [cloud->points.size() ];
    double* LargestEigen = new  double [cloud->points.size() ];

    //double* DLS = new  double [cloud->points.size() ];
    //double* DLM = new  double [cloud->points.size() ];
    //double* DMS = new  double [cloud->points.size() ];
    double* Sigma = new  double [cloud->points.size() ];

    // std::vector<double> SmallestEigen;
    // std::vector<double> MiddleEigen;
    // std::vector<double> LargestEigen;
    //
    // std::vector<double> DLS;
    // std::vector<double> DML;
    // std::vector<double> DMS;

    //  ************ All the Points of the cloud *******************
    for (size_t i = 0; i < cloud ->points.size (); ++i) {

        searchPoint.x =   cloud->points[i].x;
        searchPoint.y =   cloud->points[i].y;
        searchPoint.z =   cloud->points[i].z;

        if ( kdtree.nearestKSearch (searchPoint, KNumbersNeighbor, NeighborsKNSearch, NeighborsKNSquaredDistance) > 0 ) {
                 NumbersNeighbor[i]= NeighborsKNSearch.size (); }
            else { NumbersNeighbor[i] = 0; }

        float Xmean; float Ymean; float Zmean;
        float sum= 0.00;

        // Computing Covariance Matrix
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += cloud->points[ NeighborsKNSearch[ii] ].x;

        Xmean = sum / NumbersNeighbor[i];

        sum= 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += cloud->points[NeighborsKNSearch[ii] ].y;
        Ymean = sum / NumbersNeighbor[i];

        sum= 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += cloud->points[NeighborsKNSearch[ii] ].z;
        Zmean = sum / NumbersNeighbor[i];

        float CovXX, CovXY, CovXZ, CovYX, CovYY, CovYZ, CovZX, CovZY, CovZZ;

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].x - Xmean )  );
        CovXX = sum / ( NumbersNeighbor[i]-1);

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
        sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );
        CovXY = sum / ( NumbersNeighbor[i]-1);
        CovYX = CovXY ;

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
        sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );
        CovXZ= sum / ( NumbersNeighbor[i]-1);
        CovZX = CovXZ;

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );
        CovYY = sum / ( NumbersNeighbor[i]-1);

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );
        CovYZ = sum / ( NumbersNeighbor[i]-1);
        CovZY = CovYZ;

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += ( (cloud->points[NeighborsKNSearch[ii] ].z - Zmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );
        CovZZ = sum / ( NumbersNeighbor[i]-1);

        // Computing Eigenvalue and EigenVector
        Matrix3f Cov;
        Cov << CovXX, CovXY, CovXZ, CovYX, CovYY, CovYZ, CovZX, CovZY, CovZZ;

        SelfAdjointEigenSolver<Matrix3f> eigensolver(Cov);
        if (eigensolver.info() != Success) abort();

        double EigenValue1 = eigensolver.eigenvalues()[0];
        double EigenValue2 = eigensolver.eigenvalues()[1];
        double EigenValue3 = eigensolver.eigenvalues()[2];

        double Smallest = 0.00; double Middle = 0.00; double Largest= 0.00;

        if (EigenValue1 < EigenValue2)
            Smallest =  EigenValue1;
        else
            Smallest = EigenValue2;

        if (EigenValue3 < Smallest)
            Smallest =  EigenValue3;

        if(EigenValue1 <= EigenValue2 && EigenValue1 <= EigenValue3) {
              Smallest = EigenValue1;
        if(EigenValue2 <= EigenValue3) {Middle = EigenValue2; Largest = EigenValue3;}
        else {Middle = EigenValue3; Largest = EigenValue2;}
        }

        if(EigenValue1 >= EigenValue2 && EigenValue1 >= EigenValue3)
        {
              Largest = EigenValue1;
        if(EigenValue2 <= EigenValue3) { Smallest = EigenValue2; Middle = EigenValue3; }
        else {Smallest = EigenValue3; Middle = EigenValue2;}
        }

        if ((EigenValue1 >= EigenValue2 && EigenValue1 <= EigenValue3) || (EigenValue1 <= EigenValue2 && EigenValue1 >= EigenValue3))
        {
              Middle = EigenValue1;
        if(EigenValue2 >= EigenValue3){Largest = EigenValue2; Smallest = EigenValue3;}
        else{Largest = EigenValue3; Smallest = EigenValue2;}
        }

        SmallestEigen[i]= Smallest ;
        MiddleEigen[i]= Middle;
        LargestEigen[i]= Largest;

        //DLS[i] = std::abs ( SmallestEigen[i] / LargestEigen[i]) ;          // std::abs ( LargestEigen[i] -  SmallestEigen[i] ) ;
        //DLM[i] = std::abs ( MiddleEigen[i] /  LargestEigen[i]) ;             // std::abs (  LargestEigen[i] - MiddleEigen[i] ) ;
        //DMS[i] = std::abs ( SmallestEigen[i] / MiddleEigen[i]) ;       // std::abs ( MiddleEigen[i] -  SmallestEigen[i] ) ;

        Sigma[i] = (SmallestEigen[i] ) / ( SmallestEigen[i] + MiddleEigen[i] + LargestEigen[i] ) ;
    }

    //std::cout<< " Computing Sigma is Done! " << std::endl;
    // Color Map For the difference of the eigen values

    double MaxD = 0.00;
    double MinD = std::numeric_limits<double>::max();

    int Ncolors = 256;

    for (size_t i = 0; i < cloud ->points.size (); ++i) {

          if (Sigma [i] < MinD)
              MinD= Sigma [i];

          if (Sigma[i] > MaxD)
              MaxD = Sigma [i];
    }

    //std::cout<< " Minimum is :" << MinD<< std::endl;
    //std::cout<< " Maximum  is :" << MaxD << std::endl;

	/*
	  // computing the standard deviation
	 double ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += Sigma [i] ;}
	  double avg = ss / cloud ->points.size () ;
	  ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += (Sigma [i] -  avg ) * (  Sigma [i] -  avg ) ;}
	  double stddvtion =   sqrt (  ss  /  ( cloud ->points.size () - 1 )  ) ;

	  std::cout<< " Standard Deviation is :" << stddvtion << std::endl;

	   MaxD = ( 2 )* stddvtion;
	  //MaxD = 10* stddvtion;

	  // Color table
		double line;
		double code[Ncolors][3];
	   ifstream colorcode ( "/Path/TO/ArtificialPointClouds/JetColorDensity/ColorCodes256.txt" );
	   //store color codes in array
	    int i=0,j=0;
	    while( colorcode>> line ) {
	    code[i][j]=line;
	    j++;
	    if (j == 3)
	    i++;
	}
	    code[1][0] = 0;
	    code[1][1] = 0;
	    code[1][2] = 135.468;

	    // jet color map

	int level = 0;
	float step = ( ( MaxD -  MinD) / Ncolors ) ;
	    for (size_t i = 0; i < cloud ->points.size (); ++i) {
    if (  SmallestEigen [i] <= MaxD ) {
                level = floor( (SmallestEigen [i] - MinD ) /  step ) ;

                cloud->points[i].r = code[ level ][0];
                cloud->points[i].g =  code[ level ][1];
                cloud->points[i].b =  code[ level ][2];
    } // if sigma less than Max
                }
    */

    int ePointsCount = 0;

    int level = 0;
    float step = ( ( MaxD -  MinD) / Ncolors ) ;
    //  level = floor( (Sigma [i] - MinD ) /  step ) ;
    float threshold = ( MinD + ( 6 * step) );
    //float threshold=0.1;
    //std::cout << ( MinD + ( 6 * step) ) << "\n";

    pcl::PointCloud<pcl::PointXYZ>::Ptr edgePoints (new pcl::PointCloud<pcl::PointXYZ>);

    for (size_t i = 0; i < cloud -> points.size(); ++i) {
        if ( Sigma [i] > threshold ) {

            edgePoints->points.push_back( cloud->points[i] );
            ePointsCount++;

        }
    }
    if ( writeFile ){

        pcl::PointCloud<pcl::PointXYZRGB> cloudRGB;
        pcl::copyPointCloud(*edgePoints, cloudRGB);

        for (size_t i = 0; i < cloudRGB.points.size(); ++i) {
            cloudRGB.points[i].r = 255;
            cloudRGB.points[i].g = 0;
            cloudRGB.points[i].b = 0;
        }

        pcl::PLYWriter writePLY;
        writePLY.write(outputname, cloudRGB,  true, false);
        std::cout << "File " << outputname << " written.\n";

    }

/*
   //DELETE
   pcl::PLYWriter writePLY;
   pcl::PointCloud<pcl::PointXYZRGB> cloudRGB;
   pcl::copyPointCloud(*edgePoints,cloudRGB);

   for (size_t i = 0; i < cloudRGB.points.size(); ++i) {
                cloudRGB.points[i].r = 255;
                cloudRGB.points[i].g = 0;
                cloudRGB.points[i].b = 0;

   }

  writePLY.write(outputname, cloudRGB,  true, false);

  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered (new pcl::PointCloud<pcl::PointXYZ>);

  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud (edgePoints);
  sor.setMeanK (50);
  sor.setStddevMulThresh (0.05);
  sor.filter (*filtered);

  pcl::copyPointCloud(*filtered,cloudRGB);
  writePLY.write(outputname+"_filtered.ply", cloudRGB,  true, false);

*/

    // writing the Sigma on the disk
    //	    	   		std::ofstream ofsSigma;
    //	    	   		ofsSigma.open("/Path/TO/SigmaDragon.txt");
    //	    	            for (size_t i = 0; i < cloud ->points.size (); ++i) {
    //	    	            	ofsSigma << Sigma [i]<< ","<< std::endl ;
    //	    	                    }
    //  	pcl::PLYWriter writePLY;
    //  	writePLY.write (OUTPUT_FILE, *cloud,  false);

    //return 0;

  return edgePoints;
}