// pointcloud_map_layer.h
#pragma once
#include "map/map_layer.h"

#include <glog/logging.h>
#include <memory>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <file_system/file_path.h>

namespace Map2 {

using PointType = pcl::PointXYZINormal;
using CloudType = pcl::PointCloud<PointType>;

class PointCloudMap : public MapLayer {
public:
  PointCloudMap() { setName("point_cloud.pcd"); }

  using Ptr = std::shared_ptr<PointCloudMap>;

  bool saveToFile(const std::string &filename) const override {
    // 保存点云到PCD文件
    if (!point_cloud_)
      return false;
    return pcl::io::savePCDFileBinary(filename, *point_cloud_) == 0;
  }

  bool loadFromFile(const std::string &filename) override {
    // 从PCD文件加载点云
    point_cloud_.reset(new CloudType);
    pcl::io::loadPCDFile<PointType>(filename, *point_cloud_);
    LOG(INFO) << "Loaded point cloud from: " << filename << " with "
              << point_cloud_->points.size() << " points.";
    return point_cloud_->points.size() > 0;
  }

  std::string getTypeName() const override { return "PointCloud_Map"; }

  // 访问点云
  CloudType::Ptr getCloud() { return point_cloud_; }
  void setCloud(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr &cloud) {
    point_cloud_ = cloud;
  }

private:
  pcl::PointCloud<pcl::PointXYZINormal>::Ptr point_cloud_{
      new pcl::PointCloud<pcl::PointXYZINormal>()};
  std::string default_map_name_ = "point_cloud.pcd";
};

} // namespace Map2