/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2008, Willow Garage, 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 Willow Garage 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 DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS
#define DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS

#include <robot_sensor_msgs/Image.h>
#include <robot_sensor_msgs/CameraInfo.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <robot_image_geometry/pinhole_camera_model.h>
#include <robot_depth_image_proc/depth_traits.h>
#include <robot_depth_image_proc/point_cloud_xyz.h>

#include <algorithm>
#include <cmath>
#include <limits>

namespace depth_image_proc {

typedef robot_sensor_msgs::PointCloud2 PointCloud;

// Handles float or uint16 depths
template<typename T>
void convert(
    const robot_sensor_msgs::Image& depth_msg,
    PointCloud& cloud_msg,
    const image_geometry::PinholeCameraModel& model,
    double range_max = 0.0)
{
  // Use correct principal point from calibration
  float center_x = model.cx();
  float center_y = model.cy();

  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
  double unit_scaling = DepthTraits<T>::toMeters( T(1) );
  float constant_x = unit_scaling / model.fx();
  float constant_y = unit_scaling / model.fy();
  float bad_point = std::numeric_limits<float>::quiet_NaN();

  robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
  robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
  robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
  const T* depth_row = reinterpret_cast<const T*>(&depth_msg.data[0]);
  int row_step = depth_msg.step / sizeof(T);
  for (int v = 0; v < (int)cloud_msg.height; ++v, depth_row += row_step)
  {
    for (int u = 0; u < (int)cloud_msg.width; ++u, ++iter_x, ++iter_y, ++iter_z)
    {
      T depth = depth_row[u];

      // Missing points denoted by NaNs
      if (!DepthTraits<T>::valid(depth))
      {
        if (range_max != 0.0)
        {
          depth = DepthTraits<T>::fromMeters(range_max);
        }
        else
        {
          *iter_x = *iter_y = *iter_z = bad_point;
          continue;
        }
      }

      // Fill in XYZ
      *iter_x = (u - center_x) * depth * constant_x;
      *iter_y = (v - center_y) * depth * constant_y;
      *iter_z = DepthTraits<T>::toMeters(depth);
    }
  }
}

// True when at least `min_neighbors` of the 8-connected neighbors (full
// resolution) have a depth within `max_delta` meters of `depth_m`. Isolated
// "flying pixels" at object edges fail this test.
template<typename T>
inline bool hasConsistentNeighbors(
    const T* depth_data,
    int row_step,
    int width,
    int height,
    int u,
    int v,
    float depth_m,
    float max_delta,
    int min_neighbors)
{
  int consistent = 0;
  for (int dv = -1; dv <= 1; ++dv)
  {
    const int nv = v + dv;
    if (nv < 0 || nv >= height)
    {
      continue;
    }
    const T* neighbor_row = depth_data + static_cast<size_t>(nv) * row_step;
    for (int du = -1; du <= 1; ++du)
    {
      if (du == 0 && dv == 0)
      {
        continue;
      }
      const int nu = u + du;
      if (nu < 0 || nu >= width)
      {
        continue;
      }
      const T neighbor = neighbor_row[nu];
      if (!DepthTraits<T>::valid(neighbor))
      {
        continue;
      }
      if (std::abs(DepthTraits<T>::toMeters(neighbor) - depth_m) <= max_delta)
      {
        if (++consistent >= min_neighbors)
        {
          return true;
        }
      }
    }
  }
  return false;
}

// Converts with range clipping, NxN decimation and speckle removal.
// Produces an unorganized dense cloud (height = 1, no NaN points).
// cloud_msg must already have its xyz fields set by the caller.
template<typename T>
void convertFiltered(
    const robot_sensor_msgs::Image& depth_msg,
    PointCloud& cloud_msg,
    const image_geometry::PinholeCameraModel& model,
    const DepthFilterConfig& config)
{
  const float center_x = model.cx();
  const float center_y = model.cy();

  const double unit_scaling = DepthTraits<T>::toMeters( T(1) );
  const float constant_x = unit_scaling / model.fx();
  const float constant_y = unit_scaling / model.fy();

  const int width = static_cast<int>(depth_msg.width);
  const int height = static_cast<int>(depth_msg.height);
  const int decimation = std::max(1, config.decimation);
  const int row_step = depth_msg.step / sizeof(T);
  const T* depth_data = reinterpret_cast<const T*>(&depth_msg.data[0]);

  const float range_min = static_cast<float>(config.range_min);
  const float range_max = static_cast<float>(config.range_max);
  const float speckle_delta = static_cast<float>(config.speckle_max_delta);
  const bool use_speckle = config.speckle_min_neighbors > 0;

  const size_t max_points =
      static_cast<size_t>((height + decimation - 1) / decimation) *
      static_cast<size_t>((width + decimation - 1) / decimation);

  cloud_msg.height = 1;
  cloud_msg.is_dense = true;

  robot_sensor_msgs::PointCloud2Modifier pcd_modifier(cloud_msg);
  pcd_modifier.resize(max_points);

  robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
  robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
  robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");

  size_t valid_points = 0;
  for (int v = 0; v < height; v += decimation)
  {
    const T* depth_row = depth_data + static_cast<size_t>(v) * row_step;
    for (int u = 0; u < width; u += decimation)
    {
      const T depth = depth_row[u];
      if (!DepthTraits<T>::valid(depth))
      {
        continue;
      }

      const float z = DepthTraits<T>::toMeters(depth);
      if (z < range_min || z > range_max)
      {
        continue;
      }

      if (use_speckle &&
          !hasConsistentNeighbors<T>(
              depth_data, row_step, width, height, u, v, z, speckle_delta,
              config.speckle_min_neighbors))
      {
        continue;
      }

      *iter_x = (u - center_x) * depth * constant_x;
      *iter_y = (v - center_y) * depth * constant_y;
      *iter_z = z;
      ++iter_x;
      ++iter_y;
      ++iter_z;
      ++valid_points;
    }
  }

  pcd_modifier.resize(valid_points);
}

} // namespace depth_image_proc

#endif