/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  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.
 *
 */

#pragma once

#include <pcl/point_cloud.h> // for PointCloud
#include <pcl/point_types.h>

#include <algorithm>

namespace pcl {
template <class T>
short int
doStereoRatioFilter(const T* const acc,
                    short int dbest,
                    T sad_min,
                    int ratio_filter,
                    int maxdisp,
                    int precision = 100)
{
  const auto sad_min_1st_part_it = std::min_element(acc, acc + dbest - 1);
  const auto sad_min_2nd_part_it = std::min_element(acc + dbest + 2, acc + maxdisp);

  const auto sad_second_min = std::min(*sad_min_1st_part_it, *sad_min_2nd_part_it);

  if ((sad_min * precision) > ((precision - ratio_filter) * sad_second_min)) {
    return -2;
  }
  return dbest;
}

template <class T>
inline short int
doStereoPeakFilter(const T* const acc, short int dbest, int peak_filter, int maxdisp)
{
  // da and db = acc[index] - acc[dbest],
  // where index = (dbest + 2) or (dbest - 2)
  //   =>  index = dbest + 2 - (0 or 4)           = dbest - 2 + (0 or 4)
  //   =>  index = dbest + 2 - (0 << 2 or 1 << 2) = dbest - 2 + (0 << 2 or 1 << 2)
  //   =>  index = dbest + 2 - (condition << 2)   = dbest - 2 + (condition << 2)
  const auto da_condition = (dbest > 1);
  const auto db_condition = (dbest < maxdisp - 2);
  const auto da_index = dbest + 2 - (da_condition << 2);
  const auto db_index = dbest - 2 + (db_condition << 2);

  const auto da = acc[da_index] - acc[dbest];
  const auto db = acc[db_index] - acc[dbest];
  if ((da + db) < peak_filter) {
    return -4;
  }
  return dbest;
}

/** \brief Stereo Matching abstract class
 *
 * The class performs stereo matching on a rectified stereo pair.
 *
 * Includes the following functionalities:
 *   * preprocessing of the image pair, to improve robustness against photometric
 *     distortions (wrt. to a spatially constant additive photometric factor)
 *   * postprocessing: filtering of wrong disparities via Peak Filter (eliminating
 *     ambiguities due to low-textured regions) and Ratio Filter (eliminating generic
 *     matching ambiguities, similar to that present in OpenCV Block Matching Stereo)
 *   * postprocessing: Left-Right consistency check (eliminates wrong disparities at the
 *     cost of twice the stereo matching computation)
 *   * postprocessing: subpixel refinement of computed disparities, to reduce the depth
 *     quantization effect
 *   * postprocessing: smoothing of the disparity map via median filter
 *   * after stereo matching a PCL point cloud can be computed, given the stereo
 *     intrinsic (focal, principal point coordinates) and extrinsic (baseline)
 *     calibration parameters
 *
 * \author Federico Tombari (federico.tombari@unibo.it)
 * \ingroup stereo
 */
class PCL_EXPORTS StereoMatching {
public:
  StereoMatching();

  virtual ~StereoMatching();

  /** \brief setter for number of disparity candidates (disparity range)
   *
   * \param[in] max_disp number of disparity candidates (disparity range); has to be > 0
   */
  void
  setMaxDisparity(int max_disp)
  {
    max_disp_ = max_disp;
  };

  /** \brief setter for horizontal offset, i.e. number of pixels to shift the disparity
   * range over the target image
   *
   * \param[in] x_off horizontal offset value; has to be >= 0
   */
  void
  setXOffset(int x_off)
  {
    x_off_ = x_off;
  };

  /** \brief setter for the value of the ratio filter
   *
   * \param[in] ratio_filter value of the ratio filter; it is a number in the range
   *            [0, 100] (0: no filtering action; 100: all disparities are
   *            filtered)
   */
  void
  setRatioFilter(int ratio_filter)
  {
    ratio_filter_ = ratio_filter;
  };

  /** \brief setter for the value of the peak filter
   *
   * \param[in] peak_filter value of the peak filter; it is a number in the range
   *            [0, inf] (0: no filtering action)
   */
  void
  setPeakFilter(int peak_filter)
  {
    peak_filter_ = peak_filter;
  };

  /** \brief setter for the pre processing step
   *
   * \param[in] is_pre_proc setting the boolean to true activates the pre-processing
   *            step for both stereo images
   */
  void
  setPreProcessing(bool is_pre_proc)
  {
    is_pre_proc_ = is_pre_proc;
  };

  /** \brief setter for the left-right consistency check stage, that eliminates
   * inconsistent/wrong disparity values from the disparity map at approx. twice the
   * processing cost of the selected stereo algorithm
   *
   * \param[in] is_lr_check setting the boolean to true activates the left-right
   *            consistency check
   */
  void
  setLeftRightCheck(bool is_lr_check)
  {
    is_lr_check_ = is_lr_check;
  };

  /** \brief setter for the left-right consistency check threshold
   *
   * \param[in] lr_check_th sets the value of the left-right consistency check threshold
   *            only has some influence if the left-right check is active typically has
   *            either the value 0 ("strong" consistency check, more points being
   *            filtered) or 1 ("weak" consistency check, less points being filtered)
   */
  void
  setLeftRightCheckThreshold(int lr_check_th)
  {
    lr_check_th_ = lr_check_th;
  };

  /** \brief stereo processing, it computes a disparity map stored internally by the
   * class
   *
   * \param[in] ref_img reference array of image pixels (left image)
   * \param[in] trg_img target array of image pixels (right image)
   * \param[in] width number of elements per row for both input arrays
   * \param[in] height number of elements per column for both input arrays
   */
  virtual void
  compute(unsigned char* ref_img, unsigned char* trg_img, int width, int height) = 0;

  /** \brief stereo processing, it computes a disparity map stored internally by the
   * class
   *
   * \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference
   *            image (left image)
   * \param[in] trg point cloud of pcl::RGB type containing the pixels of the target
   *            image (right image)
   */
  virtual void
  compute(pcl::PointCloud<pcl::RGB>& ref, pcl::PointCloud<pcl::RGB>& trg) = 0;

  /** \brief median filter applied on the previously computed disparity map
   *
   * \note The "compute" method must have been previously called at least once in order
   * for this function to have any effect
   *
   * \param[in] radius radius of the squared window used to compute the median filter;
   *            the window side is equal to 2*radius + 1
   */
  void
  medianFilter(int radius);

  /** \brief computation of the 3D point cloud from the previously computed disparity
   * map without color information
   *
   * \note The "compute" method must have been previously called at least once in order
   * for this function to have any effect
   *
   * \param[in] u_c horizontal coordinate of the principal point (calibration parameter)
   * \param[in] v_c vertical coordinate of the principal point (calibration parameter)
   * \param[in] focal focal length in pixels (calibration parameter)
   * \param[in] baseline distance between the two cameras (calibration parameter); the
   *            measure unit used to specify this parameter will be the same as the 3D
   *            points in the output point cloud
   * \param[out] cloud output 3D point cloud; it is organized and non-dense, with NaNs
   *             where 3D points are invalid
   */
  virtual bool
  getPointCloud(float u_c,
                float v_c,
                float focal,
                float baseline,
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);

  /** \brief computation of the 3D point cloud from the previously computed disparity
   * map including color information
   *
   * \note The "compute" method must have been previously called at least once in order
   * for this function to have any effect
   *
   * \param[in] u_c horizontal coordinate of the principal point (calibration parameter)
   * \param[in] v_c vertical coordinate of the principal point (calibration parameter)
   * \param[in] focal focal length in pixels (calibration parameter)
   * \param[in] baseline distance between the two cameras (calibration parameter); the
   *            measure unit used to specify this parameter will be the same as the 3D
   *            points in the output point cloud \param[out] cloud output 3D point
   *            cloud; it is organized and non-dense, with NaNs where 3D points are
   *            invalid
   * \param[in] texture 3D cloud (same size of the output cloud) used to associate to
   *            each 3D point of the output cloud a color triplet
   */
  virtual bool
  getPointCloud(float u_c,
                float v_c,
                float focal,
                float baseline,
                pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                pcl::PointCloud<pcl::RGB>::Ptr texture);

  /** \brief computation of a pcl::RGB cloud with scaled disparity values it can be used
   * to display a rescaled version of the disparity map by means of the pcl::ImageViewer
   * invalid disparity values are shown in green
   *
   * \note The "compute" method must have been previously called at least once in order
   * for this function to have any effect
   *
   * \param[out] vMap output cloud
   */
  void
  getVisualMap(pcl::PointCloud<pcl::RGB>::Ptr vMap);

protected:
  /** \brief The internal disparity map. */
  short int* disp_map_;

  /** \brief Local aligned copies of the cloud data. */
  unsigned char* ref_img_;
  unsigned char* trg_img_;

  /** \brief Disparity map used for left-right check. */
  short int* disp_map_trg_;

  /** \brief Local aligned copies used for pre processing. */
  unsigned char* pp_ref_img_;
  unsigned char* pp_trg_img_;

  /** \brief number of pixels per column of the input stereo pair . */
  int width_;

  /** \brief number of pixels per row of the input stereo pair . */
  int height_;

  /** \brief Disparity range used for stereo processing. */
  int max_disp_;

  /** \brief Horizontal displacemente (x offset) used for stereo processing */
  int x_off_;

  /** \brief Threshold for the ratio filter, \f$\in [0 100]\f$ */
  int ratio_filter_;

  /** \brief Threshold for the peak filter, \f$\in [0 \infty]\f$ */
  int peak_filter_;

  /** \brief toggle for the activation of the pre-processing stage */
  bool is_pre_proc_;

  /** \brief toggle for the activation of the left-right consistency check stage */
  bool is_lr_check_;

  /** \brief Threshold for the left-right consistency check, typically either 0 or 1 */
  int lr_check_th_;

  virtual void
  preProcessing(unsigned char* img, unsigned char* pp_img) = 0;

  virtual void
  imgFlip(unsigned char*& img) = 0;

  virtual void
  compute_impl(unsigned char* ref_img, unsigned char* trg_img) = 0;

  void
  leftRightCheck();

  inline short int
  computeStereoSubpixel(int dbest, int s1, int s2, int s3)
  {
    int den = (s1 + s3 - 2 * s2);
    if (den != 0)
      return (static_cast<short int>(16 * dbest + (((s1 - s3) * 8) / den)));
    return (static_cast<short int>(dbest * 16));
  }

  inline short int
  computeStereoSubpixel(int dbest, float s1, float s2, float s3)
  {
    float den = (s1 + s3 - 2 * s2);
    if (den != 0)
      return (static_cast<short int>(16 * dbest +
                                     std::floor(.5 + (((s1 - s3) * 8) / den))));
    return (static_cast<short int>(dbest * 16));
  }
};

/** \brief Stereo Matching abstract class for Grayscale images
 *
 * The class implements some functionalities of pcl::StereoMatching specific for
 * grayscale stereo processing, such as image pre-processing and left
 *
 * \author Federico Tombari (federico.tombari@unibo.it)
 * \ingroup stereo
 */
class PCL_EXPORTS GrayStereoMatching : public StereoMatching {
public:
  GrayStereoMatching();
  ~GrayStereoMatching();

  /** \brief stereo processing, it computes a disparity map stored internally by the
   * class
   *
   * \param[in] ref_img reference array of image pixels (left image), has to be
   *            grayscale single channel
   * \param[in] trg_img target array of image pixels (right image), has to be grayscale
   *            single channel
   * \param[in] width number of elements per row for both input arrays
   * \param[in] height number of elements per column for both input arrays
   */
  void
  compute(unsigned char* ref_img,
          unsigned char* trg_img,
          int width,
          int height) override;

  /** \brief stereo processing, it computes a disparity map stored internally by the
   * class
   *
   * \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference
   *            image (left image) the pcl::RGB triplets are automatically converted to
   *            grayscale upon call of the method
   * \param[in] trg point cloud of pcl::RGB type containing the pixels of the target
   *            image (right image) the pcl::RGB triplets are automatically converted to
   *            grayscale upon call of the method
   */
  void
  compute(pcl::PointCloud<pcl::RGB>& ref, pcl::PointCloud<pcl::RGB>& trg) override;

protected:
  void
  compute_impl(unsigned char* ref_img, unsigned char* trg_img) override = 0;

  void
  preProcessing(unsigned char* img, unsigned char* pp_img) override;

  void
  imgFlip(unsigned char*& img) override;
};

/** \brief Block based (or fixed window) Stereo Matching class
 *
 * This class implements the baseline Block-based - aka Fixed Window -  stereo matching
 * algorithm. The algorithm includes a running box filter so that the computational
 * complexity is independent of the size of the window ( O(1) wrt. to the size of
 * window) The algorithm is based on the Sum of Absolute Differences (SAD) matching
 * function Only works with grayscale (single channel) rectified images
 *
 * \author Federico Tombari (federico.tombari@unibo.it)
 * \ingroup stereo
 */

class PCL_EXPORTS BlockBasedStereoMatching : public GrayStereoMatching {
public:
  BlockBasedStereoMatching();
  ~BlockBasedStereoMatching(){};

  /** \brief setter for the radius of the squared window
   * \param[in] radius radius of the squared window used to compute the block-based
   *            stereo algorithm the window side is equal to 2*radius + 1
   */
  void
  setRadius(int radius)
  {
    radius_ = radius;
  };

private:
  void
  compute_impl(unsigned char* ref_img, unsigned char* trg_img) override;

  int radius_;
};

/** \brief Adaptive Cost 2-pass Scanline Optimization Stereo Matching class
 *
 * This class implements an adaptive-cost stereo matching algorithm based on 2-pass
 * Scanline Optimization. The algorithm is inspired by the paper: [1] L. Wang et al.,
 * "High Quality Real-time Stereo using Adaptive Cost Aggregation and Dynamic
 * Programming", 3DPVT 2006 Cost aggregation is performed using adaptive weigths
 * computed on a single column as proposed in [1]. Instead of using Dynamic Programming
 * as in [1], the optimization is performed via 2-pass Scanline Optimization. The
 * algorithm is based on the Sum of Absolute Differences (SAD) matching function Only
 * works with grayscale (single channel) rectified images
 *
 * \author Federico Tombari (federico.tombari@unibo.it)
 * \ingroup stereo
 */
class PCL_EXPORTS AdaptiveCostSOStereoMatching : public GrayStereoMatching {
public:
  AdaptiveCostSOStereoMatching();

  ~AdaptiveCostSOStereoMatching(){};

  /** \brief setter for the radius (half length) of the column used for cost aggregation
   * \param[in] radius radius (half length) of the column used for cost aggregation; the
   *            total column length is equal to 2*radius + 1
   */
  void
  setRadius(int radius)
  {
    radius_ = radius;
  };

  /** \brief setter for the spatial bandwidth used for cost aggregation based on
   * adaptive weights
   * \param[in] gamma_s spatial bandwidth used for cost aggregation based on adaptive
   *            weights
   */
  void
  setGammaS(int gamma_s)
  {
    gamma_s_ = gamma_s;
  };

  /** \brief setter for the color bandwidth used for cost aggregation based on adaptive
   * weights
   * \param[in] gamma_c color bandwidth used for cost aggregation based on
   *            adaptive weights
   */
  void
  setGammaC(int gamma_c)
  {
    gamma_c_ = gamma_c;
  };

  /** \brief "weak" smoothness penalty used within 2-pass Scanline Optimization
   * \param[in] smoothness_weak "weak" smoothness penalty cost
   */
  void
  setSmoothWeak(int smoothness_weak)
  {
    smoothness_weak_ = smoothness_weak;
  };

  /** \brief "strong" smoothness penalty used within 2-pass Scanline Optimization
   * \param[in] smoothness_strong "strong" smoothness penalty cost
   */
  void
  setSmoothStrong(int smoothness_strong)
  {
    smoothness_strong_ = smoothness_strong;
  };

private:
  void
  compute_impl(unsigned char* ref_img, unsigned char* trg_img) override;

  int radius_;

  // parameters for adaptive weight cost aggregation
  double gamma_c_;
  double gamma_s_;

  // Parameters for 2-pass SO optimization
  int smoothness_strong_;
  int smoothness_weak_;
};

} // namespace pcl