/*
 * 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.
 */
#ifndef PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
#define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_

#include <pcl/stereo/disparity_map_converter.h>

#include <fstream>
#include <limits>

#include <pcl/point_types.h>
#include <pcl/console/print.h>
#include <pcl/common/intensity.h>

template <typename PointT>
pcl::DisparityMapConverter<PointT>::DisparityMapConverter () :
    center_x_ (0.0f),
    center_y_ (0.0f),
    focal_length_ (0.0f),
    baseline_ (0.0f),
    is_color_ (false),
    disparity_map_width_ (640),
    disparity_map_height_ (480),
    disparity_threshold_min_ (0.0f),
    disparity_threshold_max_ (std::numeric_limits<float>::max ())
{
}

template <typename PointT>
pcl::DisparityMapConverter<PointT>::~DisparityMapConverter ()
{
}

template <typename PointT> inline void
pcl::DisparityMapConverter<PointT>::setImageCenterX (const float center_x)
{
  center_x_ = center_x;
}

template <typename PointT> inline float
pcl::DisparityMapConverter<PointT>::getImageCenterX () const
{
  return center_x_;
}

template <typename PointT> inline void
pcl::DisparityMapConverter<PointT>::setImageCenterY (const float center_y)
{
  center_y_ = center_y;
}

template <typename PointT> inline float
pcl::DisparityMapConverter<PointT>::getImageCenterY () const
{
  return center_y_;
}

template <typename PointT> inline void
pcl::DisparityMapConverter<PointT>::setFocalLength (const float focal_length)
{
  focal_length_ = focal_length;
}

template <typename PointT> inline float
pcl::DisparityMapConverter<PointT>::getFocalLength () const
{
  return focal_length_;
}

template <typename PointT> inline void
pcl::DisparityMapConverter<PointT>::setBaseline (const float baseline)
{
  baseline_ = baseline;
}

template <typename PointT> inline float
pcl::DisparityMapConverter<PointT>::getBaseline () const
{
  return baseline_;
}

template <typename PointT> inline void
pcl::DisparityMapConverter<PointT>::setDisparityThresholdMin (
    const float disparity_threshold_min)
{
  disparity_threshold_min_ = disparity_threshold_min;
}

template <typename PointT> inline float
pcl::DisparityMapConverter<PointT>::getDisparityThresholdMin () const
{
  return disparity_threshold_min_;
}

template <typename PointT> inline void
pcl::DisparityMapConverter<PointT>::setDisparityThresholdMax (
    const float disparity_threshold_max)
{
  disparity_threshold_max_ = disparity_threshold_max;
}

template <typename PointT> inline float
pcl::DisparityMapConverter<PointT>::getDisparityThresholdMax () const
{
  return disparity_threshold_max_;
}

template <typename PointT> void
pcl::DisparityMapConverter<PointT>::setImage (
    const pcl::PointCloud<pcl::RGB>::ConstPtr &image)
{
  image_ = image;

  // Set disparity map's size same with the image size.
  disparity_map_width_ = image_->width;
  disparity_map_height_ = image_->height;

  is_color_ = true;
}

template <typename PointT> pcl::PointCloud<pcl::RGB>::Ptr
pcl::DisparityMapConverter<PointT>::getImage ()
{
  pcl::PointCloud<pcl::RGB>::Ptr image_pointer (new pcl::PointCloud<pcl::RGB>);
  *image_pointer = *image_;
  return image_pointer;
}

template <typename PointT> bool
pcl::DisparityMapConverter<PointT>::loadDisparityMap (
    const std::string &file_name)
{
  std::fstream disparity_file;

  // Open the disparity file
  disparity_file.open (file_name.c_str (), std::fstream::in);
  if (!disparity_file.is_open ())
  {
    PCL_ERROR ("[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
    return false;
  }

  // Allocate memory for the disparity map.
  disparity_map_.resize (disparity_map_width_ * disparity_map_height_);

  // Reading the disparity map.
  for (size_t row = 0; row < disparity_map_height_; ++row)
  {
    for (size_t column = 0; column < disparity_map_width_; ++column)
    {
      float disparity;
      disparity_file >> disparity;

      disparity_map_[column + row * disparity_map_width_] = disparity;
    } // column
  } // row

  return true;
}

template <typename PointT> bool
pcl::DisparityMapConverter<PointT>::loadDisparityMap (
    const std::string &file_name, const size_t width, const size_t height)
{
  // Initialize disparity map's size. 
  disparity_map_width_ = width;
  disparity_map_height_ = height;

  // Load the disparity map.
  return loadDisparityMap (file_name);
}

template <typename PointT> void
pcl::DisparityMapConverter<PointT>::setDisparityMap (
    const std::vector<float> &disparity_map)
{
  disparity_map_ = disparity_map;
}

template <typename PointT> void
pcl::DisparityMapConverter<PointT>::setDisparityMap (
    const std::vector<float> &disparity_map, 
    const size_t width, const size_t height)
{
  disparity_map_width_ = width;
  disparity_map_height_ = height;

  disparity_map_ = disparity_map;
}

template <typename PointT> std::vector<float>
pcl::DisparityMapConverter<PointT>::getDisparityMap ()
{
  return disparity_map_;
}

template <typename PointT> void
pcl::DisparityMapConverter<PointT>::compute (PointCloud &out_cloud)
{
  // Initialize the output cloud.
  out_cloud.clear ();
  out_cloud.width = disparity_map_width_;
  out_cloud.height = disparity_map_height_;
  out_cloud.resize (out_cloud.width * out_cloud.height);

  if (is_color_ && !image_)
  {
    PCL_ERROR ("[pcl::DisparityMapConverter::compute] Memory for the image was not allocated.\n");
    return;
  }

  for (size_t row = 0; row < disparity_map_height_; ++row)
  {
    for (size_t column = 0; column < disparity_map_width_; ++column)
    {
      // ID of current disparity point.
      size_t disparity_point = column + row * disparity_map_width_;

      // Disparity value.
      float disparity = disparity_map_[disparity_point];

      // New point for the output cloud.
      PointT new_point;
      
      // Init color
      if (is_color_)
      {
        pcl::common::IntensityFieldAccessor<PointT> intensity_accessor;
        intensity_accessor.set (new_point, static_cast<float> (
            image_->points[disparity_point].r +
            image_->points[disparity_point].g +
            image_->points[disparity_point].b) / 3.0f);
      }

      // Init coordinates.
      if (disparity_threshold_min_ < disparity && disparity < disparity_threshold_max_)
      {
        // Compute new coordinates.
        PointXYZ point_3D (translateCoordinates (row, column, disparity));
        new_point.x = point_3D.x;
        new_point.y = point_3D.y;
        new_point.z = point_3D.z;
      } 
      else
      {
        new_point.x = std::numeric_limits<float>::quiet_NaN ();
        new_point.y = std::numeric_limits<float>::quiet_NaN ();
        new_point.z = std::numeric_limits<float>::quiet_NaN ();
      }
      // Put the point to the output cloud.
      out_cloud[disparity_point] = new_point;
    } // column
  } // row
}

template <typename PointT> pcl::PointXYZ
pcl::DisparityMapConverter<PointT>::translateCoordinates (
    size_t row, size_t column, float disparity) const
{
  // Returning point.
  PointXYZ point_3D;

  if (disparity != 0.0f)
  {
    // Compute 3D-coordinates based on the image coordinates, the disparity and the camera parameters.
    float z_value = focal_length_ * baseline_ / disparity;
    point_3D.z = z_value;
    point_3D.x = (static_cast<float> (column) - center_x_) * (z_value / focal_length_);
    point_3D.y = (static_cast<float> (row) - center_y_) * (z_value / focal_length_);
  }

  return point_3D;
}

#define PCL_INSTANTIATE_DisparityMapConverter(T) template class PCL_EXPORTS pcl::DisparityMapConverter<T>;

#endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
