/*********************************************************************
 *
 * 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
 *         David V. Lu!!
 *********************************************************************/
#include <robot_costmap_2d/voxel_layer.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_geometry_msgs/Vector3.h>
#include <tf3/exceptions.h>
#include <boost/dll/alias.hpp>
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <cstring>

#define VOXEL_BITS 16

using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::FREE_SPACE;

using robot_costmap_2d::ObservationBuffer;
using robot_costmap_2d::Observation;

namespace robot_costmap_2d
{

void VoxelLayer::onInitialize()
{
  robot::NodeHandle nh("~");
  robot::NodeHandle priv_nh(nh, name_);

  ObstacleLayer::onInitialize();
  std::string config_file_name = "voxel_layer_params.yaml";
  getParams(config_file_name, priv_nh);
}

VoxelLayer::~VoxelLayer()
{}

bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
  try 
  {
    const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
    std::string folder;
    if (env_config && std::filesystem::exists(env_config))
    {
      folder = std::string(env_config);
      // robot::log_error("config_directory: %s", folder.c_str());
    }

    std::string path_source = getSourceFile(folder,config_file_name);
    
    YAML::Node config = YAML::LoadFile(path_source);
    YAML::Node layer = config["voxel_layer"];

    // publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
    enabled_ = loadParam(layer, "enabled", true);
    footprint_clearing_enabled_ = loadParam(layer, "footprint_clearing_enabled", true);
    max_obstacle_height_ = loadParam(layer, "max_obstacle_height", 2.0);
    size_z_ = loadParam(layer, "z_voxels", 10);
    origin_z_ = loadParam(layer, "origin_z", 0.0);
    z_resolution_ = loadParam(layer, "z_resolution", 0.2);
    unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_);
    mark_threshold_ = loadParam(layer, "mark_threshold", 0);
    combination_method_ = loadParam(layer, "combination_method", 0.0);

    int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
    if (nh.hasParam("enabled"))
      nh.getParam("enabled", enabled_);
    if (nh.hasParam("footprint_clearing_enabled"))
      nh.getParam("footprint_clearing_enabled", footprint_clearing_enabled_);
    if (nh.hasParam("max_obstacle_height"))
      nh.getParam("max_obstacle_height", max_obstacle_height_);
    if (nh.hasParam("z_voxels"))
    {
      nh.getParam("z_voxels", size_z);
      size_z_ = size_z;
    }
    if (nh.hasParam("origin_z"))
      nh.getParam("origin_z", origin_z_);
    if (nh.hasParam("unknown_threshold"))
    {
      nh.getParam("unknown_threshold", unknown_threshold);
      unknown_threshold_ = unknown_threshold + (VOXEL_BITS - size_z_);
    }
    if (nh.hasParam("mark_threshold"))
    {
      nh.getParam("mark_threshold", mark_threshold);
      mark_threshold_ = mark_threshold;
    }
    if (nh.hasParam("combination_method"))
      nh.getParam("combination_method", combination_method_);

    this->matchSize();
  } 
  catch (const YAML::BadFile& e) {
    std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
    return false;
  }

  return true;
}

void VoxelLayer::matchSize()
{
  ObstacleLayer::matchSize();
  robot_voxel_grid_.resize(size_x_, size_y_, size_z_);
  if (!(robot_voxel_grid_.sizeX() == size_x_ && robot_voxel_grid_.sizeY() == size_y_)) 
  {
    std::cerr << "[FATAL] Voxel grid size mismatch: "
              << "voxel(" << robot_voxel_grid_.sizeX() << ", " << robot_voxel_grid_.sizeY()
              << ") but costmap(" << size_x_ << ", " << size_y_ << ")\n";
    std::abort();  // dừng chương trình
  }
}

void VoxelLayer::reset()
{
  deactivate();
  resetMaps();
  robot_voxel_grid_.reset();
  activate();
}

void VoxelLayer::resetMaps()
{
  Costmap2D::resetMaps();
  robot_voxel_grid_.reset();
}

void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                       double* min_y, double* max_x, double* max_y)
{
  if (rolling_window_)
    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
  useExtraBounds(min_x, min_y, max_x, max_y);

  bool current = true;
  std::vector<Observation> observations, clearing_observations;
  std::vector<DepthCameraObservation> depth_observations;

  // get the marking observations
  current = getMarkingObservations(observations) && current;

  // get the clearing observations
  current = getClearingObservations(clearing_observations) && current;

  current = getFrustumClearingObservations(depth_observations) && current;

  // update the global current status
  current_ = current;

  for (const DepthCameraObservation& depth_observation : depth_observations)
  {
    raytraceDepthFrustum(depth_observation, min_x, min_y, max_x, max_y);
  }

  // raytrace freespace
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
  }

  // place the new obstacles into a priority queue... each with a priority of zero to begin with
  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
  {
    const Observation& obs = *it;

    const robot_sensor_msgs::PointCloud2& cloud = *(obs.cloud_);

    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;

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

    for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
    {
      // if the obstacle is too high or too far away from the robot we won't add it
      if (*iter_z > max_obstacle_height_)
        continue;

      // compute the squared distance from the hitpoint to the pointcloud's origin
      double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x)
                       + (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y)
                       + (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z);

      // if the point is far enough away... we won't consider it
      if (sq_dist >= sq_obstacle_range)
        continue;

      // now we need to compute the map coordinates for the observation
      unsigned int mx, my, mz;
      if (*iter_z < origin_z_)
      {
        if (!worldToMap3D(*iter_x, *iter_y, origin_z_, mx, my, mz))
          continue;
      }
      else if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz))
      {
        continue;
      }

      // mark the cell in the voxel grid and check if we should also mark it in the costmap
      if (robot_voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
      {
        unsigned int index = getIndex(mx, my);

        costmap_[index] = LETHAL_OBSTACLE;
        touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y);
      }
    }
  }
  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}

void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
{
  // get the cell coordinates of the center point of the window
  unsigned int mx, my;
  if (!worldToMap(wx, wy, mx, my))
    return;

  // compute the bounds of the window
  double start_x = wx - w_size_x / 2;
  double start_y = wy - w_size_y / 2;
  double end_x = start_x + w_size_x;
  double end_y = start_y + w_size_y;

  // scale the window based on the bounds of the costmap
  start_x = std::max(origin_x_, start_x);
  start_y = std::max(origin_y_, start_y);

  end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
  end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);

  // get the map coordinates of the bounds of the window
  unsigned int map_sx, map_sy, map_ex, map_ey;

  // check for legality just in case
  if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
    return;

  // we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
  unsigned int index = getIndex(map_sx, map_sy);
  unsigned char* current = &costmap_[index];
  for (unsigned int j = map_sy; j <= map_ey; ++j)
  {
    for (unsigned int i = map_sx; i <= map_ex; ++i)
    {
      // if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
      if (*current != LETHAL_OBSTACLE)
      {
        if (clear_no_info || *current != NO_INFORMATION)
        {
          *current = FREE_SPACE;
          robot_voxel_grid_.clearVoxelColumn(index);
        }
      }
      current++;
      index++;
    }
    current += size_x_ - (map_ex - map_sx) - 1;
    index += size_x_ - (map_ex - map_sx) - 1;
  }
}

void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
                                           double* max_x, double* max_y)
{
  size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width;
  if (clearing_observation_cloud_size == 0)
    return;

  double sensor_x, sensor_y, sensor_z;
  double ox = clearing_observation.origin_.x;
  double oy = clearing_observation.origin_.y;
  double oz = clearing_observation.origin_.z;

  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
  {
    robot::log_error(
        "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
        ox, oy, oz);
    return;
  }
  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
  double map_end_x = origin_x_ + getSizeInMetersX();
  double map_end_y = origin_y_ + getSizeInMetersY();

  robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
  robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
  robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");

  for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
  {
    double wpx = *iter_x;
    double wpy = *iter_y;
    double wpz = *iter_z;

    double distance = dist(ox, oy, oz, wpx, wpy, wpz);
    double scaling_fact = 1.0;
    scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
    wpx = scaling_fact * (wpx - ox) + ox;
    wpy = scaling_fact * (wpy - oy) + oy;
    wpz = scaling_fact * (wpz - oz) + oz;

    double a = wpx - ox;
    double b = wpy - oy;
    double c = wpz - oz;
    double t = 1.0;

    // we can only raytrace to a maximum z height
    if (wpz > max_obstacle_height_)
    {
      // we know we want the vector's z value to be max_z
      t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
    }
    // and we can only raytrace down to the floor
    else if (wpz < origin_z_)
    {
      // we know we want the vector's z value to be 0.0
      t = std::min(t, (origin_z_ - oz) / c);
    }

    // the minimum value to raytrace from is the origin
    if (wpx < origin_x_)
    {
      t = std::min(t, (origin_x_ - ox) / a);
    }
    if (wpy < origin_y_)
    {
      t = std::min(t, (origin_y_ - oy) / b);
    }

    // the maximum value to raytrace to is the end of the map
    if (wpx > map_end_x)
    {
      t = std::min(t, (map_end_x - ox) / a);
    }
    if (wpy > map_end_y)
    {
      t = std::min(t, (map_end_y - oy) / b);
    }

    wpx = ox + a * t;
    wpy = oy + b * t;
    wpz = oz + c * t;

    double point_x, point_y, point_z;
    if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
    {
      unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);

      // robot_voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
      robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
                                      unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
                                      cell_raytrace_range);

      updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
    }
  }
}

bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
                                 double& depth_m, bool& is_valid) const
{
  depth_m = 0.0;
  is_valid = false;

  if (u >= depth.width || v >= depth.height)
    return false;

  if (depth.encoding == "16UC1" || depth.encoding == "mono16")
  {
    const std::size_t offset = static_cast<std::size_t>(v) * depth.step + static_cast<std::size_t>(u) * 2;
    if (offset + sizeof(std::uint16_t) > depth.data.size())
      return false;

    std::uint16_t raw = 0;
    if (depth.is_bigendian)
      raw = static_cast<std::uint16_t>((depth.data[offset] << 8) | depth.data[offset + 1]);
    else
      raw = static_cast<std::uint16_t>(depth.data[offset] | (depth.data[offset + 1] << 8));

    if (raw == 0)
      return true;

    depth_m = static_cast<double>(raw) * 0.001;
    is_valid = true;
    return true;
  }

  if (depth.encoding == "32FC1")
  {
    const std::size_t offset = static_cast<std::size_t>(v) * depth.step + static_cast<std::size_t>(u) * 4;
    if (offset + sizeof(float) > depth.data.size())
      return false;

    float raw = 0.0f;
    if (depth.is_bigendian)
    {
      unsigned char bytes[sizeof(float)] = {
          depth.data[offset + 3], depth.data[offset + 2], depth.data[offset + 1], depth.data[offset]};
      std::memcpy(&raw, bytes, sizeof(float));
    }
    else
    {
      std::memcpy(&raw, &depth.data[offset], sizeof(float));
    }

    if (!std::isfinite(raw) || raw <= 0.0f)
      return true;

    depth_m = static_cast<double>(raw);
    is_valid = true;
    return true;
  }

  robot::log_error("VoxelLayer unsupported depth encoding for frustum clearing: %s\n", depth.encoding.c_str());
  return false;
}

void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
                                     unsigned int pixel_step, double fx, double fy,
                                     double cx, double cy)
{
  if (cached_depth_width_ == width && cached_depth_height_ == height &&
      cached_depth_pixel_step_ == pixel_step && cached_fx_ == fx && cached_fy_ == fy &&
      cached_cx_ == cx && cached_cy_ == cy)
  {
    return;
  }

  cached_depth_width_ = width;
  cached_depth_height_ = height;
  cached_depth_pixel_step_ = pixel_step;
  cached_fx_ = fx;
  cached_fy_ = fy;
  cached_cx_ = cx;
  cached_cy_ = cy;

  // Sample every pixel_step-th row/column and always include the last image
  // row/column, so cells marked from border pixels stay inside the swept
  // clearing fan.
  std::vector<unsigned int> u_samples, v_samples;
  u_samples.reserve(width / pixel_step + 2);
  v_samples.reserve(height / pixel_step + 2);
  for (unsigned int u = 0; u < width; u += pixel_step)
    u_samples.push_back(u);
  if (width > 0 && u_samples.back() != width - 1)
    u_samples.push_back(width - 1);
  for (unsigned int v = 0; v < height; v += pixel_step)
    v_samples.push_back(v);
  if (height > 0 && v_samples.back() != height - 1)
    v_samples.push_back(height - 1);

  cached_column_count_ = static_cast<unsigned int>(u_samples.size());
  depth_ray_cache_.clear();
  depth_ray_cache_.reserve(u_samples.size() * v_samples.size());

  for (const unsigned int v : v_samples)
  {
    for (unsigned int col = 0; col < u_samples.size(); ++col)
    {
      const unsigned int u = u_samples[col];
      const double x = (static_cast<double>(u) - cx) / fx;
      const double y = (static_cast<double>(v) - cy) / fy;
      const double inverse_norm = 1.0 / std::sqrt(x * x + y * y + 1.0);
      depth_ray_cache_.push_back(
          DepthRay{u, v, col, x * inverse_norm, y * inverse_norm, inverse_norm});
    }
  }
}

bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz)
{
  double a = wx - ox;
  double b = wy - oy;
  double c = wz - oz;
  double t = 1.0;
  constexpr double kEpsilon = 1e-9;

  if (std::fabs(a) < kEpsilon && std::fabs(b) < kEpsilon && std::fabs(c) < kEpsilon)
    return false;

  if (wz > max_obstacle_height_ && std::fabs(c) > kEpsilon)
    t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
  else if (wz < origin_z_ && std::fabs(c) > kEpsilon)
    t = std::min(t, (origin_z_ - oz) / c);

  const double map_end_x = origin_x_ + getSizeInMetersX();
  const double map_end_y = origin_y_ + getSizeInMetersY();

  if (wx < origin_x_ && std::fabs(a) > kEpsilon)
    t = std::min(t, (origin_x_ - ox) / a);
  if (wy < origin_y_ && std::fabs(b) > kEpsilon)
    t = std::min(t, (origin_y_ - oy) / b);
  if (wx > map_end_x && std::fabs(a) > kEpsilon)
    t = std::min(t, (map_end_x - ox) / a);
  if (wy > map_end_y && std::fabs(b) > kEpsilon)
    t = std::min(t, (map_end_y - oy) / b);

  if (!std::isfinite(t) || t <= 0.0)
    return false;

  wx = ox + a * t;
  wy = oy + b * t;
  wz = oz + c * t;
  return true;
}

bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
                               double raytrace_range, unsigned int cell_raytrace_range,
                               double* min_x, double* min_y, double* max_x, double* max_y)
{
  double sensor_x, sensor_y, sensor_z;
  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
    return false;

  if (!clipRaytraceEndpoint(ox, oy, oz, wx, wy, wz))
    return false;

  double point_x, point_y, point_z;
  if (!worldToMap3DFloat(wx, wy, wz, point_x, point_y, point_z))
    return false;

  robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
                                        unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
                                        cell_raytrace_range);
  updateRaytraceBounds(ox, oy, wx, wy, raytrace_range, min_x, min_y, max_x, max_y);
  return true;
}

bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
                                     double* min_x, double* min_y, double* max_x, double* max_y)
{
  if (!observation.data_)
    return false;

  const robot_sensor_msgs::DepthCameraData& depth_camera_data = *observation.data_;
  const robot_sensor_msgs::Image& depth = depth_camera_data.depth;
  const robot_sensor_msgs::CameraInfo& camera_info = depth_camera_data.camera_info;

  if (depth.width == 0 || depth.height == 0 || depth.data.empty())
    return false;

  const double fx = camera_info.K[0];
  const double fy = camera_info.K[4];
  const double cx = camera_info.K[2];
  const double cy = camera_info.K[5];
  if (fx <= 0.0 || fy <= 0.0)
    return false;

  std::string depth_frame = depth.header.frame_id.empty() ? depth_camera_data.header.frame_id : depth.header.frame_id;
  if (depth_frame.empty())
    depth_frame = camera_info.header.frame_id;
  if (depth_frame.empty() || tf_ == nullptr)
    return false;

  robot_geometry_msgs::PointStamped local_origin;
  local_origin.header = depth.header;
  local_origin.header.frame_id = depth_frame;
  if (local_origin.header.stamp.isZero())
    local_origin.header.stamp = depth_camera_data.header.stamp;
  local_origin.point.x = 0.0;
  local_origin.point.y = 0.0;
  local_origin.point.z = 0.0;

  // Look up the sensor pose at the depth image's CAPTURE time, not the latest
  // transform. The costmap update runs later than the frame was captured, so
  // during rotation the latest pose orients the clearing frustum where the depth
  // pixels were never measured from; the fan's free rays then sweep across and
  // erase freshly marked cells, and the trailing side that gets erased flips
  // with rotation direction. A stamped lookup keeps the frustum geometrically
  // consistent with its own pixels. If the transform at that stamp is
  // unavailable (stale / would extrapolate), skip clearing this cycle instead of
  // clearing from a wrong pose. Falls back to latest only when the frame carries
  // no stamp.
  const robot::Time& depth_stamp = local_origin.header.stamp;
  const tf3::Time query_time =
      depth_stamp.isZero() ? tf3::Time() : tf3::Time(depth_stamp.sec, depth_stamp.nsec);

  robot_geometry_msgs::PointStamped global_origin;
  tf3::TransformStampedMsg tfm;
  try
  {
    tfm = tf_->lookupTransform(global_frame_, depth_frame, query_time);
    tf3::doTransform(local_origin, global_origin, tfm);
  }
  catch (tf3::TransformException& ex)
  {
    robot::log_error_throttle(
        5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s at t=%.3f: %s\n",
        observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(),
        query_time.toSec(), ex.what());
    return false;
  }

  const double ox = global_origin.point.x;
  const double oy = global_origin.point.y;
  const double oz = global_origin.point.z;

  double sensor_x, sensor_y, sensor_z;
  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
  {
    robot::log_error_throttle(
        5.0, "VoxelLayer depth topic [%s] origin at (%.2f, %.2f, %.2f) is outside the voxel map\n",
        observation.topic_.c_str(), ox, oy, oz);
    return false;
  }

  const DepthFrustumConfig& frustum = observation.frustum_;
  const unsigned int step = std::max(1u, frustum.pixel_step);
  const double min_range = frustum.min_range;
  const double max_range = frustum.max_range;
  const double skip_dist =
      frustum.skip_distance >= 0.0 ? frustum.skip_distance : 2.0 * resolution_;
  const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width);
  const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height);
  updateDepthRayCache(width, height, step, fx, fy, cx, cy);

  double qx = tfm.transform.rotation.x;
  double qy = tfm.transform.rotation.y;
  double qz = tfm.transform.rotation.z;
  double qw = tfm.transform.rotation.w;
  const double quaternion_norm = std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
  if (quaternion_norm <= 0.0)
    return false;
  qx /= quaternion_norm;
  qy /= quaternion_norm;
  qz /= quaternion_norm;
  qw /= quaternion_norm;

  const double r00 = 1.0 - 2.0 * (qy * qy + qz * qz);
  const double r01 = 2.0 * (qx * qy - qz * qw);
  const double r02 = 2.0 * (qx * qz + qy * qw);
  const double r10 = 2.0 * (qx * qy + qz * qw);
  const double r11 = 1.0 - 2.0 * (qx * qx + qz * qz);
  const double r12 = 2.0 * (qy * qz - qx * qw);
  const double r20 = 2.0 * (qx * qz - qy * qw);
  const double r21 = 2.0 * (qy * qz + qx * qw);
  const double r22 = 1.0 - 2.0 * (qx * qx + qy * qy);
  const unsigned int cell_raytrace_range = cellDistance(max_range);
  bool cleared_any = false;

  // Column clearing: certify the beam length per pixel column and the distance
  // window [cover, far] where the vertical FOV spans the whole height band.
  // Outside that window a real obstacle could sit above/below the FOV, so only
  // the per-pixel 3D rays may clear there.
  const double band_min_h = frustum.column_min_height;
  const double band_max_h =
      frustum.column_max_height >= 0.0 ? frustum.column_max_height : max_obstacle_height_;
  double cover_dist = frustum.column_cover_distance;
  double far_dist = std::numeric_limits<double>::infinity();
  bool column_pass = frustum.column_clearing && band_max_h > band_min_h;

  if (column_pass && cover_dist < 0.0)
  {
    const double up_half = std::atan2(cy, fy);
    const double down_half = std::atan2(static_cast<double>(height) - 1.0 - cy, fy);
    const double axis_elev = std::atan2(r22, std::hypot(r02, r12));
    const double alpha_top = axis_elev + up_half;
    const double alpha_bot = axis_elev - down_half;
    const double band_top = band_max_h - oz;
    const double band_bot = band_min_h - oz;
    constexpr double kMinSlope = 1e-3;

    cover_dist = 0.0;
    if (band_top > 0.0)
    {
      if (alpha_top <= kMinSlope)
        column_pass = false;  // camera can never look up to the band top
      else
        cover_dist = std::max(cover_dist, band_top / std::tan(alpha_top));
    }
    else if (alpha_top < -kMinSlope)
    {
      far_dist = std::min(far_dist, band_top / std::tan(alpha_top));
    }
    if (band_bot < 0.0)
    {
      if (alpha_bot >= -kMinSlope)
        column_pass = false;  // camera can never look down to the band bottom
      else
        cover_dist = std::max(cover_dist, band_bot / std::tan(alpha_bot));
    }
    else if (alpha_bot > kMinSlope)
    {
      far_dist = std::min(far_dist, band_bot / std::tan(alpha_bot));
    }

    if (!column_pass)
    {
      robot::log_warning_throttle(
          10.0, "VoxelLayer column clearing disabled: vertical FOV [%.1f, %.1f] deg at camera "
          "height %.2f m never covers band [%.2f, %.2f] m\n",
          alpha_bot * 180.0 / M_PI, alpha_top * 180.0 / M_PI, oz, band_min_h, band_max_h);
    }
  }

  if (column_pass)
    depth_column_stats_.assign(cached_column_count_, DepthColumnStat());

  for (const DepthRay& local_ray : depth_ray_cache_)
  {
    double depth_m = 0.0;
    bool valid = false;
    if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
      continue;

    // Left-edge stereo no-disparity strip. An INVALID pixel here is structurally
    // invalid (carries no free-space evidence), so clearing it out to max_range
    // erases obstacles rotating out of the FOV on that side (the CW-turn bug) —
    // skip only those. A VALID return here is a real measured surface, so it must
    // still clear normally; otherwise the whole left border becomes a clearing
    // dead zone and obstacles there never get cleared. Invalid pixels OUTSIDE the
    // strip still clear to max_range (ghost removal).
    const bool in_left_border = local_ray.u < frustum.clear_left_border_px;
    if (in_left_border && !valid)
      continue;

    // depth images store z-depth; local_ray.z is the unit ray's optical axis
    // component, so depth / z is the Euclidean range
    const double euclid_range = valid ? depth_m / local_ray.z : 0.0;

    robot_geometry_msgs::Vector3 global_ray;
    global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z;
    global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z;
    global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z;

    if (column_pass && local_ray.col < depth_column_stats_.size())
    {
      const double horiz_norm = std::hypot(global_ray.x, global_ray.y);
      if (horiz_norm > 1e-6)
      {
        DepthColumnStat& stat = depth_column_stats_[local_ray.col];
        const double row_delta = std::fabs(static_cast<double>(local_ray.v) - cy);
        if (stat.min_band_dist < 0.0 && row_delta < stat.best_row_delta)
        {
          // no in-band return yet: aim the beam along the ray nearest the
          // principal row
          stat.azimuth = std::atan2(global_ray.y, global_ray.x);
          stat.best_row_delta = row_delta;
        }
        stat.has_ray = true;
        if (in_left_border)
          stat.in_left_border = true;

        if (valid)
        {
          const double pz = oz + global_ray.z * euclid_range;
          if (pz >= band_min_h && pz <= band_max_h)
          {
            const double dist_h = horiz_norm * euclid_range;
            if (stat.min_band_dist < 0.0 || dist_h < stat.min_band_dist)
            {
              stat.min_band_dist = dist_h;
              stat.azimuth = std::atan2(global_ray.y, global_ray.x);
            }
          }
        }
      }
    }

    double ray_len = max_range;
    if (valid && euclid_range < max_range)
      ray_len = std::max(0.0, euclid_range - skip_dist);

    if (ray_len <= min_range)
      continue;

    const double sx = ox + global_ray.x * min_range;
    const double sy = oy + global_ray.y * min_range;
    const double sz = oz + global_ray.z * min_range;
    const double wx = ox + global_ray.x * ray_len;
    const double wy = oy + global_ray.y * ray_len;
    const double wz = oz + global_ray.z * ray_len;

    cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, cell_raytrace_range,
                                min_x, min_y, max_x, max_y) || cleared_any;
  }

  if (column_pass)
  {
    cleared_any = clearDepthColumns(ox, oy, cover_dist, far_dist, min_range, max_range,
                                    std::max(0.0, frustum.column_skip_distance),
                                    min_x, min_y, max_x, max_y) ||
                  cleared_any;
  }

  return cleared_any;
}

namespace
{
/// raytraceLine action: frees the 2D cell and wipes its whole voxel column.
class ClearFullColumn
{
public:
  ClearFullColumn(unsigned char* costmap, robot_voxel_grid::VoxelGrid& voxel_grid)
    : costmap_(costmap), voxel_grid_(voxel_grid)
  {
  }

  inline void operator()(unsigned int offset)
  {
    costmap_[offset] = FREE_SPACE;
    voxel_grid_.clearVoxelColumn(offset);
  }

private:
  unsigned char* costmap_;
  robot_voxel_grid::VoxelGrid& voxel_grid_;
};
}  // namespace

bool VoxelLayer::clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const
{
  // Liang-Barsky clip against the map interior; the half-resolution margin
  // keeps clipped endpoints valid for worldToMap.
  const double min_wx = origin_x_;
  const double min_wy = origin_y_;
  const double max_wx = origin_x_ + getSizeInMetersX() - 0.5 * resolution_;
  const double max_wy = origin_y_ + getSizeInMetersY() - 0.5 * resolution_;
  const double dx = ex - sx;
  const double dy = ey - sy;
  const double p[4] = {-dx, dx, -dy, dy};
  const double q[4] = {sx - min_wx, max_wx - sx, sy - min_wy, max_wy - sy};

  double t0 = 0.0;
  double t1 = 1.0;
  for (int i = 0; i < 4; ++i)
  {
    if (std::fabs(p[i]) < 1e-12)
    {
      if (q[i] < 0.0)
        return false;
      continue;
    }
    const double r = q[i] / p[i];
    if (p[i] < 0.0)
      t0 = std::max(t0, r);
    else
      t1 = std::min(t1, r);
  }
  if (t0 > t1)
    return false;

  const double bx = sx;
  const double by = sy;
  sx = bx + t0 * dx;
  sy = by + t0 * dy;
  ex = bx + t1 * dx;
  ey = by + t1 * dy;
  return true;
}

bool VoxelLayer::clearDepthColumns(double ox, double oy, double cover_distance,
                                   double far_distance, double min_range, double max_range,
                                   double skip_dist, double* min_x, double* min_y,
                                   double* max_x, double* max_y)
{
  const double start_dist = std::max(cover_distance, min_range);
  bool cleared_any = false;

  for (const DepthColumnStat& stat : depth_column_stats_)
  {
    if (!stat.has_ray)
      continue;

    // In the left stereo strip, never clear a whole column out to max_range on a
    // missing in-band return: that is exactly the no-free-space-evidence case that
    // erases obstacles turning out of view. Only an in-band measured surface may
    // shorten (and thus clear) a border column.
    if (stat.in_left_border && stat.min_band_dist < 0.0)
      continue;

    double end_dist = stat.min_band_dist >= 0.0 ? stat.min_band_dist - skip_dist : max_range;
    end_dist = std::min(std::min(end_dist, max_range), far_distance);
    if (end_dist <= start_dist)
      continue;

    const double cos_az = std::cos(stat.azimuth);
    const double sin_az = std::sin(stat.azimuth);
    double sx = ox + cos_az * start_dist;
    double sy = oy + sin_az * start_dist;
    double ex = ox + cos_az * end_dist;
    double ey = oy + sin_az * end_dist;
    if (!clipColumnSegment(sx, sy, ex, ey))
      continue;

    unsigned int sx_m, sy_m, ex_m, ey_m;
    if (!worldToMap(sx, sy, sx_m, sy_m) || !worldToMap(ex, ey, ex_m, ey_m))
      continue;

    ClearFullColumn clearer(costmap_, robot_voxel_grid_);
    raytraceLine(clearer, sx_m, sy_m, ex_m, ey_m);
    touch(sx, sy, min_x, min_y, max_x, max_y);
    touch(ex, ey, min_x, min_y, max_x, max_y);
    cleared_any = true;
  }

  return cleared_any;
}

void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
{
  // project the new origin into the grid
  int cell_ox, cell_oy;
  cell_ox = int((new_origin_x - origin_x_) / resolution_);
  cell_oy = int((new_origin_y - origin_y_) / resolution_);

  // Most update cycles do not cross a costmap cell boundary. Avoid copying and
  // resetting the complete 2D/3D grids when the cell-aligned origin is unchanged.
  if (cell_ox == 0 && cell_oy == 0)
    return;

  // compute the associated world coordinates for the origin cell
  // beacuase we want to keep things grid-aligned
  double new_grid_ox, new_grid_oy;
  new_grid_ox = origin_x_ + cell_ox * resolution_;
  new_grid_oy = origin_y_ + cell_oy * resolution_;

  // To save casting from unsigned int to int a bunch of times
  int size_x = size_x_;
  int size_y = size_y_;

  // we need to compute the overlap of the new and existing windows
  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
  lower_left_x = std::min(std::max(cell_ox, 0), size_x);
  lower_left_y = std::min(std::max(cell_oy, 0), size_y);
  upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
  upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);

  unsigned int cell_size_x = upper_right_x - lower_left_x;
  unsigned int cell_size_y = upper_right_y - lower_left_y;

  const std::size_t overlap_size = static_cast<std::size_t>(cell_size_x) * cell_size_y;
  rolling_costmap_scratch_.resize(overlap_size);
  rolling_voxel_scratch_.resize(overlap_size);
  unsigned char* local_map = rolling_costmap_scratch_.data();
  unsigned int* local_voxel_map = rolling_voxel_scratch_.data();
  unsigned int* voxel_map = robot_voxel_grid_.getData();

  if (overlap_size > 0)
  {
    copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0,
                  cell_size_x, cell_size_x, cell_size_y);
    copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0,
                  cell_size_x, cell_size_x, cell_size_y);
  }

  // we'll reset our maps to unknown space if appropriate
  resetMaps();

  // update the origin with the appropriate world coordinates
  origin_x_ = new_grid_ox;
  origin_y_ = new_grid_oy;

  // compute the starting cell location for copying data back in
  int start_x = lower_left_x - cell_ox;
  int start_y = lower_left_y - cell_oy;

  // now we want to copy the overlapping information back into the map, but in its new location
  if (overlap_size > 0)
  {
    copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y,
                  size_x_, cell_size_x, cell_size_y);
    copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y,
                  size_x_, cell_size_x, cell_size_y);
  }

}

// Export factory function
static boost::shared_ptr<Layer> create_voxel_plugin() {
    return boost::make_shared<VoxelLayer>();
}

// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_voxel_plugin, VoxelLayer)

}  // namespace robot_costmap_2d