#include "map_builder.h"

#include <glog/logging.h>
#include <memory>

#include "file_system/file_path.h"

MapBuilder::MapBuilder(Config &config, std::shared_ptr<IESKF> kf) : m_config(config), m_kf(kf)
{
    m_imu_processor = std::make_shared<IMUProcessor>(config, kf);
    m_lidar_processor = std::make_shared<LidarProcessor>(config, kf);
    m_status = BuilderStatus::IMU_INIT;
    m_sc_manager = std::make_shared<SCManager>();
}

void MapBuilder::process(SyncPackage &package)
{
    if (m_status == BuilderStatus::IMU_INIT)
    {
        if (m_imu_processor->initialize(package))
            m_status = BuilderStatus::MAP_INIT;
        return;
    }

    m_imu_processor->undistort(package);

    if (m_status == BuilderStatus::MAP_INIT)
    {
        CloudType::Ptr cloud_world = LidarProcessor::transformCloud(package.cloud, m_lidar_processor->r_wl(), m_lidar_processor->t_wl());
        m_lidar_processor->initCloudMap(cloud_world->points);
        m_status = BuilderStatus::MAPPING;
        return;
    }

    m_lidar_processor->process(package);

    Eigen::Matrix4d pose;
    pose.block<3, 3>(0, 0) = m_lidar_processor->r_wl();
    pose.block<3, 1>(0, 3) = m_lidar_processor->t_wl();

    // m_sc_manager->makeAndSaveScancontextAndKeys(*package.cloud, pose, 0);

    // auto [loop_id, loop_yaw] = m_sc_manager->detectLoopClosureID();

    // if (loop_id != -1) {
    //     std::cout << "Loop detected! ID: " << loop_id << " yaw: " << loop_yaw << std::endl;
    // }
}

void MapBuilder::SaveMap(const std::string &map_foler_path) {

    const std::string sc_map_path =
        FileSystem2::JoinPath(map_foler_path, "sc.pbf");

    m_sc_manager->SaveToFile(sc_map_path);

    LOG(ERROR) << "Saved SC map to " << sc_map_path;
}