#include "toid_costmaps/game_elements_layer.hpp"

#include <fstream>

#include "ament_index_cpp/get_package_share_directory.hpp"
#include "nav2_util/node_utils.hpp"

namespace toid
{

void GameElementLayer::onInitialize()
{
  auto node = node_.lock();
  if (!node) {
    return;
  }
}

void GameElementLayer::activate()
{
  auto node = node_.lock();

  nav2_util::declare_parameter_if_not_declared(
    node, name_ + ".extra_elements", rclcpp::ParameterValue(std::vector<std::string>{}));
  node->get_parameter(name_ + ".extra_elements", extra_elements_);

  initGameElements();

  active_elements_ = std::make_unique<ActiveElements>();
  active_elements_->active = std::vector<bool>(game_elements_.size(), true);

  using namespace std::placeholders;
  active_elements_sub_ = node->create_subscription<ActiveElements>(
    "/active_elements", 1, std::bind(&GameElementLayer::active_elements_cb, this, _1));
}

void GameElementLayer::deactivate()
{
  auto node = node_.lock();
  active_elements_sub_.reset();
  active_elements_.reset();
  game_elements_.clear();
  static_elements_.clear();
  extra_elements_.clear();
}

void GameElementLayer::active_elements_cb(ActiveElements::SharedPtr msg)
{
  active_elements_ = msg;
}

void GameElementLayer::updateBounds(
  double, double, double, double * min_x, double * min_y, double * max_x, double * max_y)
{
  touch(1.50, 1.0, min_x, min_y, max_x, max_y);
  touch(-1.50, -1.0, min_x, min_y, max_x, max_y);
}

void GameElementLayer::updateCosts(nav2_costmap_2d::Costmap2D & grid, int, int, int, int)
{
  ulong idx = 0;
  for (const auto & elem : game_elements_) {
    if (active_elements_->active.size() > idx && active_elements_->active.at(idx)) {
      placeElement(grid, elem);
    }
    idx++;
  }

  for (const auto & elem : static_elements_) {
    placeElement(grid, elem);
  }
}

void GameElementLayer::placeElement(nav2_costmap_2d::Costmap2D & grid, const GameElement & element)
{
  bool in_bounds = true;
  double ex, ey;
  element.start(ex, ey);
  uint mx, my;
  in_bounds &= worldToMap(ex + 0.001, ey + 0.001, mx, my);
  element.end(ex, ey);
  uint mmx, mmy;
  in_bounds &= worldToMap(ex + 0.001, ey + 0.001, mmx, mmy);

  if (in_bounds) {
    for (uint j = my; j < mmy; j++) {
      for (uint i = mx; i < mmx; i++) {
        grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE);
      }
    }
  }
}

void GameElementLayer::initGameElements()
{
  std::string file_path = "/elements/elements.json";
  std::string base_path = ament_index_cpp::get_package_share_directory("toid_costmaps");

  RCLCPP_INFO(rclcpp::get_logger("toid_costmap"), "Bro hear me out");

  std::ifstream fs(base_path + file_path);
  if (!fs.is_open()) {
    RCLCPP_ERROR(
      rclcpp::get_logger("toid_costmap"), "Failed to open elements file: %s%s", base_path.c_str(),
      file_path.c_str());
    throw std::runtime_error("File not found");
  }

  boost::json::stream_parser p;
  boost::json::error_code ec;

  char buffer[4096];
  while (fs.read(buffer, sizeof(buffer)) || fs.gcount() > 0) {
    p.write(buffer, fs.gcount(), ec);
    if (ec) {
      RCLCPP_FATAL(rclcpp::get_logger("toid_costmap"), "JsonError: %s", ec.message().c_str());
      throw std::runtime_error("JsonError");
    }
  }
  p.finish(ec);
  if (ec) {
    RCLCPP_FATAL(rclcpp::get_logger("toid_costmap"), "JsonError: %s", ec.message().c_str());
    throw std::runtime_error("JsonError");
  }

  boost::json::value jv = p.release();

  game_elements_ = boost::json::value_to<std::vector<GameElement>>(jv.at("game_elements"));
  for (const auto & t : extra_elements_) {
    std::vector<GameElement> elements = boost::json::value_to<std::vector<GameElement>>(jv.at(t));
    static_elements_.insert(static_elements_.end(), elements.begin(), elements.end());
  }
}

}  // namespace toid

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(toid::GameElementLayer, nav2_costmap_2d::Layer);