#pragma once
#include <glog/logging.h>
#include <memory>
#include <unordered_map>

#include <pcl/registration/icp.h>

#include "map/map_layer.h"
#include "Frame.h"
#include "sc_map.h"

#include "pointcloud_map.h"

namespace Dobot {


struct LoopPair
{
    size_t source_id;
    size_t target_id;
    Eigen::Matrix3d r_offset;
    Eigen::Vector3d t_offset;
    double score;

    void Print() {
      LOG(INFO) << "source id: " << source_id << " target id: " << target_id
                << " score: " << score << " t offset: " << t_offset.transpose();
    }
};

class Map : public Map2::MapLayer {
public:

    using Ptr = std::shared_ptr<Map>;

    Map() = default;

    // 添加一个Frame指针
    void addFrame(const std::shared_ptr<Frame>& frame) {
        frames_[frame->getId()] = frame;
    }

    // 获取所有Frame指针
    const std::unordered_map<size_t, Frame::Ptr>& getFrames() const {
        return frames_;
    }

    const std::vector<Frame::Ptr> getFramesVec() const;

    // 获取指定id的Frame指针
    std::shared_ptr<Frame> getFrame(int id) {
        auto it = frames_.find(id);
        return it != frames_.end() ? it->second : nullptr;
    }
    std::shared_ptr<const Frame> getFrame(int id) const {
        auto it = frames_.find(id);
        return it != frames_.end() ? it->second : nullptr;
    }

    std::vector<Eigen::Vector3d> GetKFTrajectory3dof();

    void clear() {
        frames_.clear();
    }

    size_t size() const {
        return frames_.size();
    }

    bool hasFrame(int id) const {
        return frames_.count(id) > 0;
    }

    bool removeFrame(int id) {
        return frames_.erase(id) > 0;
    }

    // MapLayer接口实现
    bool saveToFile(const std::string &filename) const override {
      (void)filename;
      return true;
    }
    bool loadFromFile(const std::string &filename) override {
      (void)filename;
      return true;
    }
    std::string getTypeName() const override { return "FrameMap"; }

    Map2::PointCloudMap::Ptr toPointCloudMap();

    Map2::SCMap::Ptr toSCMap();

private:
    std::unordered_map<size_t, Frame::Ptr> frames_;

    std::vector<LoopPair> loop_info_all_;
};

LoopPair GetLoopInfo(const Frame& source_frame, const Frame& target_frame);
LoopPair GetLoopInfo2(const Frame& source_frame, const Frame& target_frame);


}