/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, 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 Willow Garage, Inc. 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.
 *
 * Author: Eitan Marder-Eppstein
 *********************************************************************/
#include <robot_costmap_2d/observation_buffer.h>

#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>

#include <cstring>

using namespace std;
using namespace tf3;

namespace robot_costmap_2d
{
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
                                     double min_obstacle_height, double max_obstacle_height, double obstacle_range,
                                     double raytrace_range, tf3::BufferCore& tf3_buffer, string global_frame,
                                     string sensor_frame, double tf_tolerance) :
    tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
    last_updated_(robot::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
    min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
    obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
{
}

ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
                                     double min_obstacle_height, double max_obstacle_height, double obstacle_range,
                                     double raytrace_range, const DepthFrustumConfig& frustum_config,
                                     tf3::BufferCore& tf3_buffer, string global_frame,
                                     string sensor_frame, double tf_tolerance) :
    tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
    last_updated_(robot::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
    min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
    obstacle_range_(obstacle_range), raytrace_range_(raytrace_range),
    tf_tolerance_(tf_tolerance),
    frustum_config_(frustum_config)
{
  frustum_config_.pixel_step = std::max(1u, frustum_config_.pixel_step);
  frustum_config_.min_range = std::max(0.0, frustum_config_.min_range);
  frustum_config_.max_range = std::max(frustum_config_.max_range, frustum_config_.min_range);
}

ObservationBuffer::~ObservationBuffer()
{
}

bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
{
  tf3::Time transform_time = tf3::Time::now();
  std::string tf_error;

  robot_geometry_msgs::TransformStamped transformStamped;
  if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
  {
    robot::log_error("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
              global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
    return false;
  }

  list<Observation>::iterator obs_it;
  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
  {
    try
    {
      Observation& obs = *obs_it;

      if (!obs.cloud_handle_.unique())
      {
        obs.cloud_handle_ = boost::make_shared<robot_sensor_msgs::PointCloud2>(*obs.cloud_);
        obs.cloud_ = obs.cloud_handle_.get();
      }

      robot_geometry_msgs::PointStamped origin;
      origin.header.frame_id = global_frame_;
      origin.header.stamp = data_convert::convertTime(transform_time);
      origin.point = obs.origin_;

      // we need to transform the origin of the observation to the new global frame
      // tf3_buffer_.transform(origin, origin, new_global_frame);
      tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
                        new_global_frame,                      // frame đích
                        origin.header.frame_id,                      // frame nguồn
                        transform_time
                        );
      tf3::doTransform(origin, origin, tfm_1);
      obs.origin_ = origin.point;

      // we also need to transform the cloud of the observation to the new global frame
      // tf3_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
      tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
                        new_global_frame,                      // frame đích
                        obs.cloud_->header.frame_id,                      // frame nguồn
                        transform_time
                        );
      tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
    }
    catch (TransformException& ex)
    {
      robot::log_error("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(),
                new_global_frame.c_str(), ex.what());
      return false;
    }
  }

  // now we need to update our global_frame member
  global_frame_ = new_global_frame;
  return true;
}

void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
{
  robot_geometry_msgs::PointStamped global_origin;
  Observation observation;

  // check whether the origin frame has been set explicitly or whether we should get it from the cloud
  string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;

  try
  {
    // given these observations come from sensors... we'll need to store the origin pt of the sensor
    robot_geometry_msgs::PointStamped local_origin;
    local_origin.header.stamp = cloud.header.stamp;
    local_origin.header.frame_id = origin_frame;
    local_origin.point.x = 0;
    local_origin.point.y = 0;
    local_origin.point.z = 0;
    // tf3_buffer_.transform(local_origin, global_origin, global_frame_);
    const tf3::TransformStampedMsg cloud_transform = tf3_buffer_.lookupTransform(
        global_frame_, cloud.header.frame_id, tf3::Time());
    if (origin_frame == cloud.header.frame_id)
      tf3::doTransform(local_origin, global_origin, cloud_transform);
    else
      tf3::doTransform(
          local_origin, global_origin,
          tf3_buffer_.lookupTransform(global_frame_, origin_frame, tf3::Time()));

    tf3::convert(global_origin.point, observation.origin_);
    observation.raytrace_range_ = raytrace_range_;
    observation.obstacle_range_ = obstacle_range_;

    robot_sensor_msgs::PointCloud2& observation_cloud = *observation.cloud_;
    tf3::doTransform(cloud, observation_cloud, cloud_transform);
    observation_cloud.header.stamp = cloud.header.stamp;

    const std::size_t cloud_size =
        static_cast<std::size_t>(observation_cloud.height) * observation_cloud.width;
    const std::size_t point_step = observation_cloud.point_step;
    std::size_t point_count = 0;
    robot_sensor_msgs::PointCloud2Iterator<float> iter_z(observation_cloud, "z");

    // Compact accepted points in-place. This avoids allocating and copying a
    // second full-size filtered cloud after the TF transform.
    for (std::size_t read_index = 0; read_index < cloud_size; ++read_index, ++iter_z)
    {
      if ((*iter_z) > max_obstacle_height_ || (*iter_z) < min_obstacle_height_)
        continue;

      if (point_count != read_index)
      {
        std::memmove(observation_cloud.data.data() + point_count * point_step,
                     observation_cloud.data.data() + read_index * point_step,
                     point_step);
      }
      ++point_count;
    }

    if (point_count != cloud_size)
    {
      robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
      modifier.resize(point_count);
    }
  }
  catch (TransformException& ex)
  {
    robot::log_error("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
              cloud.header.frame_id.c_str(), ex.what());
    return;
  }

  if (observation_keep_time_ == robot::Duration(0.0) && !observation_list_.empty())
  {
    observation_list_.front() = std::move(observation);
    observation_list_.erase(++observation_list_.begin(), observation_list_.end());
  }
  else
  {
    observation_list_.push_front(std::move(observation));
  }

  // if the update was successful, we want to update the last updated time
  last_updated_ = robot::Time::now();

  // we'll also remove any stale observations from the list
  purgeStaleObservations();
}

void ObservationBuffer::bufferDepthCamera(const robot_sensor_msgs::DepthCameraData& depth_camera_data)
{
  bufferDepthCamera(boost::make_shared<robot_sensor_msgs::DepthCameraData>(depth_camera_data));
}

void ObservationBuffer::bufferDepthCamera(robot_sensor_msgs::DepthCameraData::ConstPtr depth_camera_data)
{
  if (!depth_camera_data)
    return;

  DepthCameraObservation observation(
      std::move(depth_camera_data), topic_name_, robot::Time::now(), frustum_config_);

  if (observation_keep_time_ == robot::Duration(0.0) && !depth_observation_list_.empty())
  {
    depth_observation_list_.front() = std::move(observation);
    depth_observation_list_.erase(++depth_observation_list_.begin(), depth_observation_list_.end());
  }
  else
  {
    depth_observation_list_.push_front(std::move(observation));
  }

  // if the update was successful, we want to update the last updated time
  last_updated_ = robot::Time::now();

  // first... let's make sure that we don't have any stale observations
  purgeStaleDepthObservations();
}

// returns a copy of the observations
void ObservationBuffer::getObservations(vector<Observation>& observations)
{
  // first... let's make sure that we don't have any stale observations
  purgeStaleObservations();

  // now we'll just copy the observations for the caller
  list<Observation>::iterator obs_it;
  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
  {
    observations.push_back(*obs_it);
  }
}

void ObservationBuffer::getDepthObservations(vector<DepthCameraObservation>& observations)
{
  // first... let's make sure that we don't have any stale observations
  purgeStaleDepthObservations();

  // now we'll just copy the observations for the caller
  if (observation_keep_time_ == robot::Duration(0.0))
  {
    if (!depth_observation_list_.empty())
    {
      observations.push_back(std::move(depth_observation_list_.front()));
      depth_observation_list_.clear();
    }
    return;
  }

  observations.insert(
      observations.end(), depth_observation_list_.begin(), depth_observation_list_.end());
}

void ObservationBuffer::purgeStaleObservations()
{
  if (!observation_list_.empty())
  {
    list<Observation>::iterator obs_it = observation_list_.begin();
    // if we're keeping observations for no time... then we'll only keep one observation
    if (observation_keep_time_ == robot::Duration(0.0))
    {
      observation_list_.erase(++obs_it, observation_list_.end());
      return;
    }

    // otherwise... we'll have to loop through the observations to see which ones are stale
    for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
    {
      Observation& obs = *obs_it;
      // check if the observation is out of date... and if it is, remove it and those that follow from the list
      if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
      {
        observation_list_.erase(obs_it, observation_list_.end());
        return;
      }
    }
  }
}

void ObservationBuffer::purgeStaleDepthObservations()
{
  if (depth_observation_list_.empty())
    return;

  if (observation_keep_time_ == robot::Duration(0.0))
  {
    auto observation = depth_observation_list_.begin();
    depth_observation_list_.erase(++observation, depth_observation_list_.end());
    return;
  }

  const robot::Time now = robot::Time::now();
  for (auto observation = depth_observation_list_.begin(); observation != depth_observation_list_.end(); ++observation)
  {
    DepthCameraObservation& obs = *observation;
    if ((last_updated_ - obs.data_->header.stamp) > observation_keep_time_)
    {
      depth_observation_list_.erase(observation, depth_observation_list_.end());
      return;
    }
  }
}

bool ObservationBuffer::isCurrent() const
{
  if (expected_update_rate_ == robot::Duration(0.0))
    return true;

  bool current = (robot::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
  if (!current)
  {
    robot::log_error("The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
        topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
  }
  return current;
}

void ObservationBuffer::resetLastUpdated()
{
  last_updated_ = robot::Time::now();
}
}  // namespace robot_costmap_2d