/*
 * 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

#include <pcl/registration/ia_fpcs.h>

namespace pcl {
namespace registration {
/** \brief KFPCSInitialAlignment computes corresponding four point congruent sets based
 * on keypoints as described in: "Markerless point cloud registration with
 * keypoint-based 4-points congruent sets", Pascal Theiler, Jan Dirk Wegner, Konrad
 * Schindler. ISPRS Annals II-5/W2, 2013. Presented at ISPRS Workshop Laser Scanning,
 * Antalya, Turkey, 2013. \note Method has since been improved and some variations to
 * the paper exist. \author P.W.Theiler \ingroup registration
 */
template <typename PointSource,
          typename PointTarget,
          typename NormalT = pcl::Normal,
          typename Scalar = float>
class KFPCSInitialAlignment
: public virtual FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar> {
public:
  /** \cond */
  using Ptr =
      shared_ptr<KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
  using ConstPtr = shared_ptr<
      const KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;

  using PointCloudSource = pcl::PointCloud<PointSource>;
  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
  using PointCloudSourceIterator = typename PointCloudSource::iterator;

  using PointCloudTarget = pcl::PointCloud<PointTarget>;
  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
  using PointCloudTargetIterator = typename PointCloudTarget::iterator;

  using MatchingCandidate = pcl::registration::MatchingCandidate;
  using MatchingCandidates = pcl::registration::MatchingCandidates;
  /** \endcond */

  /** \brief Constructor. */
  KFPCSInitialAlignment();

  /** \brief Destructor. */
  ~KFPCSInitialAlignment(){};

  /** \brief Set the upper translation threshold used for score evaluation.
   * \param[in] upper_trl_boundary upper translation threshold
   */
  inline void
  setUpperTranslationThreshold(float upper_trl_boundary)
  {
    upper_trl_boundary_ = upper_trl_boundary;
  };

  /** \return the upper translation threshold used for score evaluation. */
  inline float
  getUpperTranslationThreshold() const
  {
    return (upper_trl_boundary_);
  };

  /** \brief Set the lower translation threshold used for score evaluation.
   * \param[in] lower_trl_boundary lower translation threshold
   */
  inline void
  setLowerTranslationThreshold(float lower_trl_boundary)
  {
    lower_trl_boundary_ = lower_trl_boundary;
  };

  /** \return the lower translation threshold used for score evaluation. */
  inline float
  getLowerTranslationThreshold() const
  {
    return (lower_trl_boundary_);
  };

  /** \brief Set the weighting factor of the translation cost term.
   * \param[in] lambda the weighting factor of the translation cost term
   */
  inline void
  setLambda(float lambda)
  {
    lambda_ = lambda;
  };

  /** \return the weighting factor of the translation cost term. */
  inline float
  getLambda() const
  {
    return (lambda_);
  };

  /** \brief Get the N best unique candidate matches according to their fitness score.
   * The method only returns unique transformations comparing the translation
   * and the 3D rotation to already returned transformations.
   *
   * \note The method may return less than N candidates, if the number of unique
   * candidates is smaller than N
   *
   * \param[in] n number of best candidates to return
   * \param[in] min_angle3d minimum 3D angle difference in radian
   * \param[in] min_translation3d minimum 3D translation difference
   * \param[out] candidates vector of unique candidates
   */
  void
  getNBestCandidates(int n,
                     float min_angle3d,
                     float min_translation3d,
                     MatchingCandidates& candidates);

  /** \brief Get all unique candidate matches with fitness scores above a threshold t.
   * The method only returns unique transformations comparing the translation
   * and the 3D rotation to already returned transformations.
   *
   * \param[in] t fitness score threshold
   * \param[in] min_angle3d minimum 3D angle difference in radian
   * \param[in] min_translation3d minimum 3D translation difference
   * \param[out] candidates vector of unique candidates
   */
  void
  getTBestCandidates(float t,
                     float min_angle3d,
                     float min_translation3d,
                     MatchingCandidates& candidates);

protected:
  using PCLBase<PointSource>::deinitCompute;
  using PCLBase<PointSource>::input_;
  using PCLBase<PointSource>::indices_;

  using Registration<PointSource, PointTarget, Scalar>::reg_name_;
  using Registration<PointSource, PointTarget, Scalar>::tree_;
  using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
  using Registration<PointSource, PointTarget, Scalar>::ransac_iterations_;
  using Registration<PointSource, PointTarget, Scalar>::correspondences_;
  using Registration<PointSource, PointTarget, Scalar>::converged_;

  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::delta_;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
      approx_overlap_;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_pair_diff_;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_edge_diff_;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
      coincidation_limit_;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_mse_;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
      max_inlier_dist_sqr_;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::diameter_;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
      normalize_delta_;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::fitness_score_;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
      score_threshold_;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
      linkMatchWithBase;
  using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::validateMatch;

  /** \brief Internal computation initialization. */
  bool
  initCompute() override;

  /** \brief Method to handle current candidate matches. Here we validate and evaluate
   * the matches w.r.t the base and store the sorted matches (together with score values
   * and estimated transformations).
   *
   * \param[in] base_indices indices of base B
   * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate
   * matches are reordered during this step. \param[out] candidates vector which
   * contains the candidates matches M
   */
  void
  handleMatches(const pcl::Indices& base_indices,
                std::vector<pcl::Indices>& matches,
                MatchingCandidates& candidates) override;

  /** \brief Validate the transformation by calculating the score value after
   * transforming the input source cloud. The resulting score is later used as the
   * decision criteria of the best fitting match.
   *
   * \param[out] transformation updated orientation matrix using all inliers
   * \param[out] fitness_score current best score
   * \note fitness score is only updated if the score of the current transformation
   * exceeds the input one. \return
   * * < 0 if previous result is better than the current one (score remains)
   * * = 0 current result is better than the previous one (score updated)
   */
  int
  validateTransformation(Eigen::Matrix4f& transformation,
                         float& fitness_score) override;

  /** \brief Final computation of best match out of vector of matches. To avoid cross
   * thread dependencies during parallel running, a best match for each try was
   * calculated. \note For forwards compatibility the candidates are stored in vectors
   * of 'vectors of size 1'. \param[in] candidates vector of candidate matches
   */
  void
  finalCompute(const std::vector<MatchingCandidates>& candidates) override;

  /** \brief Lower boundary for translation costs calculation.
   * \note If not set by the user, the translation costs are not used during evaluation.
   */
  float lower_trl_boundary_;

  /** \brief Upper boundary for translation costs calculation.
   * \note If not set by the user, it is calculated from the estimated overlap and the
   * diameter of the point cloud.
   */
  float upper_trl_boundary_;

  /** \brief Weighting factor for translation costs (standard = 0.5). */
  float lambda_;

  /** \brief Container for resulting vector of registration candidates. */
  MatchingCandidates candidates_;

  /** \brief Flag if translation score should be used in validation (internal
   * calculation). */
  bool use_trl_score_;

  /** \brief Subset of input indices on which we evaluate candidates.
   * To speed up the evaluation, we only use a fix number of indices defined during
   * initialization.
   */
  pcl::IndicesPtr indices_validation_;
};
}; // namespace registration
}; // namespace pcl

#include <pcl/registration/impl/ia_kfpcs.hpp>