/*
 * Software License Agreement (BSD License)
 *
 * Point Cloud Library (PCL) - www.pointclouds.org
 * Copyright (c) 2009-2012, Willow Garage, Inc.
 * Copyright (c) 2012-, 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.
 *
 * $Id$
 *
 */

#pragma once

#include <pcl/apps/in_hand_scanner/common_types.h>
#include <pcl/apps/in_hand_scanner/utils.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/pcl_exports.h>

////////////////////////////////////////////////////////////////////////////////
// InputDataProcessing
////////////////////////////////////////////////////////////////////////////////

namespace pcl {
namespace ihs {
/** \brief Bundles methods that are applied to the input data from the sensor.
 * \author Martin Saelzle
 * \ingroup apps
 */
class PCL_EXPORTS InputDataProcessing {
public:
  using PointXYZRGBA = pcl::PointXYZRGBA;
  using CloudXYZRGBA = pcl::PointCloud<PointXYZRGBA>;
  using CloudXYZRGBAPtr = CloudXYZRGBA::Ptr;
  using CloudXYZRGBAConstPtr = CloudXYZRGBA::ConstPtr;

  using PointXYZRGBNormal = pcl::PointXYZRGBNormal;
  using CloudXYZRGBNormal = pcl::PointCloud<PointXYZRGBNormal>;
  using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr;
  using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr;

  /** \brief Constructor */
  InputDataProcessing();

  /** \brief Apply the segmentation on the input cloud (XYZ and HSV).
   *
   * \param[in] cloud_in The input cloud.
   * \param[out] cloud_out The segmented cloud.
   * \param[out] cloud_discarded Cloud containing all points that were removed during
   * the HSV segmentation. The points in the XYZ segmentation are NOT used!
   *
   * \return true if success.
   *
   * \note Converts from m to cm.
   */
  bool
  segment(const CloudXYZRGBAConstPtr& cloud_in,
          CloudXYZRGBNormalPtr& cloud_out,
          CloudXYZRGBNormalPtr& cloud_discarded) const;

  /** \brief Calculate the normals of the input cloud.
   *
   * \param[in] cloud_in The input cloud.
   * \param[out] cloud_out Input cloud with normals appended.
   *
   * \return true if success.
   *
   * \note Converts from m to cm.
   */
  bool
  calculateNormals(const CloudXYZRGBAConstPtr& cloud_in,
                   CloudXYZRGBNormalPtr& cloud_out) const;

  /** @{ */
  /** \brief Points outside of X - Y - Z - min / max are discarded. The unit is cm. The
   * min values must be smaller than the max values. */
  inline void
  setXMin(const float x_min)
  {
    if (x_min < x_max_)
      x_min_ = x_min;
  }
  inline void
  setXMax(const float x_max)
  {
    if (x_max > x_min_)
      x_max_ = x_max;
  }
  inline void
  setYMin(const float y_min)
  {
    if (y_min < y_max_)
      y_min_ = y_min;
  }
  inline void
  setYMax(const float y_max)
  {
    if (y_max > y_min_)
      y_max_ = y_max;
  }
  inline void
  setZMin(const float z_min)
  {
    if (z_min < z_max_)
      z_min_ = z_min;
  }
  inline void
  setZMax(const float z_max)
  {
    if (z_max > z_min_)
      z_max_ = z_max;
  }

  inline float
  getXMin() const
  {
    return (x_min_);
  }
  inline float
  getXMax() const
  {
    return (x_max_);
  }
  inline float
  getYMin() const
  {
    return (y_min_);
  }
  inline float
  getYMax() const
  {
    return (y_max_);
  }
  inline float
  getZMin() const
  {
    return (z_min_);
  }
  inline float
  getZMax() const
  {
    return (z_max_);
  }
  /** @} */

  /** @{ */
  /** \brief Simple color segmentation in the HSV color space. Points inside of H - S -
   * V min / max are discarded. H must be in the range 0 and 360, S and V in the range 0
   * and 1.
   *
   * \note If you set values outside of the allowed range the member variables
   * are clamped to the next best value. E.g. H is set to 0 if you pass -1.
   */
  inline void
  setHMin(const float h_min)
  {
    h_min_ = pcl::ihs::clamp(h_min, 0.f, 360.f);
  }
  inline void
  setHMax(const float h_max)
  {
    h_max_ = pcl::ihs::clamp(h_max, 0.f, 360.f);
  }
  inline void
  setSMin(const float s_min)
  {
    s_min_ = pcl::ihs::clamp(s_min, 0.f, 1.f);
  }
  inline void
  setSMax(const float s_max)
  {
    s_max_ = pcl::ihs::clamp(s_max, 0.f, 1.f);
  }
  inline void
  setVMin(const float v_min)
  {
    v_min_ = pcl::ihs::clamp(v_min, 0.f, 1.f);
  }
  inline void
  setVMax(const float v_max)
  {
    v_max_ = pcl::ihs::clamp(v_max, 0.f, 1.f);
  }

  inline float
  getHMin() const
  {
    return (h_min_);
  }
  inline float
  getHMax() const
  {
    return (h_max_);
  }
  inline float
  getSMin() const
  {
    return (s_min_);
  }
  inline float
  getSMax() const
  {
    return (s_max_);
  }
  inline float
  getVMin() const
  {
    return (v_min_);
  }
  inline float
  getVMax() const
  {
    return (v_max_);
  }
  /** @} */

  /** @{ */
  /** \brief If true the color values inside of H - S - V min / max are accepted instead
   * of discarded. */
  inline void
  setColorSegmentationInverted(const bool hsv_inverted)
  {
    hsv_inverted_ = hsv_inverted;
  }
  inline bool
  getColorSegmentationInverted() const
  {
    return (hsv_inverted_);
  }
  /** @} */

  /** @{ */
  /** \brief Enable / disable the color segmentation. */
  inline void
  setColorSegmentationEnabled(const bool hsv_enabled)
  {
    hsv_enabled_ = hsv_enabled;
  }
  inline bool
  getColorSegmentationEnabled() const
  {
    return (hsv_enabled_);
  }
  /** @} */

  /** @{ */
  /** \brief The XYZ mask is eroded with a kernel of this size. */
  inline void
  setXYZErodeSize(const unsigned int size)
  {
    size_erode_ = size;
  }
  inline unsigned int
  getXYZErodeSize() const
  {
    return (size_erode_);
  }
  /** @} */

  /** @{ */
  /** \brief The HSV mask is dilated with a kernel of this size. */
  inline void
  setHSVDilateSize(const unsigned int size)
  {
    size_dilate_ = size;
  }
  inline unsigned int
  getHSVDilateSize() const
  {
    return (size_dilate_);
  }
  /** @} */

private:
  using Normal = pcl::Normal;
  using CloudNormals = pcl::PointCloud<Normal>;
  using CloudNormalsPtr = CloudNormals::Ptr;
  using CloudNormalsConstPtr = CloudNormals::ConstPtr;

  using NormalEstimation = pcl::IntegralImageNormalEstimation<PointXYZRGBA, Normal>;
  using NormalEstimationPtr = NormalEstimation::Ptr;
  using NormalEstimationConstPtr = NormalEstimation::ConstPtr;

  using MatrixXb = Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>;
  using MatrixXi = Eigen::MatrixXi;

  /** \brief Erodes the input mask k times with a diamond shaped structuring element.
   * \see http://ostermiller.org/dilate_and_erode.html
   */
  void
  erode(MatrixXb& mask, const int k) const;

  /** \brief Dilates the input mask k times with a diamond shaped structuring element.
   * \see http://ostermiller.org/dilate_and_erode.html
   */
  void
  dilate(MatrixXb& mask, const int k) const;

  /** \brief Calculates the manhattan distance map for the input matrix.
   *
   * \param[in] mat Input matrix.
   * \param[in] comp Compared value. mat==comp will have zero distance.
   *
   * \return Matrix containing the distances to mat==comp
   *         \see http://ostermiller.org/dilate_and_erode.html
   */
  MatrixXi
  manhattan(const MatrixXb& mat, const bool comp) const;

  /** \brief Conversion from the RGB to HSV color space. */
  void
  RGBToHSV(const unsigned char r,
           const unsigned char g,
           const unsigned char b,
           float& h,
           float& s,
           float& v) const;

  ////////////////////////////////////////////////////////////////////////
  // Members
  ////////////////////////////////////////////////////////////////////////

  NormalEstimationPtr normal_estimation_;

  float x_min_;
  float x_max_;
  float y_min_;
  float y_max_;
  float z_min_;
  float z_max_;

  float h_min_;
  float h_max_;
  float s_min_;
  float s_max_;
  float v_min_;
  float v_max_;

  bool hsv_inverted_;
  bool hsv_enabled_;

  unsigned int size_dilate_;
  unsigned int size_erode_;
};
} // End namespace ihs
} // End namespace pcl
