#pragma once

#include "map/map_manager.h"

#include <Eigen/Dense>
#include <memory>

#include "map_builder/commons.h"
#include "icp_localizer.h"

namespace Dobot {

class Relocalizer {

public:
    struct RelocResultInfo {
        Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
        float quality = 0.0;
        bool valid = false;
    };

public:
    Relocalizer(std::shared_ptr<Map2::MapManager> map_manager);

    bool Reloc(const CloudType::Ptr& pointcloud);

    RelocResultInfo GetRelocResult();

private:
    std::shared_ptr<Map2::MapManager> map_manager_;

    std::shared_ptr<ICPLocalizer> icp_localizer_;

    RelocResultInfo reloc_result_info_;
};

}
