/*********************************************************************
 * License & Authors
 *  - Cung cấp lớp Costmap2D để lưu trữ bản đồ chi phí 2D
 *********************************************************************/

#include <costmap_2d/costmap_2d.h>  // Header định nghĩa lớp Costmap2D
#include <cstdio>                   // Dùng cho thao tác file (fopen, fprintf, ...)

using namespace std;

namespace costmap_2d
{

/*****************************************************
 *  Hàm khởi tạo chính của Costmap2D
 *  - Tạo bản đồ kích thước size_x × size_y
 *  - Gán độ phân giải, gốc tọa độ, và giá trị mặc định
 *****************************************************/
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
                     double origin_x, double origin_y, unsigned char default_value) :
    size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
    origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
{
  access_ = new mutex_t();  // Khóa bảo vệ truy cập đa luồng

  // Cấp phát vùng nhớ cho bản đồ
  initMaps(size_x_, size_y_);

  // Gán toàn bộ bản đồ bằng giá trị mặc định (ví dụ: NO_INFORMATION)
  resetMaps();
}

/*****************************************************
 *  Xóa bộ nhớ costmap_
 *****************************************************/
void Costmap2D::deleteMaps()
{
  boost::unique_lock<mutex_t> lock(*access_);  // Giữ mutex để đảm bảo thread-safe
  delete[] costmap_;                           // Giải phóng vùng nhớ
  costmap_ = NULL;
}

/*****************************************************
 *  Cấp phát vùng nhớ cho costmap_
 *****************************************************/
void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
{
  boost::unique_lock<mutex_t> lock(*access_);
  delete[] costmap_;                      // Xóa vùng nhớ cũ nếu có
  costmap_ = new unsigned char[size_x * size_y];  // Mỗi ô lưu 1 byte (0–255)
}

/*****************************************************
 *  Thay đổi kích thước bản đồ
 *  - Thường dùng khi tạo lại bản đồ mới hoặc khi map động
 *****************************************************/
void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
                          double origin_x, double origin_y)
{
  size_x_ = size_x;
  size_y_ = size_y;
  resolution_ = resolution;
  origin_x_ = origin_x;
  origin_y_ = origin_y;

  initMaps(size_x, size_y);  // Cấp phát vùng nhớ mới
  resetMaps();               // Reset toàn bộ dữ liệu
}

/*****************************************************
 *  Reset toàn bộ bản đồ về giá trị mặc định
 *****************************************************/
void Costmap2D::resetMaps()
{
  boost::unique_lock<mutex_t> lock(*access_);
  memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
}

/*****************************************************
 *  Reset một vùng con trong costmap
 *****************************************************/
void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
{
  boost::unique_lock<mutex_t> lock(*(access_));
  unsigned int len = xn - x0;
  for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
    memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
}

/*****************************************************
 *  Tạo bản đồ mới chỉ bao gồm 1 cửa sổ (window) con của map hiện tại
 *****************************************************/
bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y,
                                  double win_size_x, double win_size_y)
{
  // Không thể tạo window của chính mình
  if (this == &map)
    return false;

  deleteMaps();  // Xóa dữ liệu cũ

  // Chuyển đổi tọa độ thế giới sang tọa độ lưới
  unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
  if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y)
      || !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y))
    return false;

  // Tính kích thước vùng con
  size_x_ = upper_right_x - lower_left_x;
  size_y_ = upper_right_y - lower_left_y;
  resolution_ = map.resolution_;
  origin_x_ = win_origin_x;
  origin_y_ = win_origin_y;

  initMaps(size_x_, size_y_);  // Cấp phát bộ nhớ mới

  // Sao chép dữ liệu vùng con từ bản đồ gốc
  copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_,
                costmap_, 0, 0, size_x_, size_x_, size_y_);
  return true;
}

/*****************************************************
 *  Toán tử gán (=): sao chép toàn bộ dữ liệu từ map khác
 *****************************************************/
Costmap2D& Costmap2D::operator=(const Costmap2D& map)
{
  if (this == &map)
    return *this;

  deleteMaps();  // Xóa map cũ

  size_x_ = map.size_x_;
  size_y_ = map.size_y_;
  resolution_ = map.resolution_;
  origin_x_ = map.origin_x_;
  origin_y_ = map.origin_y_;

  initMaps(size_x_, size_y_);  // Tạo map mới
  memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char)); // Sao chép dữ liệu

  return *this;
}

/*****************************************************
 *  Constructor sao chép
 *****************************************************/
Costmap2D::Costmap2D(const Costmap2D& map) : costmap_(NULL)
{
  access_ = new mutex_t();
  *this = map;  // Gọi lại toán tử gán
}

/*****************************************************
 *  Constructor mặc định (chưa có bản đồ)
 *****************************************************/
Costmap2D::Costmap2D() :
    size_x_(0), size_y_(0), resolution_(0.0),
    origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
{
  access_ = new mutex_t();
}

/*****************************************************
 *  Destructor: giải phóng bộ nhớ
 *****************************************************/
Costmap2D::~Costmap2D()
{
  deleteMaps();
  delete access_;
}

/*****************************************************
 *  Chuyển khoảng cách thực (mét) sang đơn vị ô
 *****************************************************/
unsigned int Costmap2D::cellDistance(double world_dist)
{
  double cells_dist = max(0.0, ceil(world_dist / resolution_));
  return (unsigned int)cells_dist;
}

/*****************************************************
 *  Trả về con trỏ dữ liệu costmap
 *****************************************************/
unsigned char* Costmap2D::getCharMap() const
{
  return costmap_;
}

/*****************************************************
 *  Lấy giá trị cost tại ô (mx, my)
 *****************************************************/
unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
{
  return costmap_[getIndex(mx, my)];
}

/*****************************************************
 *  Gán giá trị cost tại ô (mx, my)
 *****************************************************/
void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
{
  costmap_[getIndex(mx, my)] = cost;
}

/*****************************************************
 *  Chuyển từ tọa độ lưới (ô) sang tọa độ thế giới
 *****************************************************/
void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
{
  wx = origin_x_ + (mx + 0.5) * resolution_;
  wy = origin_y_ + (my + 0.5) * resolution_;
}

/*****************************************************
 *  Chuyển từ tọa độ thế giới sang tọa độ lưới
 *****************************************************/
bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
{
  if (wx < origin_x_ || wy < origin_y_)
    return false;

  mx = (int)((wx - origin_x_) / resolution_);
  my = (int)((wy - origin_y_) / resolution_);

  return (mx < size_x_ && my < size_y_);
}

/*****************************************************
 *  Phiên bản không kiểm tra biên (có thể ra ngoài map)
 *****************************************************/
void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
{
  mx = (int)((wx - origin_x_) / resolution_);
  my = (int)((wy - origin_y_) / resolution_);
}

/*****************************************************
 *  Phiên bản ép buộc tọa độ nằm trong biên bản đồ
 *****************************************************/
void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const
{
  if (wx < origin_x_)
    mx = 0;
  else if (wx >= resolution_ * size_x_ + origin_x_)
    mx = size_x_ - 1;
  else
    mx = (int)((wx - origin_x_) / resolution_);

  if (wy < origin_y_)
    my = 0;
  else if (wy >= resolution_ * size_y_ + origin_y_)
    my = size_y_ - 1;
  else
    my = (int)((wy - origin_y_) / resolution_);
}

/*****************************************************
 *  Dịch chuyển gốc của bản đồ khi robot di chuyển
 *  => Dữ liệu được dịch và giữ lại phần chồng lấn
 *****************************************************/
void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
{
  int cell_ox = int((new_origin_x - origin_x_) / resolution_);
  int cell_oy = int((new_origin_y - origin_y_) / resolution_);

  if (cell_ox == 0 && cell_oy == 0)
    return;

  // Cập nhật lại gốc bản đồ mới (theo bội số của resolution)
  double new_grid_ox = origin_x_ + cell_ox * resolution_;
  double new_grid_oy = origin_y_ + cell_oy * resolution_;

  int size_x = size_x_;
  int size_y = size_y_;

  // Tính vùng chồng lấn giữa bản đồ cũ và bản đồ mới
  int lower_left_x = min(max(cell_ox, 0), size_x);
  int lower_left_y = min(max(cell_oy, 0), size_y);
  int upper_right_x = min(max(cell_ox + size_x, 0), size_x);
  int upper_right_y = min(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;

  // Tạo bản đồ tạm để lưu phần chồng lấn
  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_,
                local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);

  resetMaps(); // Toàn bộ bản đồ được đặt lại giá trị mặc định

  origin_x_ = new_grid_ox;
  origin_y_ = new_grid_oy;

  // Copy vùng chồng lấn vào vị trí mới
  int start_x = lower_left_x - cell_ox;
  int start_y = lower_left_y - cell_oy;
  copyMapRegion(local_map, 0, 0, cell_size_x,
                costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);

  delete[] local_map; // Giải phóng vùng nhớ tạm
}

/*****************************************************
 *  Đặt giá trị cost cho vùng đa giác lồi
 *  (ví dụ: để đánh dấu vùng cấm hoặc vùng obstacle)
 *****************************************************/
bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
{
  std::vector<MapLocation> map_polygon;
  for (unsigned int i = 0; i < polygon.size(); ++i)
  {
    MapLocation loc;
    if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
      return false; // Nếu một điểm nằm ngoài map
    map_polygon.push_back(loc);
  }

  std::vector<MapLocation> polygon_cells;
  convexFillCells(map_polygon, polygon_cells); // Lấy toàn bộ ô bên trong đa giác

  for (unsigned int i = 0; i < polygon_cells.size(); ++i)
    costmap_[getIndex(polygon_cells[i].x, polygon_cells[i].y)] = cost_value;

  return true;
}

/*****************************************************
 *  polygonOutlineCells():
 *  - Tìm các ô nằm trên biên của đa giác
 *****************************************************/
void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon,
                                    std::vector<MapLocation>& polygon_cells)
{
  PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);

  for (unsigned int i = 0; i < polygon.size() - 1; ++i)
    raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);

  if (!polygon.empty())
  {
    unsigned int last_index = polygon.size() - 1;
    // Nối điểm cuối với điểm đầu để khép kín đa giác
    raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y,
                 polygon[0].x, polygon[0].y);
  }
}

/*****************************************************
 *  convexFillCells():
 *  - Điền đầy các ô bên trong một đa giác lồi
 *****************************************************/
void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon,
                                std::vector<MapLocation>& polygon_cells)
{
  if (polygon.size() < 3)  // Ít hơn 3 điểm thì không tạo thành đa giác
    return;

  polygonOutlineCells(polygon, polygon_cells);  // Lấy biên ngoài

  // Sắp xếp theo trục X để dễ quét cột
  MapLocation swap;
  unsigned int i = 0;
  while (i < polygon_cells.size() - 1)
  {
    if (polygon_cells[i].x > polygon_cells[i + 1].x)
    {
      swap = polygon_cells[i];
      polygon_cells[i] = polygon_cells[i + 1];
      polygon_cells[i + 1] = swap;
      if (i > 0) --i;
    }
    else ++i;
  }

  // Quét từng cột X để điền đầy Y
  i = 0;
  MapLocation min_pt, max_pt;
  unsigned int min_x = polygon_cells[0].x;
  unsigned int max_x = polygon_cells.back().x;

  for (unsigned int x = min_x; x <= max_x; ++x)
  {
    if (i >= polygon_cells.size() - 1) break;

    if (polygon_cells[i].y < polygon_cells[i + 1].y)
    {
      min_pt = polygon_cells[i];
      max_pt = polygon_cells[i + 1];
    }
    else
    {
      min_pt = polygon_cells[i + 1];
      max_pt = polygon_cells[i];
    }

    i += 2;
    while (i < polygon_cells.size() && polygon_cells[i].x == x)
    {
      if (polygon_cells[i].y < min_pt.y)
        min_pt = polygon_cells[i];
      else if (polygon_cells[i].y > max_pt.y)
        max_pt = polygon_cells[i];
      ++i;
    }

    // Điền đầy ô từ min_y đến max_y
    for (unsigned int y = min_pt.y; y <= max_pt.y; ++y)
      polygon_cells.push_back({x, y});
  }
}

/*****************************************************
 *  Các hàm getter cơ bản
 *****************************************************/
unsigned int Costmap2D::getSizeInCellsX() const { return size_x_; }
unsigned int Costmap2D::getSizeInCellsY() const { return size_y_; }
double Costmap2D::getSizeInMetersX() const { return (size_x_ - 1 + 0.5) * resolution_; }
double Costmap2D::getSizeInMetersY() const { return (size_y_ - 1 + 0.5) * resolution_; }
double Costmap2D::getOriginX() const { return origin_x_; }
double Costmap2D::getOriginY() const { return origin_y_; }
double Costmap2D::getResolution() const { return resolution_; }

/*****************************************************
 *  Lưu costmap ra file PGM (grayscale) để debug
 *****************************************************/
bool Costmap2D::saveMap(std::string file_name)
{
  FILE *fp = fopen(file_name.c_str(), "w");
  if (!fp)
    return false;

  fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
  for (unsigned int iy = 0; iy < size_y_; iy++)
  {
    for (unsigned int ix = 0; ix < size_x_; ix++)
      fprintf(fp, "%d ", getCost(ix, iy));
    fprintf(fp, "\n");
  }
  fclose(fp);
  return true;
}

}  // namespace costmap_2d
