/*********************************************************************
 *
 * 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 <algorithm>
#include <robot_costmap_2d/inflation_layer.h>
#include <robot_costmap_2d/costmap_math.h>
#include <robot_costmap_2d/footprint.h>
#include <boost/thread.hpp>
#include <boost/dll/alias.hpp>


using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using robot_costmap_2d::NO_INFORMATION;

namespace robot_costmap_2d
{

InflationLayer::InflationLayer()
  : resolution_(0)
  , inflation_radius_(0)
  , inscribed_radius_(0)
  , weight_(0)
  , inflate_unknown_(false)
  , cell_inflation_radius_(0)
  , cached_cell_inflation_radius_(0)
  , cached_costs_(NULL)
  , cached_distances_(NULL)
  , last_min_x_(-std::numeric_limits<float>::max())
  , last_min_y_(-std::numeric_limits<float>::max())
  , last_max_x_(std::numeric_limits<float>::max())
  , last_max_y_(std::numeric_limits<float>::max())
{
  inflation_access_ = new boost::recursive_mutex();
}

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

  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
  current_ = true;
  seen_.clear();
  seen_generation_ = 0;
  need_reinflation_ = false;
  std::string config_file_name = "inflation_layer_params.yaml";
  // std::cout << "InflationLayer: " << config_file_name << std::endl;
  getParams(config_file_name, priv_nh);

  matchSize();
}

bool InflationLayer::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["inflation_layer"];
        double cost_scaling_factor = loadParam(layer, "cost_scaling_factor", 15.0);
        double inflation_radius = loadParam(layer, "inflation_radius", 0.55);

        if (nh.hasParam("cost_scaling_factor"))
          nh.getParam("cost_scaling_factor", cost_scaling_factor);
        if (nh.hasParam("inflation_radius"))
          nh.getParam("inflation_radius", inflation_radius);

        setInflationParameters(inflation_radius, cost_scaling_factor);

        bool enabled = loadParam(layer, "enabled", true);
        bool inflate_unknown = loadParam(layer, "inflate_unknown", false);

        if (nh.hasParam("enabled"))
          nh.getParam("enabled", enabled);
        if (nh.hasParam("inflate_unknown"))
          nh.getParam("inflate_unknown", inflate_unknown);

        if (enabled_ != enabled || inflate_unknown_ != inflate_unknown) {
            enabled_ = enabled;
            inflate_unknown_ = inflate_unknown;
            need_reinflation_ = true;
        }
    } 
    catch (const YAML::BadFile& e) {
        std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
        return false;
    }

    return true;

}

void InflationLayer::matchSize()
{
  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
  robot_costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
  resolution_ = costmap->getResolution();
  cell_inflation_radius_ = cellDistance(inflation_radius_);
  computeCaches();

  unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
  seen_.assign(static_cast<std::size_t>(size_x) * size_y, 0);
  seen_generation_ = 0;
}

void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                           double* min_y, double* max_x, double* max_y)
{
  if (need_reinflation_)
  {
    last_min_x_ = *min_x;
    last_min_y_ = *min_y;
    last_max_x_ = *max_x;
    last_max_y_ = *max_y;
    // For some reason when I make these -<double>::max() it does not
    // work with Costmap2D::worldToMapEnforceBounds(), so I'm using
    // -<float>::max() instead.
    *min_x = -std::numeric_limits<float>::max();
    *min_y = -std::numeric_limits<float>::max();
    *max_x = std::numeric_limits<float>::max();
    *max_y = std::numeric_limits<float>::max();
    need_reinflation_ = false;
  }
  else
  {
    double tmp_min_x = last_min_x_;
    double tmp_min_y = last_min_y_;
    double tmp_max_x = last_max_x_;
    double tmp_max_y = last_max_y_;
    last_min_x_ = *min_x;
    last_min_y_ = *min_y;
    last_max_x_ = *max_x;
    last_max_y_ = *max_y;
    *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_;
    *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_;
    *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_;
    *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_;
  }
}

void InflationLayer::onFootprintChanged()
{
  inscribed_radius_ = layered_costmap_->getInscribedRadius();
  cell_inflation_radius_ = cellDistance(inflation_radius_);
  computeCaches();
  need_reinflation_ = true;

  robot::log_info("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
            " inscribed_radius_ = %.3f, inflation_radius_ = %.3f\n",
            layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
}

void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
  if (cell_inflation_radius_ == 0)
    return;

  for (std::vector<CellData>& cells : inflation_cells_)
    cells.clear();

  unsigned char* master_array = master_grid.getCharMap();
  unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();

  const std::size_t map_size = static_cast<std::size_t>(size_x) * size_y;
  if (seen_.size() != map_size)
  {
    seen_.assign(map_size, 0);
    seen_generation_ = 0;
  }

  if (seen_generation_ == std::numeric_limits<std::uint32_t>::max())
  {
    std::fill(seen_.begin(), seen_.end(), 0);
    seen_generation_ = 1;
  }
  else
  {
    ++seen_generation_;
  }

  // We need to include in the inflation cells outside the bounding
  // box min_i...max_j, by the amount cell_inflation_radius_.  Cells
  // up to that distance outside the box can still influence the costs
  // stored in cells inside the box.
  min_i -= cell_inflation_radius_;
  min_j -= cell_inflation_radius_;
  max_i += cell_inflation_radius_;
  max_j += cell_inflation_radius_;

  min_i = std::max(0, min_i);
  min_j = std::max(0, min_j);
  max_i = std::min(int(size_x), max_i);
  max_j = std::min(int(size_y), max_j);

  // Precomputed distance buckets preserve priority ordering without a tree lookup
  // for every enqueued cell.

  // Start with lethal obstacles: by definition distance is 0.0
  if (inflation_cells_.empty())
    return;
  std::vector<CellData>& obs_bin = inflation_cells_.front();
  for (int j = min_j; j < max_j; j++)
  {
    for (int i = min_i; i < max_i; i++)
    {
      int index = master_grid.getIndex(i, j);
      unsigned char cost = master_array[index];
      if (cost == LETHAL_OBSTACLE)
      {
        obs_bin.push_back(CellData(index, i, j, i, j));
      }
    }
  }

  // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
  // can overtake previously inserted but farther away cells
  for (std::vector<CellData>& bin : inflation_cells_)
  {
    for (std::size_t i = 0; i < bin.size(); ++i)
    {
      // process all cells at distance dist_bin.first
      const CellData& cell = bin[i];

      unsigned int index = cell.index_;

      // ignore if already visited
      if (seen_[index] == seen_generation_)
      {
        continue;
      }

      seen_[index] = seen_generation_;

      unsigned int mx = cell.x_;
      unsigned int my = cell.y_;
      unsigned int sx = cell.src_x_;
      unsigned int sy = cell.src_y_;

      // assign the cost associated with the distance from an obstacle to the cell
      unsigned char cost = costLookup(mx, my, sx, sy);
      unsigned char old_cost = master_array[index];
      if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
        master_array[index] = cost;
      else
        master_array[index] = std::max(old_cost, cost);

      // attempt to put the neighbors of the current cell onto the inflation list
      if (mx > 0)
        enqueue(index - 1, mx - 1, my, sx, sy);
      if (my > 0)
        enqueue(index - size_x, mx, my - 1, sx, sy);
      if (mx < size_x - 1)
        enqueue(index + 1, mx + 1, my, sx, sy);
      if (my < size_y - 1)
        enqueue(index + size_x, mx, my + 1, sx, sy);
    }
  }

}

/**
 * @brief  Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
 * @param  grid The costmap
 * @param  index The index of the cell
 * @param  mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
 * @param  my The y coordinate of the cell (can be computed from the index, but saves time to store it)
 * @param  src_x The x index of the obstacle point inflation started at
 * @param  src_y The y index of the obstacle point inflation started at
 */
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
                                    unsigned int src_x, unsigned int src_y)
{
  if (seen_[index] != seen_generation_)
  {
    // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
    double distance = distanceLookup(mx, my, src_x, src_y);

    // we only want to put the cell in the list if it is within the inflation radius of the obstacle point
    if (distance > cell_inflation_radius_)
      return;

    const unsigned int dx = std::abs(static_cast<int>(mx) - static_cast<int>(src_x));
    const unsigned int dy = std::abs(static_cast<int>(my) - static_cast<int>(src_y));
    const unsigned int bin_index = distance_bin_lookup_[dx * distance_lookup_size_ + dy];
    inflation_cells_[bin_index].push_back(CellData(index, mx, my, src_x, src_y));
  }
}

void InflationLayer::computeCaches()
{
  if (cell_inflation_radius_ == 0)
    return;

  // based on the inflation radius... compute distance and cost caches
  if (cell_inflation_radius_ != cached_cell_inflation_radius_)
  {
    deleteKernels();

    cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
    cached_distances_ = new double*[cell_inflation_radius_ + 2];

    for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
    {
      cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
      cached_distances_[i] = new double[cell_inflation_radius_ + 2];
      for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
      {
        cached_distances_[i][j] = hypot(i, j);
      }
    }

    cached_cell_inflation_radius_ = cell_inflation_radius_;

    distance_lookup_size_ = cell_inflation_radius_ + 2;
    distance_levels_.clear();
    for (unsigned int i = 0; i < distance_lookup_size_; ++i)
    {
      for (unsigned int j = 0; j < distance_lookup_size_; ++j)
      {
        if (cached_distances_[i][j] <= cell_inflation_radius_)
          distance_levels_.push_back(cached_distances_[i][j]);
      }
    }
    std::sort(distance_levels_.begin(), distance_levels_.end());
    distance_levels_.erase(
        std::unique(distance_levels_.begin(), distance_levels_.end()), distance_levels_.end());

    inflation_cells_.clear();
    inflation_cells_.resize(distance_levels_.size());
    distance_bin_lookup_.assign(
        static_cast<std::size_t>(distance_lookup_size_) * distance_lookup_size_, 0);
    for (unsigned int i = 0; i < distance_lookup_size_; ++i)
    {
      for (unsigned int j = 0; j < distance_lookup_size_; ++j)
      {
        const double distance = cached_distances_[i][j];
        if (distance > cell_inflation_radius_)
          continue;
        distance_bin_lookup_[i * distance_lookup_size_ + j] =
            static_cast<unsigned int>(
                std::lower_bound(distance_levels_.begin(), distance_levels_.end(), distance) -
                distance_levels_.begin());
      }
    }
  }

  for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
  {
    for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
    {
      cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
    }
  }
}

void InflationLayer::deleteKernels()
{
  inflation_cells_.clear();
  distance_levels_.clear();
  distance_bin_lookup_.clear();
  distance_lookup_size_ = 0;
  if (cached_distances_ != NULL)
  {
    for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
    {
      if (cached_distances_[i])
        delete[] cached_distances_[i];
    }
    if (cached_distances_)
      delete[] cached_distances_;
    cached_distances_ = NULL;
  }

  if (cached_costs_ != NULL)
  {
    for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
    {
      if (cached_costs_[i])
        delete[] cached_costs_[i];
    }
    delete[] cached_costs_;
    cached_costs_ = NULL;
  }
}

void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor)
{
  if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius)
  {
    // Lock here so that reconfiguring the inflation radius doesn't cause segfaults
    // when accessing the cached arrays
    boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);

    inflation_radius_ = inflation_radius;
    cell_inflation_radius_ = cellDistance(inflation_radius_);
    weight_ = cost_scaling_factor;
    need_reinflation_ = true;
    computeCaches();
  }
}

void InflationLayer::handleImpl(const void* data,
                              const std::type_info& info,
                              const std::string& source)
{
  robot::log_error("This function is not available!\n");
}


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

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

}  // namespace robot_costmap_2d