/*
 * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
 *
 * 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
 *       notice, 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.
 *
 * 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 OWNER 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.
 *
 * $Id: nearest.cpp 21050 2009-08-07 21:24:30Z jfaustwg $
 *
 */

/** \author Radu Bogdan Rusu */

#include <Eigen/Eigenvalues>

#include <point_cloud_mapping/geometry/angles.h>
#include <point_cloud_mapping/geometry/nearest.h>
#include <point_cloud_mapping/geometry/point.h>
#include <point_cloud_mapping/geometry/statistics.h>

#include <point_cloud_mapping/kdtree/kdtree.h>
#include <point_cloud_mapping/kdtree/kdtree_ann.h>

namespace cloud_geometry
{

  namespace nearest
  {

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the centroid of a set of points and return it as a PointCloud message with 1 value.
      * \param points the input point cloud
      * \param centroid the output centroid
      */
    void
      computeCentroid (const sensor_msgs::PointCloud &points, sensor_msgs::PointCloud &centroid)
    {
      // Prepare the data output
      centroid.points.resize (1);
      centroid.points[0].x = centroid.points[0].y = centroid.points[0].z = 0;
      centroid.channels.resize (points.get_channels_size ());
      for (unsigned int d = 0; d < points.get_channels_size (); d++)
      {
        centroid.channels[d].name = points.channels[d].name;
        centroid.channels[d].values.resize (1);
      }

      // For each point in the cloud
      for (unsigned int i = 0; i < points.get_points_size (); i++)
      {
        centroid.points[0].x += points.points[i].x;
        centroid.points[0].y += points.points[i].y;
        centroid.points[0].z += points.points[i].z;

        for (unsigned int d = 0; d < points.get_channels_size (); d++)
          centroid.channels[d].values[0] += points.channels[d].values[i];
      }

      centroid.points[0].x /= points.get_points_size ();
      centroid.points[0].y /= points.get_points_size ();
      centroid.points[0].z /= points.get_points_size ();
      for (unsigned int d = 0; d < points.get_channels_size (); d++)
        centroid.channels[d].values[0] /= points.get_points_size ();
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the centroid of a set of points using their indices and return it as a PointCloud message with 1 value.
      * \param points the input point cloud
      * \param indices the point cloud indices that need to be used
      * \param centroid the output centroid
      */
    void
      computeCentroid (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, sensor_msgs::PointCloud &centroid)
    {
      // Prepare the data output
      centroid.points.resize (1);
      centroid.points[0].x = centroid.points[0].y = centroid.points[0].z = 0;
      centroid.channels.resize (points.get_channels_size ());
      for (unsigned int d = 0; d < points.get_channels_size (); d++)
      {
        centroid.channels[d].name = points.channels[d].name;
        centroid.channels[d].values.resize (1);
      }

      // For each point in the cloud
      for (unsigned int i = 0; i < indices.size (); i++)
      {
        centroid.points[0].x += points.points.at (indices.at (i)).x;
        centroid.points[0].y += points.points.at (indices.at (i)).y;
        centroid.points[0].z += points.points.at (indices.at (i)).z;

        for (unsigned int d = 0; d < points.get_channels_size (); d++)
          centroid.channels[d].values[0] += points.channels[d].values.at (indices.at (i));
      }

      centroid.points[0].x /= indices.size ();
      centroid.points[0].y /= indices.size ();
      centroid.points[0].z /= indices.size ();
      for (unsigned int d = 0; d < points.get_channels_size (); d++)
        centroid.channels[d].values[0] /= indices.size ();
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the eigenvalues and eigenvectors of a given surface patch
      * \param points the input point cloud
      * \param eigen_vectors the resultant eigenvectors
      * \param eigen_values the resultant eigenvalues
      */
    void
      computePatchEigen (const sensor_msgs::PointCloud &points, Eigen::Matrix3d &eigen_vectors, Eigen::Vector3d &eigen_values)
    {
      geometry_msgs::Point32 centroid;
      // Compute the 3x3 covariance matrix
      Eigen::Matrix3d covariance_matrix;
      computeCovarianceMatrix (points, covariance_matrix, centroid);

      // Extract the eigenvalues and eigenvectors
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
      eigen_values = ei_symm.eigenvalues ();
      eigen_vectors = ei_symm.eigenvectors ();
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the eigenvalues and eigenvectors of a given surface patch
      * \param points the input point cloud
      * \param indices the point cloud indices that need to be used
      * \param eigen_vectors the resultant eigenvectors
      * \param eigen_values the resultant eigenvalues
      */
    void
      computePatchEigen (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, Eigen::Matrix3d &eigen_vectors, Eigen::Vector3d &eigen_values)
    {
      geometry_msgs::Point32 centroid;
      // Compute the 3x3 covariance matrix
      Eigen::Matrix3d covariance_matrix;
      computeCovarianceMatrix (points, indices, covariance_matrix, centroid);

      // Extract the eigenvalues and eigenvectors
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
      eigen_values = ei_symm.eigenvalues ();
      eigen_vectors = ei_symm.eigenvectors ();
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the eigenvalues and eigenvectors of a given surface patch from its normalized covariance matrix
      * \param points the input point cloud
      * \param eigen_vectors the resultant eigenvectors
      * \param eigen_values the resultant eigenvalues
      * \param centroid the centroid of the points
      */
    void
      computePatchEigenNormalized (const sensor_msgs::PointCloud &points, Eigen::Matrix3d &eigen_vectors, Eigen::Vector3d &eigen_values, geometry_msgs::Point32& centroid)
    {
      //geometry_msgs::Point32 centroid;
      // Compute the 3x3 covariance matrix
      Eigen::Matrix3d covariance_matrix;
      computeCovarianceMatrix (points, covariance_matrix, centroid);

      // Normalize the matrix by 1/N
      for (unsigned int i = 0 ; i < 3 ; i++)
      {
        for (unsigned int j = 0 ; j < 3 ; j++)
        {
          covariance_matrix(i, j) /= static_cast<double> (points.points.size ());
        }
      }

      // Extract the eigenvalues and eigenvectors
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
      eigen_values = ei_symm.eigenvalues ();
      eigen_vectors = ei_symm.eigenvectors ();
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the eigenvalues and eigenvectors of a given surface patch from its normalized covariance matrix
      * \param points the input point cloud
      * \param indices the point cloud indices that need to be used
      * \param eigen_vectors the resultant eigenvectors
      * \param eigen_values the resultant eigenvalues
      * \param centroid the centroid of the points
      */
    void
      computePatchEigenNormalized (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, Eigen::Matrix3d &eigen_vectors, Eigen::Vector3d &eigen_values, geometry_msgs::Point32& centroid)
    {
      //geometry_msgs::Point32 centroid;
      // Compute the 3x3 covariance matrix
      Eigen::Matrix3d covariance_matrix;
      computeCovarianceMatrix (points, indices, covariance_matrix, centroid);

      // Normalize the matrix by 1/N
      for (unsigned int i = 0 ; i < 3 ; i++)
      {
        for (unsigned int j = 0 ; j < 3 ; j++)
        {
          covariance_matrix(i, j) /= static_cast<double> (indices.size ());
        }
      }

      // Extract the eigenvalues and eigenvectors
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
      eigen_values = ei_symm.eigenvalues ();
      eigen_vectors = ei_symm.eigenvectors ();
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the Least-Squares plane fit for a given set of points, and return the estimated plane parameters
      * together with the surface curvature.
      * \param points the input point cloud
      * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0)
      * \param curvature the estimated surface curvature as a measure of
      * \f[
      * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
      * \f]
      */
    void
      computePointNormal (const sensor_msgs::PointCloud &points, Eigen::Vector4d &plane_parameters, double &curvature)
    {
      geometry_msgs::Point32 centroid;
      // Compute the 3x3 covariance matrix
      Eigen::Matrix3d covariance_matrix;
      computeCovarianceMatrix (points, covariance_matrix, centroid);

      // Extract the eigenvalues and eigenvectors
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
      Eigen::Vector3d eigen_values  = ei_symm.eigenvalues ();
      Eigen::Matrix3d eigen_vectors = ei_symm.eigenvectors ();

      // Normalize the surface normal (eigenvector corresponding to the smallest eigenvalue)
      double norm = sqrt ( eigen_vectors (0, 0) * eigen_vectors (0, 0) +
                           eigen_vectors (0, 1) * eigen_vectors (0, 1) +
                           eigen_vectors (0, 2) * eigen_vectors (0, 2));
      plane_parameters (0) = eigen_vectors (0, 0) / norm;
      plane_parameters (1) = eigen_vectors (0, 1) / norm;
      plane_parameters (2) = eigen_vectors (0, 2) / norm;

      // Hessian form (D = nc . p_plane (centroid here) + p)
      plane_parameters (3) = -1 * (plane_parameters (0) * centroid.x + plane_parameters (1) * centroid.y + plane_parameters (2) * centroid.z);

      // Compute the curvature surface change
      curvature = fabs ( eigen_values (0) / (eigen_values (0) + eigen_values (1) + eigen_values (2)) );
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices,
      * and return the estimated plane parameters together with the surface curvature.
      * \param points the input point cloud
      * \param indices the point cloud indices that need to be used
      * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0)
      * \param curvature the estimated surface curvature as a measure of
      * \f[
      * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
      * \f]
      */
    void
      computePointNormal (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, Eigen::Vector4d &plane_parameters, double &curvature)
    {
      geometry_msgs::Point32 centroid;
      // Compute the 3x3 covariance matrix
      Eigen::Matrix3d covariance_matrix;
      computeCovarianceMatrix (points, indices, covariance_matrix, centroid);

      // Extract the eigenvalues and eigenvectors
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
      Eigen::Vector3d eigen_values  = ei_symm.eigenvalues ();
      Eigen::Matrix3d eigen_vectors = ei_symm.eigenvectors ();

      // Normalize the surface normal (eigenvector corresponding to the smallest eigenvalue)
      // Note: Remember to take care of the eigen_vectors ordering
      double norm = sqrt ( eigen_vectors (0, 0) * eigen_vectors (0, 0) +
                           eigen_vectors (1, 0) * eigen_vectors (1, 0) +
                           eigen_vectors (2, 0) * eigen_vectors (2, 0));
      plane_parameters (0) = eigen_vectors (0, 0) / norm;
      plane_parameters (1) = eigen_vectors (1, 0) / norm;
      plane_parameters (2) = eigen_vectors (2, 0) / norm;

      // Hessian form (D = nc . p_plane (centroid here) + p)
      plane_parameters (3) = -1 * (plane_parameters (0) * centroid.x + plane_parameters (1) * centroid.y + plane_parameters (2) * centroid.z);

      // Compute the curvature surface change
      curvature = fabs ( eigen_values (0) / (eigen_values (0) + eigen_values (1) + eigen_values (2)) );
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices,
      * and return the estimated plane parameters together with the surface curvature.
      * \param points the input point cloud
      * \param indices the point cloud indices that need to be used
      * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0)
      * \param curvature the estimated surface curvature as a measure of
      * \f[
      * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
      * \f]
      */
    void
      computePointNormal (const sensor_msgs::PointCloudConstPtr &points, const std::vector<int> &indices, Eigen::Vector4d &plane_parameters, double &curvature)
    {
      geometry_msgs::Point32 centroid;
      // Compute the 3x3 covariance matrix
      Eigen::Matrix3d covariance_matrix;
      computeCovarianceMatrix (points, indices, covariance_matrix, centroid);

      // Extract the eigenvalues and eigenvectors
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
      Eigen::Vector3d eigen_values  = ei_symm.eigenvalues ();
      Eigen::Matrix3d eigen_vectors = ei_symm.eigenvectors ();

      // Normalize the surface normal (eigenvector corresponding to the smallest eigenvalue)
      // Note: Remember to take care of the eigen_vectors ordering
      double norm = sqrt ( eigen_vectors (0, 0) * eigen_vectors (0, 0) +
                           eigen_vectors (1, 0) * eigen_vectors (1, 0) +
                           eigen_vectors (2, 0) * eigen_vectors (2, 0));
      plane_parameters (0) = eigen_vectors (0, 0) / norm;
      plane_parameters (1) = eigen_vectors (1, 0) / norm;
      plane_parameters (2) = eigen_vectors (2, 0) / norm;

      // Hessian form (D = nc . p_plane (centroid here) + p)
      plane_parameters (3) = -1 * (plane_parameters (0) * centroid.x + plane_parameters (1) * centroid.y + plane_parameters (2) * centroid.z);

      // Compute the curvature surface change
      curvature = fabs ( eigen_values (0) / (eigen_values (0) + eigen_values (1) + eigen_values (2)) );
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the 3 moment invariants (j1, j2, j3) for a given set of points.
      * \param points the input point cloud
      * \param j1 the first moment invariant
      * \param j2 the second moment invariant
      * \param j3 the third moment invariant
      */
    void
      computeMomentInvariants (const sensor_msgs::PointCloud &points, double &j1, double &j2, double &j3)
    {
      // Compute the centroid
      geometry_msgs::Point32 centroid;
      computeCentroid (points, centroid);

      // Demean the pointset
      sensor_msgs::PointCloud points_c;
      points_c.points.resize (points.points.size ());
      for (unsigned int i = 0; i < points.points.size (); i++)
      {
        points_c.points[i].x = points.points[i].x - centroid.x;
        points_c.points[i].y = points.points[i].y - centroid.y;
        points_c.points[i].z = points.points[i].z - centroid.z;
      }

      double mu200 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 2.0, 0.0, 0.0);
      double mu020 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 0.0, 2.0, 0.0);
      double mu002 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 0.0, 0.0, 2.0);
      double mu110 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 1.0, 1.0, 0.0);
      double mu101 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 1.0, 0.0, 1.0);
      double mu011 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 0.0, 1.0, 1.0);

      j1 = mu200 + mu020 + mu002;
      j2 = mu200*mu020 + mu200*mu002 + mu020*mu002 - mu110*mu110 - mu101*mu101 - mu011*mu011;
      j3 = mu200*mu020*mu002 + 2*mu110*mu101*mu011 - mu002*mu110*mu110 - mu020*mu101*mu101 - mu200*mu011*mu011;
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the 3 moment invariants (j1, j2, j3) for a given set of points, using their indices.
      * \param points the input point cloud
      * \param indices the point cloud indices that need to be used
      * \param j1 the first moment invariant
      * \param j2 the second moment invariant
      * \param j3 the third moment invariant
      */
    void
      computeMomentInvariants (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, double &j1, double &j2, double &j3)
    {
      // Compute the centroid
      geometry_msgs::Point32 centroid;
      computeCentroid (points, indices, centroid);

      // Demean the pointset
      sensor_msgs::PointCloud points_c;
      points_c.points.resize (indices.size ());
      for (unsigned int i = 0; i < indices.size (); i++)
      {
        points_c.points[i].x = points.points.at (indices.at (i)).x - centroid.x;
        points_c.points[i].y = points.points.at (indices.at (i)).y - centroid.y;
        points_c.points[i].z = points.points.at (indices.at (i)).z - centroid.z;
      }

      double mu200 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 2.0, 0.0, 0.0);
      double mu020 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 0.0, 2.0, 0.0);
      double mu002 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 0.0, 0.0, 2.0);
      double mu110 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 1.0, 1.0, 0.0);
      double mu101 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 1.0, 0.0, 1.0);
      double mu011 = cloud_geometry::statistics::computeCentralizedMoment (points_c, 0.0, 1.0, 1.0);

      j1 = mu200 + mu020 + mu002;
      j2 = mu200*mu020 + mu200*mu002 + mu020*mu002 - mu110*mu110 - mu101*mu101 - mu011*mu011;
      j3 = mu200*mu020*mu002 + 2*mu110*mu101*mu011 - mu002*mu110*mu110 - mu020*mu101*mu101 - mu200*mu011*mu011;
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Check whether a point is a boundary point in a planar patch of projected points given by indices.
      * \note A coordinate system u-v-n must be computed a-priori using \a getCoordinateSystemOnPlane
      * \param points a pointer to the input point cloud
      * \param q_idx the index of the query point in \a points
      * \param neighbors the estimated point neighbors of the query point
      * \param u the u direction
      * \param v the v direction
      * \param angle_threshold the threshold angle (default $\pi / 2.0$)
      */
    bool
      isBoundaryPoint (const sensor_msgs::PointCloud &points, int q_idx, const std::vector<int> &neighbors,
                       const Eigen::Vector3d& u, const Eigen::Vector3d& v, double angle_threshold)
    {
      if (neighbors.size () < 3)
        return (false);
      double uvn_nn[2];
      // Compute the angles between each neighbouring point and the query point itself
      std::vector<double> angles;
      angles.reserve (neighbors.size ());
      for (unsigned int i = 0; i < neighbors.size (); i++)
      {
        uvn_nn[0] = (points.points.at (neighbors.at (i)).x - points.points.at (q_idx).x) * u (0) +
                    (points.points.at (neighbors.at (i)).y - points.points.at (q_idx).y) * u (1) +
                    (points.points.at (neighbors.at (i)).z - points.points.at (q_idx).z) * u (2);
        uvn_nn[1] = (points.points.at (neighbors.at (i)).x - points.points.at (q_idx).x) * v (0) +
                    (points.points.at (neighbors.at (i)).y - points.points.at (q_idx).y) * v (1) +
                    (points.points.at (neighbors.at (i)).z - points.points.at (q_idx).z) * v (2);
        if (uvn_nn[0] == 0 && uvn_nn[1] == 0)
          continue;
        angles.push_back (cloud_geometry::angles::getAngle2D (uvn_nn));
      }
      sort (angles.begin (), angles.end ());

      // Compute the maximal angle difference between two consecutive angles
      double max_dif = DBL_MIN, dif;
      for (unsigned int i = 0; i < angles.size () - 1; i++)
      {
        dif = angles[i + 1] - angles[i];
        if (max_dif < dif)
          max_dif = dif;
      }
      // Get the angle difference between the last and the first
      dif = 2 * M_PI - angles[angles.size () - 1] + angles[0];
      if (max_dif < dif)
        max_dif = dif;

      // Check results
      if (max_dif > angle_threshold)
        return (true);
      else
        return (false);
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Estimate (in place) the point normals and surface curvatures for a given point cloud dataset (points)
      * using the data from a different point cloud (surface) for least-squares planar estimation.
      * \param points the input point cloud (on output, 4 extra channels will be added: \a nx, \a ny, \a nz, and \a curvatures)
      * \param surface the point cloud data to use for least-squares planar estimation
      * \param k use a fixed number of k-nearest neighbors for the least-squares fit
      * \param viewpoint the viewpoint where the cloud was acquired from (used for normal flip)
      */
    void
      computePointCloudNormals (sensor_msgs::PointCloud &points, const sensor_msgs::PointCloud &surface, int k,
                                const geometry_msgs::PointStamped &viewpoint)
    {
      int nr_points = points.points.size ();
      int orig_dims = points.channels.size ();
      points.channels.resize (orig_dims + 4);                     // Reserve space for 4 channels: nx, ny, nz, curvature
      points.channels[orig_dims + 0].name = "nx";
      points.channels[orig_dims + 1].name = "ny";
      points.channels[orig_dims + 2].name = "nz";
      points.channels[orig_dims + 3].name = "curvatures";
      for (unsigned int d = orig_dims; d < points.channels.size (); d++)
        points.channels[d].values.resize (nr_points);

      cloud_kdtree::KdTree *kdtree = new cloud_kdtree::KdTreeANN (surface);

#pragma omp parallel for schedule(dynamic)
      for (int i = 0; i < nr_points; i++)                     // Get the nearest neighbors for all the point indices in the bounds
      {
        std::vector<int> nn_indices;
        std::vector<float> nn_distances;
        kdtree->nearestKSearch (points.points[i], k, nn_indices, nn_distances);

        Eigen::Vector4d plane_parameters;                     // Compute the point normals (nx, ny, nz), surface curvature estimates (c)
        double curvature;
        computePointNormal (surface, nn_indices, plane_parameters, curvature);

        cloud_geometry::angles::flipNormalTowardsViewpoint (plane_parameters, points.points[i], viewpoint);

        points.channels[orig_dims + 0].values[i] = plane_parameters (0);
        points.channels[orig_dims + 1].values[i] = plane_parameters (1);
        points.channels[orig_dims + 2].values[i] = plane_parameters (2);
        points.channels[orig_dims + 3].values[i] = curvature;
      }
      delete kdtree;                                          // Delete the kd-tree
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Estimate (in place) the point normals and surface curvatures for a given point cloud dataset (points)
      * using the data from a different point cloud (surface) for least-squares planar estimation.
      * \param points the input point cloud (on output, 4 extra channels will be added: \a nx, \a ny, \a nz, and \a curvatures)
      * \param surface the point cloud data to use for least-squares planar estimation
      * \param radius use a neighbor fixed radius search for the least-squares fit
      * \param viewpoint the viewpoint where the cloud was acquired from (used for normal flip)
      */
    void
      computePointCloudNormals (sensor_msgs::PointCloud &points, const sensor_msgs::PointCloud &surface, double radius,
                                const geometry_msgs::PointStamped &viewpoint)
    {
      int nr_points = points.points.size ();
      int orig_dims = points.channels.size ();
      points.channels.resize (orig_dims + 4);                     // Reserve space for 4 channels: nx, ny, nz, curvature
      points.channels[orig_dims + 0].name = "nx";
      points.channels[orig_dims + 1].name = "ny";
      points.channels[orig_dims + 2].name = "nz";
      points.channels[orig_dims + 3].name = "curvatures";
      for (unsigned int d = orig_dims; d < points.channels.size (); d++)
        points.channels[d].values.resize (nr_points);

      cloud_kdtree::KdTree *kdtree = new cloud_kdtree::KdTreeANN (surface);

#pragma omp parallel for schedule(dynamic)
      for (int i = 0; i < nr_points; i++)                     // Get the nearest neighbors for all the point indices in the bounds
      {
        std::vector<int> nn_indices;
        std::vector<float> nn_distances;
        kdtree->radiusSearch (points.points[i], radius, nn_indices, nn_distances);

        Eigen::Vector4d plane_parameters;                     // Compute the point normals (nx, ny, nz), surface curvature estimates (c)
        double curvature;
        computePointNormal (surface, nn_indices, plane_parameters, curvature);

        cloud_geometry::angles::flipNormalTowardsViewpoint (plane_parameters, points.points[i], viewpoint);

        points.channels[orig_dims + 0].values[i] = plane_parameters (0);
        points.channels[orig_dims + 1].values[i] = plane_parameters (1);
        points.channels[orig_dims + 2].values[i] = plane_parameters (2);
        points.channels[orig_dims + 3].values[i] = curvature;
      }
      delete kdtree;                                          // Delete the kd-tree
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Estimate (in place) the point normals and surface curvatures for a given point cloud dataset (points)
      * \param points the input point cloud (on output, 4 extra channels will be added: \a nx, \a ny, \a nz, and \a curvatures)
      * \param k use a fixed number of k-nearest neighbors for the least-squares fit
      * \param viewpoint the viewpoint where the cloud was acquired from (used for normal flip)
      */
    void
      computePointCloudNormals (sensor_msgs::PointCloud &points, int k, const geometry_msgs::PointStamped &viewpoint)
    {
      int nr_points = points.points.size ();
      int orig_dims = points.channels.size ();
      points.channels.resize (orig_dims + 4);                     // Reserve space for 4 channels: nx, ny, nz, curvature
      points.channels[orig_dims + 0].name = "nx";
      points.channels[orig_dims + 1].name = "ny";
      points.channels[orig_dims + 2].name = "nz";
      points.channels[orig_dims + 3].name = "curvatures";
      for (unsigned int d = orig_dims; d < points.channels.size (); d++)
        points.channels[d].values.resize (nr_points);

      // Peter: timing the different pieces of this
      ros::Time ts = ros::Time::now();
      cloud_kdtree::KdTree *kdtree = new cloud_kdtree::KdTreeANN (points);
      ROS_INFO("KdTree created in %f seconds", (ros::Time::now () - ts).toSec ());

      double total_search_time = 0.0, total_normal_time = 0.0;

#pragma omp parallel for schedule(dynamic)
      for (int i = 0; i < nr_points; i++)                     // Get the nearest neighbors for all the point indices in the bounds
      {
        std::vector<int> nn_indices;
        std::vector<float> nn_distances;

	ros::Time ts_search = ros::Time::now();
        kdtree->nearestKSearch (points.points[i], k, nn_indices, nn_distances);
	total_search_time += (ros::Time::now () - ts_search).toSec ();

        Eigen::Vector4d plane_parameters;                     // Compute the point normals (nx, ny, nz), surface curvature estimates (c)
        double curvature;

	ros::Time ts_normal = ros::Time::now();
        computePointNormal (points, nn_indices, plane_parameters, curvature);
	total_normal_time += (ros::Time::now () - ts_normal).toSec ();

        cloud_geometry::angles::flipNormalTowardsViewpoint (plane_parameters, points.points[i], viewpoint);

        points.channels[orig_dims + 0].values[i] = plane_parameters (0);
        points.channels[orig_dims + 1].values[i] = plane_parameters (1);
        points.channels[orig_dims + 2].values[i] = plane_parameters (2);
        points.channels[orig_dims + 3].values[i] = curvature;
      }
      ROS_INFO("Breakdown: %f seconds to search, %f seconds to compute normals", total_search_time, total_normal_time);

      delete kdtree;                                          // Delete the kd-tree
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Estimate (in place) the point normals and surface curvatures for a given point cloud dataset (points)
      * \param points the input point cloud (on output, 4 extra channels will be added: \a nx, \a ny, \a nz, and \a curvatures)
      * \param radius use a neighbor fixed radius search for the least-squares fit
      * \param viewpoint the viewpoint where the cloud was acquired from (used for normal flip)
      */
    void
      computePointCloudNormals (sensor_msgs::PointCloud &points, double radius, const geometry_msgs::PointStamped &viewpoint)
    {
      int nr_points = points.points.size ();
      int orig_dims = points.channels.size ();
      points.channels.resize (orig_dims + 4);                     // Reserve space for 4 channels: nx, ny, nz, curvature
      points.channels[orig_dims + 0].name = "nx";
      points.channels[orig_dims + 1].name = "ny";
      points.channels[orig_dims + 2].name = "nz";
      points.channels[orig_dims + 3].name = "curvatures";
      for (unsigned int d = orig_dims; d < points.channels.size (); d++)
        points.channels[d].values.resize (nr_points);

      cloud_kdtree::KdTree *kdtree = new cloud_kdtree::KdTreeANN (points);

#pragma omp parallel for schedule(dynamic)
      for (int i = 0; i < nr_points; i++)                     // Get the nearest neighbors for all the point indices in the bounds
      {
        std::vector<int> nn_indices;
        std::vector<float> nn_distances;
        kdtree->radiusSearch (points.points[i], radius, nn_indices, nn_distances);

        Eigen::Vector4d plane_parameters;                     // Compute the point normals (nx, ny, nz), surface curvature estimates (c)
        double curvature;
        computePointNormal (points, nn_indices, plane_parameters, curvature);

        cloud_geometry::angles::flipNormalTowardsViewpoint (plane_parameters, points.points[i], viewpoint);

        points.channels[orig_dims + 0].values[i] = plane_parameters (0);
        points.channels[orig_dims + 1].values[i] = plane_parameters (1);
        points.channels[orig_dims + 2].values[i] = plane_parameters (2);
        points.channels[orig_dims + 3].values[i] = curvature;
      }
      delete kdtree;                                          // Delete the kd-tree
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Estimate the point normals and surface curvatures for a given organized point cloud dataset (points)
      * using the data from a different point cloud (surface) for least-squares planar estimation.
      *
      * \note The main difference between this method and its \a computePointCloudNormals () sibblings is that
      * assumptions are being made regarding the organization of points in the cloud. For example, point clouds
      * acquired using camera sensors are assumed to be row-major, and the width and height of the depth image
      * are assumed to be known, etc.
      * \param points the input point cloud (on output, 4 extra channels will be added: \a nx, \a ny, \a nz, and \a curvatures)
      * \param surface the point cloud data to use for least-squares planar estimation
      * \param k the windowing factor (i.e., how many pixels in the depth image in all directions should the neighborhood of a point contain)
      * \param downsample_factor factor for downsampling the input data, i.e., take every Nth row and column in the depth image
      * \param width the width in pixels of the depth image
      * \param height the height in pixels of the depth image
      * \param max_z maximum distance threshold (on Z) between the query point and its neighbors (set to -1 to ignore the check)
      * \param viewpoint the viewpoint where the cloud was acquired from (used for normal flip)
      */
    void
      computeOrganizedPointCloudNormals (sensor_msgs::PointCloud &points, const sensor_msgs::PointCloud &surface,
                                         int k, int downsample_factor, int width, int height, double max_z,
                                         const geometry_msgs::Point32 &viewpoint)
    {
      // Reduce by a factor of N
      int nr_points = lrint (ceil (width / (double)downsample_factor)) *
                      lrint (ceil (height / (double)downsample_factor));
      int orig_dims = points.channels.size ();
      points.channels.resize (orig_dims + 4);                     // Reserve space for 4 channels: nx, ny, nz, curvature
      points.channels[orig_dims + 0].name = "nx";
      points.channels[orig_dims + 1].name = "ny";
      points.channels[orig_dims + 2].name = "nz";
      points.channels[orig_dims + 3].name = "curvatures";
      points.points.resize (nr_points);
      // Reserve space for 4 channels: nx, ny, nz, curvature
      for (unsigned int d = orig_dims; d < points.channels.size (); d++)
        points.channels[d].values.resize (nr_points);

      // Reserve enough space for the neighborhood
      std::vector<int> nn_indices ((k + k + 1) * (k + k + 1));
      Eigen::Vector4d plane_parameters;
      double curvature;

      int j = 0;
#pragma omp parallel for schedule(dynamic)
      for (int i = 0; i < (int)surface.points.size (); i++)
      {
        // Obtain the <u,v> pixel values
        int u = i / width;
        int v = i % width;

        // Get every Nth pixel in both rows, cols
        if ((u % downsample_factor != 0) || (v % downsample_factor != 0))
          continue;

        // Copy the data
        points.points[j] = surface.points[i];

        // Get all point neighbors in a k x k window
        int l = 0;
        for (int x = -k; x < k+1; x++)
        {
          for (int y = -k; y < k+1; y++)
          {
            int idx = (u+x) * width + (v+y);
            if (idx == i)
              continue;
            // If the index is not in the point cloud, continue
            if (idx < 0 || idx >= (int)surface.points.size ())
              continue;
            // If the difference in Z (depth) between the query point and the current neighbor is smaller than max_z
            if (max_z != -1)
            {
              if ( fabs (points.points[j].z - surface.points[idx].z) <  max_z )
                nn_indices[l++] = idx;
            }
            else
              nn_indices[l++] = idx;
          }
        }
        nn_indices.resize (l);
        if (nn_indices.size () < 4)
        {
          //ROS_ERROR ("Not enough neighboring indices found for point %d (%f, %f, %f).", i, surface.points[i].x, surface.points[i].y, surface.points[i].z);
          continue;
        }

        // Compute the point normals (nx, ny, nz), surface curvature estimates (c)
        cloud_geometry::nearest::computePointNormal (surface, nn_indices, plane_parameters, curvature);
        cloud_geometry::angles::flipNormalTowardsViewpoint (plane_parameters, surface.points[i], viewpoint);

        points.channels[orig_dims + 0].values[j] = plane_parameters (0);
        points.channels[orig_dims + 1].values[j] = plane_parameters (1);
        points.channels[orig_dims + 2].values[j] = plane_parameters (2);
        points.channels[orig_dims + 3].values[j] = curvature;
        j++;
      }
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Estimate the point normals and surface curvatures for a given organized point cloud dataset (points)
      * using the data from a different point cloud (surface) for least-squares planar estimation.
      *
      * \note The main difference between this method and its \a computePointCloudNormals () sibblings is that
      * assumptions are being made regarding the organization of points in the cloud. For example, point clouds
      * acquired using camera sensors are assumed to be row-major, and the width and height of the depth image
      * are assumed to be known, etc.
      * \param points the input point cloud (on output, 4 extra channels will be added: \a nx, \a ny, \a nz, and \a curvatures)
      * \param surface the point cloud data to use for least-squares planar estimation
      * \param k the windowing factor (i.e., how many pixels in the depth image in all directions should the neighborhood of a point contain)
      * \param downsample_factor factor for downsampling the input data, i.e., take every Nth row and column in the depth image
      * \param width the width in pixels of the depth image
      * \param height the height in pixels of the depth image
      * \param max_z maximum distance threshold (on Z) between the query point and its neighbors (set to -1 to ignore the check)
      * \param viewpoint the viewpoint where the cloud was acquired from (used for normal flip)
      */
    void
      computeOrganizedPointCloudNormals (sensor_msgs::PointCloud &points, const sensor_msgs::PointCloudConstPtr &surface,
                                         int k, int downsample_factor, int width, int height, double max_z,
                                         const geometry_msgs::Point32 &viewpoint)
    {
      // Reduce by a factor of N
      int nr_points = lrint (ceil (width / (double)downsample_factor)) *
                      lrint (ceil (height / (double)downsample_factor));
      int orig_dims = points.channels.size ();
      points.channels.resize (orig_dims + 4);                     // Reserve space for 4 channels: nx, ny, nz, curvature
      points.channels[orig_dims + 0].name = "nx";
      points.channels[orig_dims + 1].name = "ny";
      points.channels[orig_dims + 2].name = "nz";
      points.channels[orig_dims + 3].name = "curvatures";
      points.points.resize (nr_points);
      // Reserve space for 4 channels: nx, ny, nz, curvature
      for (unsigned int d = orig_dims; d < points.channels.size (); d++)
        points.channels[d].values.resize (nr_points);

      // Reserve enough space for the neighborhood
      std::vector<int> nn_indices ((k + k + 1) * (k + k + 1));
      Eigen::Vector4d plane_parameters;
      double curvature;

      int j = 0;
#pragma omp parallel for schedule(dynamic)
      for (int i = 0; i < (int)surface->points.size (); i++)
      {
        // Obtain the <u,v> pixel values
        int u = i / width;
        int v = i % width;

        // Get every Nth pixel in both rows, cols
        if ((u % downsample_factor != 0) || (v % downsample_factor != 0))
          continue;

        // Copy the data
        points.points[j] = surface->points[i];

        // Get all point neighbors in a k x k window
        int l = 0;
        for (int x = -k; x < k+1; x++)
        {
          for (int y = -k; y < k+1; y++)
          {
            int idx = (u+x) * width + (v+y);
            if (idx == i)
              continue;
            // If the index is not in the point cloud, continue
            if (idx < 0 || idx >= (int)surface->points.size ())
              continue;
            // If the difference in Z (depth) between the query point and the current neighbor is smaller than max_z
            if (max_z != -1)
            {
              if ( fabs (points.points[j].z - surface->points[idx].z) <  max_z )
                nn_indices[l++] = idx;
            }
            else
              nn_indices[l++] = idx;
          }
        }
        nn_indices.resize (l);
        if (nn_indices.size () < 4)
        {
          //ROS_ERROR ("Not enough neighboring indices found for point %d (%f, %f, %f).", i, surface->points[i].x, surface->points[i].y, surface->points[i].z);
          continue;
        }

        // Compute the point normals (nx, ny, nz), surface curvature estimates (c)
        cloud_geometry::nearest::computePointNormal (surface, nn_indices, plane_parameters, curvature);
        cloud_geometry::angles::flipNormalTowardsViewpoint (plane_parameters, surface->points[i], viewpoint);

        points.channels[orig_dims + 0].values[j] = plane_parameters (0);
        points.channels[orig_dims + 1].values[j] = plane_parameters (1);
        points.channels[orig_dims + 2].values[j] = plane_parameters (2);
        points.channels[orig_dims + 3].values[j] = curvature;
        j++;
      }
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Estimate the point normals and surface curvatures for a given organized point cloud dataset (points)
      * using the data from a different point cloud (surface) for least-squares planar estimation.
      *
      * \note The main difference between this method and its \a computePointCloudNormals () sibblings is that
      * assumptions are being made regarding the organization of points in the cloud. For example, point clouds
      * acquired using camera sensors are assumed to be row-major, and the width and height of the depth image
      * are assumed to be known, etc.
      * \param points the input point cloud (on output, 4 extra channels will be added: \a nx, \a ny, \a nz, and \a curvatures)
      * \param surface the point cloud data to use for least-squares planar estimation
      * \param k the windowing factor (i.e., how many pixels in the depth image in all directions should the neighborhood of a point contain)
      * \param downsample_factor factor for downsampling the input data, i.e., take every Nth row and column in the depth image
      * \param width the width in pixels of the depth image
      * \param height the height in pixels of the depth image
      * \param max_z maximum distance threshold (on Z) between the query point and its neighbors (set to -1 to ignore the check)
      * \param min_angle the minimum angle allowed between the viewpoint ray and the line formed by two subsequent points(used for filtering jump edge data)
      * \param max_angle the maximum angle allowed between the viewpoint ray and the line formed by two subsequent points(used for filtering jump edge data)
      * \param viewpoint the viewpoint where the cloud was acquired from (used for normal flip)
      */
    void
      computeOrganizedPointCloudNormalsWithFiltering (sensor_msgs::PointCloud &points, const sensor_msgs::PointCloud &surface,
                                                      int k, int downsample_factor, int width, int height, double max_z,
                                                      double min_angle, double max_angle,
                                                      const geometry_msgs::Point32 &viewpoint)
    {
      // Reduce by a factor of N
      int nr_points = lrint (ceil (width / (double)downsample_factor)) *
                      lrint (ceil (height / (double)downsample_factor));
      int orig_dims = points.channels.size ();
      points.channels.resize (orig_dims + 4);                     // Reserve space for 4 channels: nx, ny, nz, curvature
      points.channels[orig_dims + 0].name = "nx";
      points.channels[orig_dims + 1].name = "ny";
      points.channels[orig_dims + 2].name = "nz";
      points.channels[orig_dims + 3].name = "curvatures";
      points.points.resize (nr_points);
      // Reserve space for 4 channels: nx, ny, nz, curvature
      for (unsigned int d = orig_dims; d < points.channels.size (); d++)
        points.channels[d].values.resize (nr_points);

      // Reserve enough space for the neighborhood
      std::vector<int> nn_indices ((k + k + 1) * (k + k + 1));
      Eigen::Vector4d plane_parameters;
      double curvature;

      int j = 0;
#pragma omp parallel for schedule(dynamic)
      for (int i = 0; i < (int)surface.points.size (); i++)
      {
        // Obtain the <u,v> pixel values
        int u = i / width;
        int v = i % width;

        // Get every Nth pixel in both rows, cols
        if ((u % downsample_factor != 0) || (v % downsample_factor != 0))
          continue;

        // Copy the data
        points.points[j] = surface.points[i];

        // Get all point neighbors in a k x k window
        int l = 0;
        for (int x = -k; x < k+1; x++)
        {
          for (int y = -k; y < k+1; y++)
          {
            int idx = (u+x) * width + (v+y);
            if (idx == i)
              continue;

            // If the index is not in the point cloud, continue
            if (idx < 0 || idx >= (int)surface.points.size ())
              continue;

            // Get the angle between the lines formed with the viewpoint
            double angle = getAngleWithViewpoint (points.points[j].x, points.points[j].y, points.points[j].z,
                                                  surface.points[idx].x, surface.points[idx].y, surface.points[idx].z,
                                                  viewpoint.x, viewpoint.y, viewpoint.z);
            if (angle < min_angle || angle > max_angle)
              continue;

            // If the difference in Z (depth) between the query point and the current neighbor is smaller than max_z
            if (max_z != -1)
            {
              if ( fabs (points.points[j].z - surface.points[idx].z) <  max_z )
                nn_indices[l++] = idx;
            }
            else
              nn_indices[l++] = idx;
         }
        }
        nn_indices.resize (l);
        if (nn_indices.size () < 4)
        {
          //ROS_ERROR ("Not enough neighboring indices found for point %d (%f, %f, %f).", i, surface.points[i].x, surface.points[i].y, surface.points[i].z);
          continue;
        }

        // Compute the point normals (nx, ny, nz), surface curvature estimates (c)
        cloud_geometry::nearest::computePointNormal (surface, nn_indices, plane_parameters, curvature);
        cloud_geometry::angles::flipNormalTowardsViewpoint (plane_parameters, surface.points[i], viewpoint);

        points.channels[orig_dims + 0].values[j] = plane_parameters (0);
        points.channels[orig_dims + 1].values[j] = plane_parameters (1);
        points.channels[orig_dims + 2].values[j] = plane_parameters (2);
        points.channels[orig_dims + 3].values[j] = curvature;
        j++;
      }
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Filter jump edges in an organized point cloud dataset (e.g., acquired using TOF or dense stereo, etc)
      *
      * \NOTE the input point cloud size is assumed to have (width*height) number of points, in order!
      * \param points the input organized point cloud data
      * \param points_filtered the output filtered point cloud data
      * \param k the windowing factor (i.e., how many pixels in the depth image in all directions should the neighborhood of a point contain)
      * \param width the width in pixels of the depth image
      * \param height the height in pixels of the depth image
      * \param min_angle the minimum angle allowed between the viewpoint ray and the line formed by two subsequent points(used for filtering jump edge data)
      * \param max_angle the maximum angle allowed between the viewpoint ray and the line formed by two subsequent points(used for filtering jump edge data)
      * \param viewpoint the viewpoint where the cloud was acquired from (used for normal flip)
      */
    void
      filterJumpEdges (const sensor_msgs::PointCloud &points, sensor_msgs::PointCloud &points_filtered, int k, int width, int height,
                       double min_angle, double max_angle, const geometry_msgs::Point32 &viewpoint)
    {
      // Copy the header, and reserve the correct number of channels and points, etc
      points_filtered.header = points.header;
      points_filtered.points.resize (points.points.size ());
      points_filtered.channels.resize (points.channels.size ());
      for (unsigned int d = 0; d < points.channels.size (); d++)
      {
        points_filtered.channels[d].name = points.channels[d].name;
        points_filtered.channels[d].values.resize (points.channels[d].values.size ());
      }

      int j = 0;
#pragma omp parallel for schedule(dynamic)
      for (int i = 0; i < (int)points.points.size (); i++)
      {
        // Obtain the <u,v> pixel values
        int u = i / width;
        int v = i % width;

        // Get all point neighbors in a k x k window
        bool valid_point = true;
        for (int x = -k; x < k+1; x++)
        {
          if (!valid_point)
            break;
          for (int y = -k; y < k+1; y++)
          {
            int idx = (u+x) * width + (v+y);
            if (idx == i)
              continue;

            // If the index is not in the point cloud, continue
            if (idx < 0 || idx >= (int)points.points.size ())
              continue;

            // Get the angle between the lines formed with the viewpoint
            double angle = getAngleWithViewpoint (points.points[i].x, points.points[i].y, points.points[i].z,
                                                  points.points[idx].x, points.points[idx].y, points.points[idx].z,
                                                  viewpoint.x, viewpoint.y, viewpoint.z);
            if (angle < min_angle || angle > max_angle)
            {
              valid_point = false;
              break;
            }

          }
        }

        if (valid_point)
        {
          // Copy the data
          points_filtered.points[j] = points.points[i];
          for (unsigned int d = 0; d < points.channels.size (); d++)
            points_filtered.channels[d].values[j] = points.channels[d].values[j];
          j++;
        }
      }
      points_filtered.points.resize (j);
      for (unsigned int d = 0; d < points.channels.size (); d++)
        points_filtered.channels[d].values.resize (j);
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal
      * angular deviation
      * \NOTE: assumes normalized point normals !
      * \param points the point cloud message
      * \param indices a list of point indices to use
      * \param tolerance the spatial tolerance as a measure in the L2 Euclidean space
      * \param clusters the resultant clusters containing point indices
      * \param nx_idx the index of the channel containing the X component of the normal (use -1 to disable the check)
      * \param ny_idx the index of the channel containing the Y component of the normal (use -1 to disable the check)
      * \param nz_idx the index of the channel containing the Z component of the normal (use -1 to disable the check)
      * \param eps_angle the maximum allowed difference between normals in degrees for cluster/region growing
      *                  (only valid if nx_idx && ny_idx && nz_idx != -1)
      * \param min_pts_per_cluster minimum number of points that a cluster may contain (default = 1)
      */
    void
      extractEuclideanClusters (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, double tolerance,
                                std::vector<std::vector<int> > &clusters,
                                int nx_idx, int ny_idx, int nz_idx, double eps_angle, unsigned int min_pts_per_cluster)
    {
      // Create a tree for these points
      cloud_kdtree::KdTree* tree = new cloud_kdtree::KdTreeANN (points, indices);

      int nr_points = indices.size ();
      // Create a bool vector of processed point indices, and initialize it to false
      std::vector<bool> processed;
      processed.resize (nr_points, false);

      std::vector<int> nn_indices;
      std::vector<float> nn_distances;
      // Process all points in the indices vector
      for (int i = 0; i < nr_points; i++)
      {
        if (processed[i])
          continue;

        std::vector<int> seed_queue;
//        seed_queue.reserve (indices.size ());
        int sq_idx = 0;
        seed_queue.push_back (i);

        processed[i] = true;

        while (sq_idx < (int)seed_queue.size ())
        {
          // Search for sq_idx
          tree->radiusSearch (seed_queue.at (sq_idx), tolerance, nn_indices, nn_distances);

          for (unsigned int j = 1; j < nn_indices.size (); j++)       // nn_indices[0] should be sq_idx
          {
            if (processed.at (nn_indices[j]))                         // Has this point been processed before ?
              continue;

            processed[nn_indices[j]] = true;
            if (nx_idx != -1)                                         // Are point normals present ?
            {
              // [-1;1]
              double dot_p = points.channels[nx_idx].values[indices.at (i)] * points.channels[nx_idx].values[indices.at (nn_indices[j])] +
                             points.channels[ny_idx].values[indices.at (i)] * points.channels[ny_idx].values[indices.at (nn_indices[j])] +
                             points.channels[nz_idx].values[indices.at (i)] * points.channels[nz_idx].values[indices.at (nn_indices[j])];
              if ( fabs (acos (dot_p)) < eps_angle )
              {
                processed[nn_indices[j]] = true;
                seed_queue.push_back (nn_indices[j]);
              }
            }
            // If normal information is not present, perform a simple Euclidean clustering
            else
            {
              processed[nn_indices[j]] = true;
              seed_queue.push_back (nn_indices[j]);
            }
          }

          sq_idx++;
        }

        // If this queue is satisfactory, add to the clusters
        if (seed_queue.size () >= min_pts_per_cluster)
        {
          std::vector<int> r (seed_queue.size ());
          for (unsigned int j = 0; j < seed_queue.size (); j++)
            r[j] = indices.at (seed_queue[j]);

          sort (r.begin (), r.end ());
          r.erase (unique (r.begin (), r.end ()), r.end ());

          clusters.push_back (r);
        }
      }

      // Destroy the tree
      delete tree;
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal
      * angular deviation
      * \NOTE: assumes normalized point normals !
      * \param points the point cloud message
      * \param tolerance the spatial tolerance as a measure in the L2 Euclidean space
      * \param clusters the resultant clusters containing point indices
      * \param nx_idx the index of the channel containing the X component of the normal (use -1 to disable the check)
      * \param ny_idx the index of the channel containing the Y component of the normal (use -1 to disable the check)
      * \param nz_idx the index of the channel containing the Z component of the normal (use -1 to disable the check)
      * \param eps_angle the maximum allowed difference between normals in degrees for cluster/region growing
      *                  (only valid if nx_idx && ny_idx && nz_idx != -1)
      * \param min_pts_per_cluster minimum number of points that a cluster may contain (default = 1)
      */
    void
      extractEuclideanClusters (const sensor_msgs::PointCloud &points, double tolerance,
                                std::vector<std::vector<int> > &clusters,
                                int nx_idx, int ny_idx, int nz_idx, double eps_angle, unsigned int min_pts_per_cluster)
    {
      // Create a tree for these points
      cloud_kdtree::KdTree* tree = new cloud_kdtree::KdTreeANN (points);

      int nr_points = points.points.size ();
      // Create a bool vector of processed point indices, and initialize it to false
      std::vector<bool> processed;
      processed.resize (nr_points, false);

      std::vector<int> nn_indices;
      std::vector<float> nn_distances;
      // Process all points in the indices vector
      for (int i = 0; i < nr_points; i++)
      {
        if (processed[i])
          continue;

        std::vector<int> seed_queue;
        int sq_idx = 0;
        seed_queue.push_back (i);

        processed[i] = true;

        while (sq_idx < (int)seed_queue.size ())
        {
          // Search for sq_idx
          tree->radiusSearch (seed_queue.at (sq_idx), tolerance, nn_indices, nn_distances);

          for (unsigned int j = 1; j < nn_indices.size (); j++)       // nn_indices[0] should be sq_idx
          {
            if (processed.at (nn_indices[j]))                         // Has this point been processed before ?
              continue;

            processed[nn_indices[j]] = true;
            if (nx_idx != -1)                                         // Are point normals present ?
            {
              // [-1;1]
              double dot_p = points.channels[nx_idx].values[i] * points.channels[nx_idx].values[nn_indices[j]] +
                             points.channels[ny_idx].values[i] * points.channels[ny_idx].values[nn_indices[j]] +
                             points.channels[nz_idx].values[i] * points.channels[nz_idx].values[nn_indices[j]];
              if ( fabs (acos (dot_p)) < eps_angle )
              {
                processed[nn_indices[j]] = true;
                seed_queue.push_back (nn_indices[j]);
              }
            }
            // If normal information is not present, perform a simple Euclidean clustering
            else
            {
              processed[nn_indices[j]] = true;
              seed_queue.push_back (nn_indices[j]);
            }
          }

          sq_idx++;
        }

        // If this queue is satisfactory, add to the clusters
        if (seed_queue.size () >= min_pts_per_cluster)
        {
          std::vector<int> r (seed_queue.size ());
          for (unsigned int j = 0; j < seed_queue.size (); j++)
            r[j] = seed_queue[j];

          // Remove duplicates
          sort (r.begin (), r.end ());
          r.erase (unique (r.begin (), r.end ()), r.end ());

          clusters.push_back (r);
        }
      }

      // Destroy the tree
      delete tree;
    }

  }
}


//
//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================
//
#ifndef BOOST_MUTABLE_QUEUE_HPP
#define BOOST_MUTABLE_QUEUE_HPP

#include <vector>
#include <algorithm>
#include <functional>
#include <boost/property_map/property_map.hpp>
#include <boost/pending/mutable_heap.hpp>
#include <boost/pending/is_heap.hpp>
#include <boost/graph/detail/array_binary_tree.hpp>
#include <iterator>

namespace boost {

  // The mutable queue whose elements are indexed
  //
  // This adaptor provides a special kind of priority queue that has
  // and update operation. This allows the ordering of the items to
  // change. After the ordering criteria for item x changes, one must
  // call the Q.update(x)
  //
  // In order to efficiently find x in the queue, a functor must be
  // provided to map value_type to a unique ID, which the
  // mutable_queue will then use to map to the location of the
  // item. The ID's generated must be between 0 and N, where N is the
  // value passed to the constructor of mutable_queue

  template <class IndexedType,
            class RandomAccessContainer = std::vector<IndexedType>,
            class Comp = std::less<typename RandomAccessContainer::value_type>,
            class ID = identity_property_map >
  class mutable_queue {
  public:
    typedef IndexedType value_type;
    typedef typename RandomAccessContainer::size_type size_type;
  protected:
    typedef typename RandomAccessContainer::iterator iterator;
#if !defined BOOST_NO_STD_ITERATOR_TRAITS
    typedef array_binary_tree_node<iterator, ID> Node;
#else
    typedef array_binary_tree_node<iterator, value_type, ID> Node;
#endif
    typedef compare_array_node<RandomAccessContainer,Comp> Compare;
    typedef std::vector<size_type> IndexArray;
  public:
    typedef Compare value_compare;
    typedef ID id_generator;

    mutable_queue(size_type n, const Comp& x, const ID& _id)
      : index_array(n), comp(x), id(_id) {
      c.reserve(n);
    }
    template <class ForwardIterator>
    mutable_queue(ForwardIterator first, ForwardIterator last,
                  const Comp& x, const ID& _id)
      : index_array(std::distance(first, last)), comp(x), id(_id)
    {
      while( first != last ) {
        push(*first);
        ++first;
      }
    }

    bool empty() const { return c.empty(); }

    void pop() {
      value_type tmp = c.back();
      c.back() = c.front();
      c.front() = tmp;

      size_type id_f = get(id, c.back());
      size_type id_b = get(id, tmp);
      size_type i = index_array[ id_b ];
      index_array[ id_b ] = index_array[ id_f ];
      index_array[ id_f ] = i;

      c.pop_back();
      Node node(c.begin(), c.end(), c.begin(), id);
      down_heap(node, comp, index_array);
    }
    void push(const IndexedType& x) {
      c.push_back(x);
      /*set index-array*/
      index_array[ get(id, x) ] = c.size()-1;
      Node node(c.begin(), c.end(), c.end() - 1, id);
      up_heap(node, comp, index_array);
    }

    void update(const IndexedType& x) {
      size_type current_pos = index_array[ get(id, x) ];
      c[current_pos] = x;

      Node node(c.begin(), c.end(), c.begin()+current_pos, id);
      update_heap(node, comp, index_array);
    }

    value_type& front() { return c.front(); }
    value_type& top() { return c.front(); }

    const value_type& front() const { return c.front(); }
    const value_type& top() const { return c.front(); }

    size_type size() const { return c.size(); }

    void clear() { c.clear(); }

#if 0
        // dwa 2003/7/11 - I don't know what compiler is supposed to
        // be able to compile this, but is_heap is not standard!!
    bool test() {
      return std::is_heap(c.begin(), c.end(), Comp());
    }
#endif

   protected:
    IndexArray index_array;
    Compare comp;
    RandomAccessContainer c;
    ID id;
  };

}

#endif // BOOST_MUTABLE_QUEUE_HPP


#ifndef DATA_TYPES_HPP
#define DATA_TYPES_HPP

#define NOMINMAX // Removes windows min and max macros
#define _USE_MATH_DEFINES
#include <cmath>
#include "FAST/Exception.hpp"
#include "CL/OpenCL.hpp"
#include "FAST/ExecutionDevice.hpp"
#include <iostream>
#include <Eigen/Dense>

// These have to be outside of fast namespace or it will not compile with Qt on Windows. Why?
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;

namespace fast {

using Eigen::MatrixXf;
using Eigen::Matrix2f;
using Eigen::Matrix3f;
using Eigen::Matrix4f;
using Eigen::VectorXf;
using Eigen::VectorXi;
using Eigen::Vector4f;
using Eigen::Vector3f;
using Eigen::Vector2f;
using Eigen::Vector4i;
using Eigen::Vector3i;
using Eigen::Vector2i;
using Eigen::Affine3f;
typedef Eigen::Matrix<uint, Eigen::Dynamic, 1> VectorXui;
typedef Eigen::Matrix<uint, 4, 1> Vector4ui;
typedef Eigen::Matrix<uint, 3, 1> Vector3ui;
typedef Eigen::Matrix<uint, 2, 1> Vector2ui;

enum DataType {
    TYPE_FLOAT,
    TYPE_UINT8,
    TYPE_INT8,
    TYPE_UINT16,
    TYPE_INT16,
    TYPE_UNORM_INT16, // Unsigned normalized 16 bit integer. A 16 bit int interpreted as a float between 0 and 1.
    TYPE_SNORM_INT16 // Signed normalized 16 bit integer. A 16 bit int interpreted as a float between -1 and 1.
};

enum PlaneType {PLANE_X, PLANE_Y, PLANE_Z};

// Returns the C type for a DataType as a string
FAST_EXPORT std::string getCTypeAsString(DataType type);

#define fastCaseTypeMacro(fastType, cType, call) case fastType: {typedef cType FAST_TYPE; call;} break;

#define fastSwitchTypeMacro(call) \
        fastCaseTypeMacro(TYPE_FLOAT, float, call) \
        fastCaseTypeMacro(TYPE_INT8, char, call) \
        fastCaseTypeMacro(TYPE_UINT8, uchar, call) \
        fastCaseTypeMacro(TYPE_INT16, short, call) \
        fastCaseTypeMacro(TYPE_UINT16, ushort, call) \
        fastCaseTypeMacro(TYPE_SNORM_INT16, short, call) \
        fastCaseTypeMacro(TYPE_UNORM_INT16, ushort, call) \

FAST_EXPORT cl::ImageFormat getOpenCLImageFormat(OpenCLDevice::pointer, cl_mem_object_type imageType, DataType type, unsigned int components);

FAST_EXPORT size_t getSizeOfDataType(DataType type, unsigned int nrOfComponents);

FAST_EXPORT float getDefaultIntensityLevel(DataType type);
FAST_EXPORT float getDefaultIntensityWindow(DataType type);

FAST_EXPORT void deleteArray(void * data, DataType type);

} // end namespace
#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_POINT_ON_LINE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_POINT_ON_LINE_HPP

#include <boost/geometry/algorithms/distance.hpp>

namespace boost { namespace geometry
{

//----------------------------------------------------------------------
// Function     : point_on_linestring -> rename to alongLine NO, different
//----------------------------------------------------------------------
// Purpose      : Calculates coordinates of a point along a given line
//                on a specified distance
// Parameters   : const L& : line,
//                float position: position to calculate point
//                P& point: point to calculate
// Return       : true if point lies on line
//----------------------------------------------------------------------
// Author       : Barend, Geodan BV Amsterdam
// Date         : spring 1996
//----------------------------------------------------------------------
template <typename P, typename L>
bool point_on_linestring(L const& line, double const& position, P& point)
{
    double current_distance = 0.0;
    if (line.size() < 2)
    {
        return false;
    }

    typename L::const_iterator vertex = line.begin();
    typename L::const_iterator previous = vertex++;

    while (vertex != line.end())
    {
        double const dist = distance(*previous, *vertex);
        current_distance += dist;

        if (current_distance > position)
        {
            // It is not possible that dist == 0 here because otherwise
            // the current_distance > position would not become true (current_distance is increased by dist)
            double const fraction = 1.0 - ((current_distance - position) / dist);

            // point i is too far, point i-1 to near, add fraction of
            // distance in each direction
            point.x ( previous->x() + (vertex->x() - previous->x()) * fraction);
            point.y ( previous->y() + (vertex->y() - previous->y()) * fraction);

            return true;
        }
        previous = vertex++;
    }

    // point at specified position does not lie on line
    return false;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_POINT_ON_LINE_HPP


#include <iostream>
#include <fstream>
#include <string>
#include <boost/filesystem.hpp>
#include <vector>

// mapnik
#include <mapnik/map.hpp>
#include <mapnik/load_map.hpp>
#include <mapnik/value_error.hpp>
#include <mapnik/datasource_cache.hpp>
#include <mapnik/box2d.hpp>
#include <mapnik/proj_transform.hpp>
#include <mapnik/image_util.hpp>

// mapnik vector tiles generation
#include <vector_tile.pb.h>
#include <vector_tile_projection.hpp>
#include <vector_tile_processor.hpp>
#include <vector_tile_backend_pbf.hpp>
#include <vector_tile_datasource.hpp>
#include <vector_tile_util.hpp>
#include <vector_tile_compression.hpp>

using namespace std;
using namespace mapnik;

static int compression = 0;
static int tile_size = 256;

void create_tiles(int z, int maxz, int x, int y, Map tile_map) ;
void create_single_tile(int z, int x, int y, Map tile_map) ;
void create_path(int z, int x) ;

int main (int argc, char* argv[]) {
    int minz, maxz, x, y;
    string style_path;

    for (int i=0 ; i<argc ; i++){
        if (compression)
            argv[i - compression] = argv[i];
        if (strncmp(argv[i], "--compress", 10) == 0)
            compression ++;
    }

    if (argc != 6 && argc != 7) {
        clog << "command: " << argv[0] << "[--compress] minz maz x y path/to/stylesheet.xml\n";
        clog << "    where x and y are the values at the minz zoom_level\n";
        return EXIT_FAILURE;
    }

    minz = atoi(argv[1]);
    maxz = atoi(argv[2]);
    x = atoi(argv[3]);
    y = atoi(argv[4]);
    style_path = argv[5];

    //add plugins to be able to load the shapefile (and also other formats) in load_map()
    datasource_cache::instance().register_datasources("/usr/lib/mapnik/input/");

    GOOGLE_PROTOBUF_VERIFY_VERSION;

    // initialize map with the data
    Map tile_map(tile_size, tile_size, "900913");
    load_map(tile_map, style_path, true);

    create_path(minz, x);
    create_tiles(maxz, minz, x, y, tile_map);
    cout << "Tiles created.\n";

    google::protobuf::ShutdownProtobufLibrary();

    return EXIT_SUCCESS;
}

/*
    create the directories where the tiles will be generated
*/
void create_path(int z, int x) {
    string pathname = to_string(z) + "/" + to_string(x);
    boost::filesystem::path path(pathname);
    boost::filesystem::create_directories(pathname);
}

/*
    create all tiles and paths recursively
*/
void create_tiles(int maxz, int z, int x, int y, Map tile_map) {

    //create the current tile
    create_single_tile(z, x, y, tile_map);

    if (z+1 <= maxz) {
        create_path(z+1, 2*x);
        create_path(z+1, 2*x+1);

        //recursively create the four subtiles (at the next zoom level) of this tile
        create_tiles(maxz, z+1, 2*x,   2*y  , tile_map);
        create_tiles(maxz, z+1, 2*x+1, 2*y  , tile_map);
        create_tiles(maxz, z+1, 2*x,   2*y+1, tile_map);
        create_tiles(maxz, z+1, 2*x+1, 2*y+1, tile_map);
    }
}

/*
    creation of a single tile
*/
void create_single_tile(int z, int x, int y, Map tile_map) {
    int path_multiplier = 16;
    double minx, miny, maxx, maxy;
    string buffer, compressed;

    // bounding box
    mapnik::vector_tile_impl::spherical_mercator merc(tile_size);
    merc.xyz(x, y, z, minx, miny, maxx, maxy);

    box2d<double> bbox;
    bbox.init(minx, miny, maxx, maxy);

    tile_map.zoom_to_box(bbox);

    vector_tile::Tile tile;
    mapnik::vector_tile_impl::backend_pbf backend(tile, path_multiplier);
    request mapnik_request(tile_size, tile_size, bbox);
    mapnik::vector_tile_impl::processor<mapnik::vector_tile_impl::backend_pbf> ren(backend, tile_map, mapnik_request);

    ren.apply();

    tile.SerializeToString(&buffer);
    if (compression) {
        mapnik::vector_tile_impl::zlib_compress(buffer, compressed);
        buffer = compressed;
        cout << "Creating compressed tile for z = " << z << " ; x = " << x << " ; y = " << y << "\n" ;
    } else {
        cout << "Creating tile for z = " << z << " ; x = " << x << " ; y = " << y << "\n";
    }
    ofstream output(to_string(z) + "/" + to_string(x) + "/" + to_string(y) + ".pbf");
    output << buffer ;
}


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Graph/PrivateGraph.h"

#include "Molassembler/Temple/Functional.h"

#include "Molassembler/IO/SmilesParser.h"
#include "Molassembler/Molecule.h"
#include "Molassembler/Graph.h"

using namespace Scine;
using namespace Molassembler;

BOOST_AUTO_TEST_CASE(SplitGraph, *boost::unit_test::label("Molassembler")) {
  PrivateGraph methane(4);
  methane.elementType(0) = Utils::ElementType::C;
  methane.elementType(1) = Utils::ElementType::H;
  methane.elementType(2) = Utils::ElementType::H;
  methane.elementType(3) = Utils::ElementType::H;
  auto bridge = methane.addEdge(0, 1, BondType::Single);
  methane.addEdge(0, 2, BondType::Single);
  methane.addEdge(0, 3, BondType::Single);

  auto sides = methane.splitAlongBridge(bridge);

  BOOST_CHECK(
    std::find(
      std::begin(sides.first),
      std::end(sides.first),
      boost::source(bridge, methane.bgl())
    ) != std::end(sides.first)
  );

  BOOST_CHECK(
    std::find(
      std::begin(sides.second),
      std::end(sides.second),
      boost::target(bridge, methane.bgl())
    ) != std::end(sides.second)
  );
}

BOOST_AUTO_TEST_CASE(BridgeEdges, *boost::unit_test::label("Molassembler")) {
  PrivateGraph ethane(8);
  ethane.elementType(0) = Utils::ElementType::C;
  ethane.elementType(1) = Utils::ElementType::C;
  ethane.addEdge(0, 1, BondType::Single);
  for(unsigned i = 2; i < 8; ++i) {
    ethane.elementType(i) = Utils::ElementType::H;
  }
  for(unsigned i = 2; i < 5; ++i) {
    ethane.addEdge(0, i, BondType::Single);
  }
  for(unsigned i = 5; i < 8; ++i) {
    ethane.addEdge(1, i, BondType::Single);
  }

  // All edges must be bridge edges
  BOOST_CHECK(
    Temple::all_of(
      ethane.edges(),
      [&](const auto& e) -> bool {
        return !ethane.canRemove(e);
      }
    )
  );

  // All carbons must be articulation vertices
  BOOST_CHECK(!ethane.canRemove(0) && !ethane.canRemove(1));

  // And again from SMILES
  auto e = IO::Experimental::parseSmilesSingleMolecule("CC");
  BOOST_CHECK(e.graph().adjacent(0, 1));
  BOOST_CHECK(!e.graph().canRemove(BondIndex {0, 1}));
}


/**
 * \copyright
 * Copyright (c) 2012-2021, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include <gtest/gtest.h>

#include <Eigen/Eigen>
#include <cmath>
#include <memory>

#include "BaseLib/FileTools.h"
#include "InfoLib/TestInfo.h"
#include "MeshLib/Mesh.h"
#include "MeshLib/MeshGenerators/MeshGenerator.h"
#include "ProcessLib/ComponentTransport/CreateLookupTable.h"
#include "ProcessLib/ComponentTransport/LookupTable.h"
#include "ProcessLib/ProcessVariable.h"
#include "Tests/TestTools.h"

using namespace ProcessLib::ComponentTransport;

namespace
{
std::unique_ptr<LookupTable> createTestLookupTable(const char xml[])
{
    auto const ptree = Tests::readXml(xml);
    BaseLib::ConfigTree conf(ptree, "", BaseLib::ConfigTree::onerror,
                             BaseLib::ConfigTree::onwarning);

    std::vector<std::unique_ptr<MeshLib::Mesh>> meshes;
    {
        std::size_t const n_elements = 4;
        auto mesh = std::unique_ptr<MeshLib::Mesh>(
            MeshLib::MeshGenerator::generateRegularQuadMesh(1.0, n_elements));
        meshes.push_back(std::move(mesh));
    }

    std::map<std::string,
             std::unique_ptr<MathLib::PiecewiseLinearInterpolation>>
        curves;

    std::vector<std::unique_ptr<ParameterLib::ParameterBase>> parameters;
    {
        auto const& para_config = conf.getConfigSubtree("parameter");
        auto p = ParameterLib::createParameter(para_config, meshes, curves);
        parameters.push_back(std::move(p));
    }

    std::vector<ProcessLib::ProcessVariable> variables;
    auto const& process_variables_config =
        conf.getConfigSubtree("process_variables");
    for (auto var_config :
         process_variables_config.getConfigSubtreeList("process_variable"))
    {
        variables.emplace_back(var_config, *meshes[0], meshes, parameters,
                               curves);
    }

    std::vector<
        std::vector<std::reference_wrapper<ProcessLib::ProcessVariable>>>
        process_variables;
    {
        for (auto& pv : variables)
        {
            std::vector<std::reference_wrapper<ProcessLib::ProcessVariable>>
                per_process_variable;

            per_process_variable.push_back(pv);
            process_variables.push_back(std::move(per_process_variable));
        }
    }

    std::optional<std::string> file_name(TestInfoLib::TestInfo::data_path +
                                         "/FileIO/sparse_table.txt");
    if (!BaseLib::isProjectDirectorySet())
    {
        BaseLib::setProjectDirectory(BaseLib::extractPath(*file_name));
    }

    return createLookupTable(file_name, process_variables);
}

}  // namespace

TEST(ComponentTransport, checkLookupTable)
{
    const char xml[] =
        "<parameter>"
        "     <name>c0</name>"
        "     <type>Constant</type>"
        "     <value>0</value>"
        "</parameter>"
        "<process_variables>"
        "    <process_variable>"
        "        <name>Ni</name>"
        "        <components>1</components>"
        "        <order>1</order>"
        "        <initial_condition>c0</initial_condition>"
        "        <boundary_conditions>"
        "        </boundary_conditions>"
        "    </process_variable>"
        "    <process_variable>"
        "        <name>Np(5)</name>"
        "        <components>1</components>"
        "        <order>1</order>"
        "        <initial_condition>c0</initial_condition>"
        "        <boundary_conditions>"
        "        </boundary_conditions>"
        "    </process_variable>"
        "    <process_variable>"
        "        <name>Th</name>"
        "        <components>1</components>"
        "        <order>1</order>"
        "        <initial_condition>c0</initial_condition>"
        "        <boundary_conditions>"
        "        </boundary_conditions>"
        "    </process_variable>"
        "    <process_variable>"
        "        <name>Ra</name>"
        "        <components>1</components>"
        "        <order>1</order>"
        "        <initial_condition>c0</initial_condition>"
        "        <boundary_conditions>"
        "        </boundary_conditions>"
        "    </process_variable>"
        "</process_variables>";

    auto const lookup_table = createTestLookupTable(xml);
    auto const& tabular_data = lookup_table->tabular_data;

    ASSERT_EQ(1.e-5, tabular_data.at("Ni")[0]);
    ASSERT_EQ(1e-9, tabular_data.at("Ni_prev")[0]);
    ASSERT_EQ(0., tabular_data.at("Ni_new")[0]);

    ASSERT_EQ(1.e-6, tabular_data.at("Np(5)")[0]);
    ASSERT_EQ(1.e-10, tabular_data.at("Np(5)_prev")[0]);
    ASSERT_EQ(0., tabular_data.at("Np(5)_new")[0]);

    ASSERT_EQ(1.e-7, tabular_data.at("Th")[0]);
    ASSERT_EQ(1.e-11, tabular_data.at("Th_prev")[0]);
    ASSERT_EQ(0., tabular_data.at("Th_new")[0]);

    ASSERT_EQ(1.e-8, tabular_data.at("Ra")[0]);
    ASSERT_EQ(1.e-12, tabular_data.at("Ra_prev")[0]);
    ASSERT_EQ(9.977098263915e-13, tabular_data.at("Ra_new")[0]);
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_UTILITIES_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_UTILITIES_HPP

#include <boost/geometry/strategies/distance.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace closest_points
{

struct set_segment_from_points
{
    template <typename Point, typename Segment>
    static inline void apply(Point const& p1, Point const& p2, Segment& segment)
    {
        assign_point_to_index<0>(p1, segment);
        assign_point_to_index<1>(p2, segment);
    }
};

struct swap_segment_points
{
    template <typename Segment>
    static inline void apply(Segment& segment)
    {
        geometry::detail::for_each_dimension<Segment>([&](auto index)
        {
            auto temp = get<0,index>(segment);
            set<0,index>(segment, get<1,index>(segment));
            set<1,index>(segment, temp);
        });
    }
};

template <typename Geometry1, typename Geometry2, typename Strategies>
using distance_strategy_t = decltype(
    std::declval<Strategies>().distance(std::declval<Geometry1>(), std::declval<Geometry2>()));

template <typename Geometry1, typename Geometry2, typename Strategies>
using creturn_t = typename strategy::distance::services::return_type
    <
        typename strategy::distance::services::comparable_type
            <
                distance_strategy_t<Geometry1, Geometry2, Strategies>
            >::type,
        typename point_type<Geometry1>::type,
        typename point_type<Geometry2>::type
    >::type;

}} // namespace detail::closest_points

}} // namespace boost::geometry

#endif //BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_UTILITIES_HPP


#include "stdafx.h"
#include <random>
#include <iostream>
#include <fstream>
#include <set>
#include <string>
#include <chrono>
#include <cairo\cairo.h>
#include <gtk\gtk.h>
#include <boost\tokenizer.hpp>
#include "Agent.h"
#include "Target.h"
#include "Obstacle.h"
#include "PSystem.h"

std::string environment_file = "environments/sparse.csv";

int population_cap = 50;
float spawn_chance = 0.2f;
std::vector<Vector3> spawn_locations;

int target_count = 2;

int window_width = 800;
int window_height = 600;

int canvas_width = 0;
int canvas_height = 0;

float graph_scale = 25;

std::set<Agent*> units;
std::vector<Target*> targets;
std::vector<Obstacle*> obstacles;

GtkWidget *drawing_area;
guint update_timer;

std::random_device rd;
std::mt19937 gen(rd());
std::uniform_real_distribution<> spawn_chance_distribution(0, 1.0);
std::uniform_real_distribution<> spawn_location_distribution(-5.0, 5.0);

using TimePoint = std::chrono::time_point<std::chrono::steady_clock>;
using TimeDuration = std::chrono::duration<double>;
TimePoint start_time;

double benchmark_duration = 120.0;
bool benchmark_complete = true;

bool render = true;

unsigned int units_completed = 0;

static void do_drawing(cairo_t *cr)
{
	cairo_set_source_rgb(cr, 0, 0, 0);

	for (auto unit: units)
	{
		unit->Draw(cr);
	}
	for (auto target : targets) {
		target->Draw(cr);
	}
	for (auto obstacle : obstacles) {
		obstacle->Draw(cr);
	}
}

static gboolean draw_callback(GtkWidget *widget, cairo_t *cr, gpointer data) {
	if (render) do_drawing(cr);
	return FALSE;
}

void update_population() {
	std::vector<Agent*> to_remove;
	for (auto u : units)
	{
		if (u->IsComplete()) to_remove.push_back(u);
		u->Update();
	}
	PSystem::GetInstance().UpdateInteractions();
	for (auto u : to_remove)
	{
		units_completed++;
		units.erase(u);
	}

	if (spawn_locations.empty()) return;

	if (units.size() < population_cap && spawn_chance_distribution(gen) <= spawn_chance) {
		Vector3 random_position = spawn_locations.at(std::rand() % spawn_locations.size());
		Agent* u = new Agent(random_position[0] + spawn_location_distribution(gen), random_position[1] + spawn_location_distribution(gen));

		int random_target_index = std::rand() % target_count;
		Target* random_target = targets.at(random_target_index);
		u->SetTarget(random_target->x, random_target->y);
		units.emplace(u);
	}
}

void print_statistics() {
	std::cout << PSystem::GetInstance().Stats() << std::endl;
	std::cout << "Agent Tasks Completed: " << std::to_string(units_completed) << std::endl;
	PSystem::GetInstance().ResetStats();
}

static gboolean tick(GtkWidget* widget) {
	GtkAllocation alloc;
	gtk_widget_get_allocation(widget, &alloc);
	canvas_width = alloc.width;
	canvas_height = alloc.height;

	if (benchmark_complete) return TRUE;

	TimePoint current_time = std::chrono::high_resolution_clock::now();
	TimeDuration elapsed = current_time - start_time;
	if (elapsed.count() >= benchmark_duration) {
		print_statistics();
		benchmark_complete = true;
		return TRUE;
	}

	update_population();

	gtk_widget_queue_draw_area(widget, 0, 0, alloc.width, alloc.height);
	return TRUE;
}

void init_graph() {
	PSystem::GetInstance().CreateLayer(0U);
	Vector3 origin{ 0, 0, 0 };

	PGraph* graph = nullptr;
	float graph_width;
	float graph_height;
	std::vector<Vector3> obstacle_positions;
	spawn_locations.clear();

	if (environment_file == "") {
		graph_width = static_cast<float>(canvas_width);
		graph_height = static_cast<float>(canvas_height);
	}
	else {
		std::string line;
		std::ifstream file(environment_file);
		if (file.is_open()) {
			std::vector<std::vector<bool>> file_data;
			while (getline(file, line)) {
				std::vector<bool> line_data;
				boost::tokenizer<boost::escaped_list_separator <char>> tokenizer{ line };
				for (const auto &t : tokenizer) {
					line_data.push_back(t == "1");
				}
				file_data.push_back(line_data);
			}
			int rows = static_cast<int>(file_data.size());
			int columns = static_cast<int>(file_data.at(0).size());
			std::cout << "Loaded file - " << rows << "x" << columns << std::endl;
			graph_height = (rows - 1) * graph_scale;
			graph_width = (columns - 1) * graph_scale;
			for (int y = 0; y < rows; y++)
			{
				for (int x = 0; x < columns; x++)
				{
					if (file_data.at(y).at(x)) obstacle_positions.push_back(Vector3{ x * graph_scale,y * graph_scale,0 });
					else spawn_locations.push_back(Vector3{ x * graph_scale,y * graph_scale,0 });
				}
			}
		}
		else {
			std::cout << "Error loading .csv" << std::endl;
			graph_width = static_cast<float>(canvas_width);
			graph_height = static_cast<float>(canvas_height);
		}
	}

	Vector3 dimensions{ graph_width , graph_height , 0 };
	graph = PSystem::GetInstance().InitGraph(0U, origin, dimensions, graph_scale);

	for (size_t i = 0; i < obstacle_positions.size(); i++)
	{
		Vector3 pos = obstacle_positions.at(i);
		Obstacle* cairo_obstacle = new Obstacle(pos[0],pos[1],graph_scale,graph_scale);
		obstacles.push_back(cairo_obstacle);
		graph->NodeAt(pos)->obstacle = true;
	}
}

void init_targets() {
	for (int i = 0; i < targets.size(); i++) {
		delete targets.at(i);
	}
	targets.clear();

	for (int i = 0; i < target_count; i++)
	{
		Vector3 random_position = spawn_locations.at(std::rand() % spawn_locations.size());
		targets.push_back(new Target(random_position[0] + static_cast<float>(spawn_location_distribution(gen)), random_position[1] + static_cast<float>(spawn_location_distribution(gen))));
	}
}

void reset_units() {
	for (auto u : units)
	{
		delete u;
	}
	units.clear();
}

static void reset(GtkWidget* widget, gpointer data) {
	init_graph();
	init_targets();
	reset_units();
	units_completed = 0;
	benchmark_complete = false;
	start_time = std::chrono::high_resolution_clock::now();
}

int main(int argc, char *argv[])
{
	GtkWidget *window;
	GtkWidget *main_container;
	GtkWidget *action_bar;
	GtkWidget *reset_button;

	gtk_init(&argc, &argv);

	window = gtk_window_new(GTK_WINDOW_TOPLEVEL);

	main_container = gtk_box_new(GTK_ORIENTATION_VERTICAL, 0);
	gtk_container_add(GTK_CONTAINER(window), main_container);

	action_bar = gtk_action_bar_new();

	reset_button = gtk_button_new_with_label("Reset");

	gtk_action_bar_pack_start((GtkActionBar*)action_bar, reset_button);
	gtk_box_pack_start((GtkBox*)main_container, action_bar, FALSE, FALSE, 0);

	drawing_area = gtk_drawing_area_new();
	gtk_box_pack_start((GtkBox*)main_container, drawing_area, TRUE, TRUE, 0);

	g_signal_connect(G_OBJECT(drawing_area), "draw", G_CALLBACK(draw_callback), NULL);
	g_signal_connect(window, "destroy", G_CALLBACK(gtk_main_quit), NULL);
	g_signal_connect(reset_button, "clicked", G_CALLBACK(reset), NULL);

	update_timer = g_timeout_add_full(G_PRIORITY_HIGH,17, (GSourceFunc)tick, drawing_area,NULL);

	gtk_window_set_position(GTK_WINDOW(window), GTK_WIN_POS_CENTER);
	gtk_window_set_default_size(GTK_WINDOW(window), window_width, window_height);
	gtk_window_set_title(GTK_WINDOW(window), "Pathfinding Examples");

	gtk_widget_show_all(window);

	srand(time(NULL));

	gtk_main();

	return 0;
}