/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *
 *  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.
 *   * Neither the name of Willow Garage, Inc. 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 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.
 *
 * Author : Sergey Ushakov
 * Email  : sergey.s.ushakov@mail.ru
 *
 */

#pragma once

#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/Vertices.h> // for Vertices
#include <pcl/features/feature.h>
#include <set>

namespace pcl
{
  /** \brief
    * This class implements the method for extracting RoPS features presented in the article
    * "Rotational Projection Statistics for 3D Local Surface Description and Object Recognition" by
    * Yulan Guo, Ferdous Sohel, Mohammed Bennamoun, Min Lu and Jianwei Wan.
    */
  template <typename PointInT, typename PointOutT>
  class PCL_EXPORTS ROPSEstimation : public pcl::Feature <PointInT, PointOutT>
  {
    public:

      using Feature <PointInT, PointOutT>::input_;
      using Feature <PointInT, PointOutT>::indices_;
      using Feature <PointInT, PointOutT>::surface_;
      using Feature <PointInT, PointOutT>::tree_;

      using PointCloudOut = typename pcl::Feature <PointInT, PointOutT>::PointCloudOut;
      using PointCloudIn = typename pcl::Feature <PointInT, PointOutT>::PointCloudIn;

    public:

      /** \brief Simple constructor. */
      ROPSEstimation ();

      /** \brief Virtual destructor. */
      
      ~ROPSEstimation ();

      /** \brief Allows to set the number of partition bins that is used for distribution matrix calculation.
        * \param[in] number_of_bins number of partition bins
        */
      void
      setNumberOfPartitionBins (unsigned int number_of_bins);

      /** \brief Returns the number of partition bins. */
      unsigned int
      getNumberOfPartitionBins () const;

      /** \brief This method sets the number of rotations.
        * \param[in] number_of_rotations number of rotations
        */
      void
      setNumberOfRotations (unsigned int number_of_rotations);

      /** \brief returns the number of rotations. */
      unsigned int
      getNumberOfRotations () const;

      /** \brief Allows to set the support radius that is used to crop the local surface of the point.
        * \param[in] support_radius support radius
        */
      void
      setSupportRadius (float support_radius);

      /** \brief Returns the support radius. */
      float
      getSupportRadius () const;

      /** \brief This method sets the triangles of the mesh.
        * \param[in] triangles list of triangles of the mesh
        */
      void
      setTriangles (const std::vector <pcl::Vertices>& triangles);

      /** \brief Returns the triangles of the mesh.
        * \param[out] triangles triangles of the mesh
        */
      void
      getTriangles (std::vector <pcl::Vertices>& triangles) const;

    private:

      /** \brief Abstract feature estimation method.
        * \param[out] output the resultant features
        */
      void
      computeFeature (PointCloudOut& output) override;

      /** \brief This method simply builds the list of triangles for every point.
        * The list of triangles for each point consists of indices of triangles it belongs to.
        * The only purpose of this method is to improve performance of the algorithm.
        */
      void
      buildListOfPointsTriangles ();

      /** \brief This method crops all the triangles within the given radius of the given point.
        * \param[in] point point for which the local surface is computed
        * \param[out] local_triangles stores the indices of the triangles that belong to the local surface
        * \param[out] local_points stores the indices of the points that belong to the local surface
        */
      void
      getLocalSurface (const PointInT& point, std::set <unsigned int>& local_triangles, pcl::Indices& local_points) const;

      /** \brief This method computes LRF (Local Reference Frame) matrix for the given point.
        * \param[in] point point for which the LRF is computed
        * \param[in] local_triangles list of triangles that represents the local surface of the point
        * \paran[out] lrf_matrix stores computed LRF matrix for the given point
        */
      void
      computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const;

      /** \brief This method calculates the eigen values and eigen vectors
        * for the given covariance matrix. Note that it returns normalized eigen
        * vectors that always form the right-handed coordinate system.
        * \param[in] matrix covariance matrix of the cloud
        * \param[out] major_axis eigen vector which corresponds to a major eigen value
        * \param[out] middle_axis eigen vector which corresponds to a middle eigen value
        * \param[out] minor_axis eigen vector which corresponds to a minor eigen value
        */
      void
      computeEigenVectors (const Eigen::Matrix3f& matrix, Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis,
                           Eigen::Vector3f& minor_axis) const;

      /** \brief This method translates the cloud so that the given point becomes the origin.
        * After that the cloud is rotated with the help of the given matrix.
        * \param[in] point point which stores the translation information
        * \param[in] matrix rotation matrix
        * \param[in] local_points point to transform
        * \param[out] transformed_cloud stores the transformed cloud
        */
      void
      transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const pcl::Indices& local_points, PointCloudIn& transformed_cloud) const;

      /** \brief This method rotates the cloud around the given axis and computes AABB of the rotated cloud.
        * \param[in] axis axis around which cloud must be rotated
        * \param[in] angle angle in degrees
        * \param[in] cloud cloud to rotate
        * \param[out] rotated_cloud stores the rotated cloud
        * \param[out] min stores the min point of the AABB
        * \param[out] max stores the max point of the AABB
        */
      void
      rotateCloud (const PointInT& axis, const float angle, const PointCloudIn& cloud, PointCloudIn& rotated_cloud,
                   Eigen::Vector3f& min, Eigen::Vector3f& max) const;

      /** \brief This method projects the local surface onto the XY, XZ or YZ plane
        * and computes the distribution matrix.
        * \param[in] projection represents the case of projection. 1 - XY, 2 - XZ, 3 - YZ
        * \param[in] min min point of the AABB
        * \param[in] max max point of the AABB
        * \param[in] cloud cloud containing the points of the local surface
        * \param[out] matrix stores computed distribution matrix
        */
      void
      getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const;

      /** \brief This method computes the set ofcentral moments for the given matrix.
        * \param[in] matrix input matrix
        * \param[out] moments set of computed moments
        */
      void
      computeCentralMoments (const Eigen::MatrixXf& matrix, std::vector <float>& moments) const;

    private:

      /** \brief Stores the number of partition bins that is used for distribution matrix calculation. */
      unsigned int number_of_bins_;

      /** \brief Stores number of rotations. Central moments are calculated for every rotation. */
      unsigned int number_of_rotations_;

      /** \brief Support radius that is used to crop the local surface of the point. */
      float support_radius_;

      /** \brief Stores the squared support radius. Used to improve performance. */
      float sqr_support_radius_;

      /** \brief Stores the angle step. Step is calculated with respect to number of rotations. */
      float step_;

      /** \brief Stores the set of triangles representing the mesh. */
      std::vector <pcl::Vertices> triangles_;

      /** \brief Stores the set of triangles for each point. Its purpose is to improve performance. */
      std::vector <std::vector <unsigned int> > triangles_of_the_point_;

    public:
      PCL_MAKE_ALIGNED_OPERATOR_NEW
  };
}

#define PCL_INSTANTIATE_ROPSEstimation(InT, OutT) template class pcl::ROPSEstimation<InT, OutT>;

#ifdef PCL_NO_PRECOMPILE
#include <pcl/features/impl/rops_estimation.hpp>
#endif