/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, 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_clear_costmap_recovery/clear_costmap_recovery.h>
#include <boost/dll/alias.hpp>
#include <boost/pointer_cast.hpp>
#include <vector>

using robot_costmap_2d::NO_INFORMATION;

namespace robot_clear_costmap_recovery
{
  ClearCostmapRecovery::ClearCostmapRecovery() : global_costmap_(NULL), local_costmap_(NULL),
                                                 tf_(NULL), initialized_(false) {}

  void ClearCostmapRecovery::initialize(std::string name, tf3::BufferCore *tf,
                                        robot_costmap_2d::Costmap2DROBOT *global_costmap, robot_costmap_2d::Costmap2DROBOT *local_costmap)
  {
    if (!initialized_)
    {
      name_ = name;
      tf_ = tf;
      global_costmap_ = global_costmap;
      local_costmap_ = local_costmap;

      // get some parameters from the parameter server
      robot::NodeHandle private_nh("~/" + name_);

      private_nh.param("reset_distance", reset_distance_, 3.0);
      private_nh.param("invert_area_to_clear", invert_area_to_clear_, false);
      private_nh.param("force_updating", force_updating_, false);
      private_nh.param("affected_maps", affected_maps_, std::string("both"));
      if (affected_maps_ != "local" && affected_maps_ != "global" && affected_maps_ != "both")
      {
        robot::log_warning("Wrong value for affected_maps parameter: '%s'; valid values are 'local', 'global' or 'both'; "
                 "defaulting to 'both'",
                 affected_maps_.c_str());
        affected_maps_ = "both";
      }

      std::vector<std::string> clearable_layers_default, clearable_layers;
      clearable_layers_default.push_back(std::string("obstacles"));
      private_nh.param("layer_names", clearable_layers, clearable_layers_default);

      for (unsigned i = 0; i < clearable_layers.size(); i++)
      {
        robot::log_info("Recovery behavior will clear layer '%s'", clearable_layers[i].c_str());
        clearable_layers_.insert(clearable_layers[i]);
      }
      initialized_ = true;
    }
    else
    {
      robot::log_error("You should not call initialize twice on this object, doing nothing ");
    }
  }

  void ClearCostmapRecovery::runBehavior()
  {
    robot::log_info("[ClearCostmapRecovery::runBehavior] ===== ENTRY =====");
    robot::log_info("[ClearCostmapRecovery::runBehavior] Name: %s", name_.c_str());
    
    // Check initialization
    if (!initialized_)
    {
      robot::log_error("[ClearCostmapRecovery::runBehavior] ERROR: Not initialized!");
      robot::log_info("[ClearCostmapRecovery::runBehavior] ===== EXIT (not initialized) =====");
      return;
    }
    robot::log_debug("[ClearCostmapRecovery::runBehavior] Initialization check: OK");

    // Check costmaps
    if (global_costmap_ == NULL || local_costmap_ == NULL)
    {
      robot::log_error("[ClearCostmapRecovery::runBehavior] ERROR: Costmaps are NULL!");
      robot::log_error("[ClearCostmapRecovery::runBehavior] global_costmap_: %s", global_costmap_ ? "valid" : "NULL");
      robot::log_error("[ClearCostmapRecovery::runBehavior] local_costmap_: %s", local_costmap_ ? "valid" : "NULL");
      robot::log_info("[ClearCostmapRecovery::runBehavior] ===== EXIT (NULL costmaps) =====");
      return;
    }
    robot::log_debug("[ClearCostmapRecovery::runBehavior] Costmaps check: OK");

    // Log parameters
    robot::log_info("[ClearCostmapRecovery::runBehavior] Parameters:");
    robot::log_info("[ClearCostmapRecovery::runBehavior]   - reset_distance: %.2f m", reset_distance_);
    robot::log_info("[ClearCostmapRecovery::runBehavior]   - invert_area_to_clear: %s", invert_area_to_clear_ ? "true" : "false");
    robot::log_info("[ClearCostmapRecovery::runBehavior]   - force_updating: %s", force_updating_ ? "true" : "false");
    robot::log_info("[ClearCostmapRecovery::runBehavior]   - affected_maps: %s", affected_maps_.c_str());
    robot::log_info("[ClearCostmapRecovery::runBehavior]   - clearable_layers count: %zu", clearable_layers_.size());
    for (const auto& layer : clearable_layers_)
    {
      robot::log_info("[ClearCostmapRecovery::runBehavior]     - layer: '%s'", layer.c_str());
    }

    // Log clearing mode
    if (!invert_area_to_clear_)
    {
      robot::log_warning("[ClearCostmapRecovery::runBehavior] %s is Clearing %s costmap%s outside a square (%.2fm) large centered on the robot.", 
                         name_.c_str(), affected_maps_.c_str(),
                         affected_maps_ == "both" ? "s" : "", reset_distance_);
    }
    else
    {
      robot::log_warning("[ClearCostmapRecovery::runBehavior] %s is Clearing %s costmap%s inside a square (%.2fm) large centered on the robot.", 
                         name_.c_str(), affected_maps_.c_str(),
                         affected_maps_ == "both" ? "s" : "", reset_distance_);
    }

    // Process global costmap
    robot::WallTime t0 = robot::WallTime::now();
    if (affected_maps_ == "global" || affected_maps_ == "both")
    {
      robot::log_info("[ClearCostmapRecovery::runBehavior] Processing GLOBAL costmap...");
      robot::WallTime clear_start = robot::WallTime::now();
      
      clear(global_costmap_);
      
      robot::WallDuration clear_duration = robot::WallTime::now() - clear_start;
      robot::log_info("[ClearCostmapRecovery::runBehavior] Global costmap clear() completed in %.6f s", clear_duration.toSec());

      if (force_updating_)
      {
        robot::log_info("[ClearCostmapRecovery::runBehavior] Force updating global costmap...");
        robot::WallTime update_start = robot::WallTime::now();
        global_costmap_->updateMap();
        robot::WallDuration update_duration = robot::WallTime::now() - update_start;
        robot::log_info("[ClearCostmapRecovery::runBehavior] Global costmap updateMap() completed in %.6f s", update_duration.toSec());
      }

      robot::WallDuration total_duration = robot::WallTime::now() - t0;
      robot::log_info("[ClearCostmapRecovery::runBehavior] Global costmap total processing time: %.6f s", total_duration.toSec());
      robot::log_debug("[ClearCostmapRecovery::runBehavior] Global costmap cleared in %fs", total_duration.toSec());
    }
    else
    {
      robot::log_debug("[ClearCostmapRecovery::runBehavior] Skipping global costmap (affected_maps='%s')", affected_maps_.c_str());
    }

    // Process local costmap
    t0 = robot::WallTime::now();
    if (affected_maps_ == "local" || affected_maps_ == "both")
    {
      robot::log_info("[ClearCostmapRecovery::runBehavior] Processing LOCAL costmap...");
      robot::WallTime clear_start = robot::WallTime::now();
      
      clear(local_costmap_);
      
      robot::WallDuration clear_duration = robot::WallTime::now() - clear_start;
      robot::log_info("[ClearCostmapRecovery::runBehavior] Local costmap clear() completed in %.6f s", clear_duration.toSec());

      if (force_updating_)
      {
        robot::log_info("[ClearCostmapRecovery::runBehavior] Force updating local costmap...");
        robot::WallTime update_start = robot::WallTime::now();
        local_costmap_->updateMap();
        robot::WallDuration update_duration = robot::WallTime::now() - update_start;
        robot::log_info("[ClearCostmapRecovery::runBehavior] Local costmap updateMap() completed in %.6f s", update_duration.toSec());
      }

      robot::WallDuration total_duration = robot::WallTime::now() - t0;
      robot::log_info("[ClearCostmapRecovery::runBehavior] Local costmap total processing time: %.6f s", total_duration.toSec());
      robot::log_debug("[ClearCostmapRecovery::runBehavior] Local costmap cleared in %fs", total_duration.toSec());
    }
    else
    {
      robot::log_debug("[ClearCostmapRecovery::runBehavior] Skipping local costmap (affected_maps='%s')", affected_maps_.c_str());
    }

    robot::log_info("[ClearCostmapRecovery::runBehavior] ===== EXIT (success) =====");
  }

  void ClearCostmapRecovery::clear(robot_costmap_2d::Costmap2DROBOT *costmap)
  {
    robot::log_debug("[ClearCostmapRecovery::clear] ===== ENTRY =====");
    
    std::vector<boost::shared_ptr<robot_costmap_2d::Layer>> *plugins = costmap->getLayeredCostmap()->getPlugins();
    robot::log_debug("[ClearCostmapRecovery::clear] Found %zu plugins", plugins ? plugins->size() : 0);

    robot_geometry_msgs::PoseStamped pose;

    if (!costmap->getRobotPose(pose))
    {
      robot::log_error("[ClearCostmapRecovery::clear] ERROR: Cannot clear map because pose cannot be retrieved");
      robot::log_debug("[ClearCostmapRecovery::clear] ===== EXIT (no pose) =====");
      return;
    }

    double x = pose.pose.position.x;
    double y = pose.pose.position.y;
    robot::log_debug("[ClearCostmapRecovery::clear] Robot pose: x=%.3f, y=%.3f", x, y);

    if (!plugins)
    {
      robot::log_error("[ClearCostmapRecovery::clear] ERROR: plugins pointer is NULL");
      robot::log_debug("[ClearCostmapRecovery::clear] ===== EXIT (NULL plugins) =====");
      return;
    }

    int processed_layers = 0;
    int skipped_layers = 0;
    
    for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp)
    {
      boost::shared_ptr<robot_costmap_2d::Layer> plugin = *pluginp;
      if (!plugin)
      {
        robot::log_warning("[ClearCostmapRecovery::clear] WARNING: Found NULL plugin, skipping");
        skipped_layers++;
        continue;
      }
      
      std::string name = plugin->getName();
      std::string original_name = name;
      int slash = name.rfind('/');
      if (slash != std::string::npos)
      {
        name = name.substr(slash + 1);
      }
      
      robot::log_debug("[ClearCostmapRecovery::clear] Processing layer: '%s' (original: '%s')", name.c_str(), original_name.c_str());

      if (clearable_layers_.count(name) != 0)
      {
        robot::log_info("[ClearCostmapRecovery::clear] Layer '%s' is in clearable_layers, processing...", name.c_str());
        
        // check if the value is convertable
        if (!dynamic_cast<robot_costmap_2d::CostmapLayer *>(plugin.get()))
        {
          robot::log_error("[ClearCostmapRecovery::clear] ERROR: Layer '%s' is not derived from costmap_2d::CostmapLayer", name.c_str());
          skipped_layers++;
          continue;
        }

        boost::shared_ptr<robot_costmap_2d::CostmapLayer> costmap_layer;
        costmap_layer = boost::static_pointer_cast<robot_costmap_2d::CostmapLayer>(plugin);
        
        robot::log_debug("[ClearCostmapRecovery::clear] Calling clearMap for layer '%s' at (%.3f, %.3f)", name.c_str(), x, y);
        clearMap(costmap_layer, x, y);
        processed_layers++;
        robot::log_info("[ClearCostmapRecovery::clear] Successfully cleared layer '%s'", name.c_str());
      }
      else
      {
        robot::log_debug("[ClearCostmapRecovery::clear] Layer '%s' is NOT in clearable_layers, skipping", name.c_str());
        skipped_layers++;
      }
    }
    
    robot::log_info("[ClearCostmapRecovery::clear] Summary: processed=%d, skipped=%d, total=%zu", 
                    processed_layers, skipped_layers, plugins->size());
    robot::log_debug("[ClearCostmapRecovery::clear] ===== EXIT =====");
  }

  void ClearCostmapRecovery::clearMap(boost::shared_ptr<robot_costmap_2d::CostmapLayer> costmap,
                                      double pose_x, double pose_y)
  {
    robot::log_debug("[ClearCostmapRecovery::clearMap] ===== ENTRY =====");
    robot::log_debug("[ClearCostmapRecovery::clearMap] Robot pose: (%.3f, %.3f)", pose_x, pose_y);
    robot::log_debug("[ClearCostmapRecovery::clearMap] Reset distance: %.3f m", reset_distance_);
    robot::log_debug("[ClearCostmapRecovery::clearMap] Invert area: %s", invert_area_to_clear_ ? "true" : "false");
    
    boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
    robot::log_debug("[ClearCostmapRecovery::clearMap] Mutex acquired");

    double start_point_x = pose_x - reset_distance_ / 2;
    double start_point_y = pose_y - reset_distance_ / 2;
    double end_point_x = start_point_x + reset_distance_;
    double end_point_y = start_point_y + reset_distance_;

    robot::log_debug("[ClearCostmapRecovery::clearMap] Clear area (world coords):");
    robot::log_debug("[ClearCostmapRecovery::clearMap]   start: (%.3f, %.3f)", start_point_x, start_point_y);
    robot::log_debug("[ClearCostmapRecovery::clearMap]   end: (%.3f, %.3f)", end_point_x, end_point_y);

    int start_x, start_y, end_x, end_y;
    costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
    costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
    
    robot::log_debug("[ClearCostmapRecovery::clearMap] Clear area (map coords):");
    robot::log_debug("[ClearCostmapRecovery::clearMap]   start: (%d, %d)", start_x, start_y);
    robot::log_debug("[ClearCostmapRecovery::clearMap]   end: (%d, %d)", end_x, end_y);
    
    costmap->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
    robot::log_debug("[ClearCostmapRecovery::clearMap] clearArea() called");

    double ox = costmap->getOriginX(), oy = costmap->getOriginY();
    double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();
    robot::log_debug("[ClearCostmapRecovery::clearMap] Costmap bounds:");
    robot::log_debug("[ClearCostmapRecovery::clearMap]   origin: (%.3f, %.3f)", ox, oy);
    robot::log_debug("[ClearCostmapRecovery::clearMap]   size: %.3f x %.3f m", width, height);
    
    costmap->addExtraBounds(ox, oy, ox + width, oy + height);
    robot::log_debug("[ClearCostmapRecovery::clearMap] addExtraBounds() called");
    robot::log_debug("[ClearCostmapRecovery::clearMap] ===== EXIT =====");
    return;
  }

  std::shared_ptr<ClearCostmapRecovery> ClearCostmapRecovery::create()
  {
    return std::make_shared<ClearCostmapRecovery>();
  }

};

BOOST_DLL_ALIAS(robot_clear_costmap_recovery::ClearCostmapRecovery::create, ClearCostmapRecovery)