/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2014-, Open Perception, Inc.
 *
 *  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 the copyright holder(s) 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.
 *
 */

#pragma once

// common includes
#include <pcl/pcl_base.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

// segmentation and sample consensus includes
#include <pcl/segmentation/supervoxel_clustering.h>
#include <pcl/segmentation/lccp_segmentation.h>
#include <pcl/sample_consensus/sac.h>

#include <pcl/segmentation/extract_clusters.h>

#define PCL_INSTANTIATE_CPCSegmentation(T) template class PCL_EXPORTS pcl::CPCSegmentation<T>;

namespace pcl
{
  /** \brief A segmentation algorithm partitioning a supervoxel graph. It uses planar cuts induced by local concavities for the recursive segmentation. Cuts are estimated using locally constrained directed RANSAC.
   *  \note If you use this in a scientific work please cite the following paper:
   *  M. Schoeler, J. Papon, F. Woergoetter
   *  Constrained Planar Cuts - Object Partitioning for Point Clouds
   *  In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2015
   *  Inherits most of its functionality from \ref LCCPSegmentation
   *  \author Markus Schoeler (mschoeler@web.de)
   *  \ingroup segmentation
   */
  template <typename PointT>
  class CPCSegmentation : public LCCPSegmentation<PointT>
  {
      using WeightSACPointType = PointXYZINormal;
      using LCCP = LCCPSegmentation<PointT>;
      // LCCP typedefs
      using EdgeID = typename LCCP::EdgeID;
      using EdgeIterator = typename LCCP::EdgeIterator;
      // LCCP methods
      using LCCP::calculateConvexConnections;
      using LCCP::applyKconvexity;
      using LCCP::doGrouping;
      using LCCP::mergeSmallSegments;
      // LCCP variables
      using LCCP::sv_adjacency_list_;
      using LCCP::k_factor_;
      using LCCP::grouping_data_valid_;
      using LCCP::sv_label_to_seg_label_map_;
      using LCCP::sv_label_to_supervoxel_map_;
      using LCCP::concavity_tolerance_threshold_;
      using LCCP::seed_resolution_;
      using LCCP::supervoxels_set_;

    public:
      CPCSegmentation ();

      ~CPCSegmentation ();

      /** \brief Merge supervoxels using cuts through local convexities. The input parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref relabelCloud method.
       *  \note There are three ways to retrieve the segmentation afterwards (inherited from \ref LCCPSegmentation): \ref relabelCloud, \ref getSupervoxelToSegmentMap and \ref getSupervoxelToSegmentMap */
      void
      segment ();

      /** \brief Determines if we want to use cutting planes
       *  \param[in] max_cuts Maximum number of cuts
       *  \param[in] cutting_min_segments Minimum segment size for cutting
       *  \param[in] cutting_min_score Minimum score a proposed cut has to achieve for being performed
       *  \param[in] locally_constrained Decide if we constrain our cuts locally
       *  \param[in] directed_cutting Decide if we prefer cuts perpendicular to the edge-direction
       *  \param[in] clean_cutting Decide if we cut only edges with supervoxels on opposite sides of the plane (clean) or all edges within the seed_resolution_ distance to the plane (not clean). The later was used in the paper.
       */
      inline void
      setCutting (const std::uint32_t max_cuts = 20,
                  const std::uint32_t cutting_min_segments = 0,
                  const float cutting_min_score = 0.16,
                  const bool locally_constrained = true,
                  const bool directed_cutting = true,
                  const bool clean_cutting = false)
      {
        max_cuts_ = max_cuts;
        min_segment_size_for_cutting_ = cutting_min_segments;
        min_cut_score_ = cutting_min_score;
        use_local_constrains_ = locally_constrained;
        use_directed_weights_ = directed_cutting;
        use_clean_cutting_ = clean_cutting;
      }

      /** \brief Set the number of iterations for the weighted RANSAC step (best cut estimations)
       *  \param[in] ransac_iterations The number of iterations */
      inline void
      setRANSACIterations (const std::uint32_t ransac_iterations)
      {
        ransac_itrs_ = ransac_iterations;
      }

    private:

      /** \brief Used in for CPC to find and fit cutting planes to the pointcloud.
       *  \note Is used recursively
       *  \param[in] depth_levels_left When first calling the function set this parameter to the maximum levels you want to cut down */
      void
      applyCuttingPlane (std::uint32_t depth_levels_left);

      ///  *** Parameters *** ///

      /** \brief Maximum number of cuts */
      std::uint32_t max_cuts_;

      /** \brief Minimum segment size for cutting */
      std::uint32_t min_segment_size_for_cutting_;

      /** \brief Cut_score threshold */
      float min_cut_score_;

      /** \brief Use local constrains for cutting */
      bool use_local_constrains_;

      /** \brief Use directed weights for the cutting */
      bool use_directed_weights_;

      /** \brief Use clean cutting */
      bool use_clean_cutting_;

      /** \brief Iterations for RANSAC */
      std::uint32_t ransac_itrs_;


/******************************************* Directional weighted RANSAC declarations ******************************************************************/
      /** \brief @b WeightedRandomSampleConsensus represents an implementation of the Directionally Weighted RANSAC algorithm, as described in: "Constrained Planar Cuts - Part Segmentation for Point Clouds", CVPR 2015, M. Schoeler, J. Papon, F. Wörgötter.
        *  \note It only uses points with a weight > 0 for the model calculation, but uses all points for the evaluation (scoring of the model)
        *  Only use in conjunction with sac_model_plane
        *  If you use this in a scientific work please cite the following paper:
        *  M. Schoeler, J. Papon, F. Woergoetter
        *  Constrained Planar Cuts - Object Partitioning for Point Clouds
        *  In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2015
        *  \author Markus Schoeler (mschoeler@web.de)
        *  \ingroup segmentation
        */

      class WeightedRandomSampleConsensus : public SampleConsensus<WeightSACPointType>
      {
          using SampleConsensusModelPtr = SampleConsensusModel<WeightSACPointType>::Ptr;

        public:
          using Ptr = shared_ptr<WeightedRandomSampleConsensus>;
          using ConstPtr = shared_ptr<const WeightedRandomSampleConsensus>;

          /** \brief WeightedRandomSampleConsensus (Weighted RAndom SAmple Consensus) main constructor
            * \param[in] model a Sample Consensus model
            * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
            */
          WeightedRandomSampleConsensus (const SampleConsensusModelPtr &model,
                                        bool random = false)
            : SampleConsensus<WeightSACPointType> (model, random)
          {
            initialize ();
          }

          /** \brief WeightedRandomSampleConsensus (Weighted RAndom SAmple Consensus) main constructor
            * \param[in] model a Sample Consensus model
            * \param[in] threshold distance to model threshold
            * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
            */
          WeightedRandomSampleConsensus (const SampleConsensusModelPtr &model,
                                        double threshold,
                                        bool random = false)
            : SampleConsensus<WeightSACPointType> (model, threshold, random)
          {
            initialize ();
          }

          /** \brief Compute the actual model and find the inliers
            * \param[in] debug_verbosity_level enable/disable on-screen debug information and set the verbosity level
            */
          bool
          computeModel (int debug_verbosity_level = 0) override;

          /** \brief Set the weights for the input points
          *  \param[in] weights Weights for input samples. Negative weights are counted as penalty.
          */
          void
          setWeights (const std::vector<double> &weights,
                      const bool directed_weights = false)
          {
            if (weights.size () != full_cloud_pt_indices_->size ())
            {
              PCL_ERROR ("[pcl::WeightedRandomSampleConsensus::setWeights] Cannot assign weights. Weight vector needs to have the same length as the input pointcloud\n");
              return;
            }
            weights_ = weights;
            model_pt_indices_->clear ();
            for (std::size_t i = 0; i < weights.size (); ++i)
            {
              if (weights[i] > std::numeric_limits<double>::epsilon ())
                model_pt_indices_->push_back (i);
            }
            use_directed_weights_ = directed_weights;
          }

          /** \brief Get the best score
          *  \returns The best score found.
          */
          double
          getBestScore () const
          {
            return (best_score_);
          }

        protected:
          /** \brief Initialize the model parameters. Called by the constructors. */
          void
          initialize ()
          {
            // Maximum number of trials before we give up.
            max_iterations_ = 10000;
            use_directed_weights_ = false;
            model_pt_indices_.reset (new Indices);
            full_cloud_pt_indices_.reset (new Indices (* (sac_model_->getIndices ())));
            point_cloud_ptr_ = sac_model_->getInputCloud ();
          }

          /** \brief  weight each positive weight point by the inner product between the normal and the plane normal */
          bool use_directed_weights_;

          /** \brief  vector of weights assigned to points. Set by the setWeights-method */
          std::vector<double> weights_;

          /** \brief  The indices used for estimating the RANSAC model. Only those whose weight is > 0 */
          pcl::IndicesPtr model_pt_indices_;

          /** \brief  The complete list of indices used for the model evaluation */
          pcl::IndicesPtr full_cloud_pt_indices_;

          /** \brief  Pointer to the input PointCloud */
          pcl::PointCloud<WeightSACPointType>::ConstPtr point_cloud_ptr_;

          /** \brief  Highest score found so far */
          double best_score_;
      };

  };
}

#ifdef PCL_NO_PRECOMPILE
  #include <pcl/segmentation/impl/cpc_segmentation.hpp>
#elif defined(PCL_ONLY_CORE_POINT_TYPES)
  //pcl::PointXYZINormal is not a core point type (so we cannot use the precompiled classes here)
  #include <pcl/sample_consensus/impl/sac_model_plane.hpp>
  #include <pcl/segmentation/impl/extract_clusters.hpp>
#endif // PCL_NO_PRECOMPILE / PCL_ONLY_CORE_POINT_TYPES