#include "relocalizer.h"

#include "glog/logging.h"
#include "icp_localizer.h"
#include "pointcloud_map.h"
#include "sc_map.h"
#include "sophus/so3.hpp"
#include <memory>

namespace Dobot {

Relocalizer::Relocalizer(std::shared_ptr<Map2::MapManager> map_manager) {

    CHECK_NOTNULL(map_manager);
    map_manager_ = map_manager;

    ICPConfig icp_config;

    icp_localizer_ = std::make_shared<ICPLocalizer>(icp_config);

    auto pointcloud_map = map_manager->GetLayer<Map2::PointCloudMap>();

    icp_localizer_->setMap(pointcloud_map->getCloud());
}

bool Relocalizer::Reloc(const CloudType::Ptr &pointcloud) {

    auto sc_map = map_manager_->GetLayer<Map2::SCMap>();

    SCRelocInfo sc_reloc_info = sc_map->QueryMap(*pointcloud);

    if (sc_reloc_info.frame_id == -1) {
        reloc_result_info_.valid = false;
        reloc_result_info_.quality = 0.;
        return false;
    }

    Eigen::Matrix4d pose_reloc_raw = sc_reloc_info.pose;

    LOG(INFO) << "Reloc pose: " << pose_reloc_raw;

    double yaw_diff_rad = -sc_reloc_info.yaw_diff_rad;

    Eigen::AngleAxisd yaw_rot(yaw_diff_rad, Eigen::Vector3d::UnitZ());
    Eigen::Matrix3d delta_R = yaw_rot.toRotationMatrix();

    Eigen::Matrix3d R_orig = pose_reloc_raw.block<3,3>(0,0);
    Eigen::Vector3d t_orig = pose_reloc_raw.block<3,1>(0,3);

    Eigen::Matrix3d R_new = R_orig * delta_R;

    Eigen::Matrix4d pose_new = Eigen::Matrix4d::Identity();
    pose_new.block<3,3>(0,0) = R_new;
    pose_new.block<3,1>(0,3) = t_orig;  // 平移不变

    LOG(INFO) << "map align pose before: " << pose_new;

    Eigen::Matrix4f pose_new_f = pose_new.cast<float>();

    icp_localizer_->setInput(pointcloud);
    icp_localizer_->align(pose_new_f);

    LOG(INFO) << "map align pose: " << pose_new_f;

    Eigen::Quaternionf q_f = Eigen::Quaternionf(pose_new_f.block<3, 3>(0, 0));
    q_f = q_f.normalized();

    reloc_result_info_.valid = true;
    reloc_result_info_.pose = pose_new_f.cast<double>();
    reloc_result_info_.pose.block<3, 3>(0, 0) =
        q_f.normalized().toRotationMatrix().cast<double>();

    return true;
}

Relocalizer::RelocResultInfo Relocalizer::GetRelocResult() {

    return reloc_result_info_;
}

}