#include "backend.h"

#include "Frame.h"
#include "Map.h"

namespace Dobot {

void BackEnd::Optimize() {

    LOG(INFO) << "Loop infos size: " << m_cache_pairs.size();

    auto map = map_manager_->GetLayer<Dobot::Map>();

    for (LoopPair &pair : m_cache_pairs) {
        m_graph.add(gtsam::BetweenFactor<gtsam::Pose3>(
            pair.target_id, pair.source_id,
            gtsam::Pose3(gtsam::Rot3(pair.r_offset), gtsam::Point3(pair.t_offset)),
            gtsam::noiseModel::Diagonal::Variances(gtsam::Vector6::Ones() *
                                                pair.score)));
    }

    const std::vector<Frame::Ptr> frames = map->getFramesVec();

    m_r_offset.setIdentity();
    m_t_offset.setZero();
    Frame::Ptr last_frame = nullptr;

    for (const auto& frame : frames) {

        const size_t id = frame->getId();

        Eigen::Matrix3d init_r = m_r_offset * frame->getRotation();
        Eigen::Vector3d init_t = m_r_offset * frame->getTranslatioin() + m_t_offset;

        if (id == 0)
        {

            LOG(INFO) << "add prior";

            // 添加先验约束
            gtsam::noiseModel::Diagonal::shared_ptr noise =
                gtsam::noiseModel::Diagonal::Variances(gtsam::Vector6::Ones() *
                                                       1e-12);
            m_graph.add(gtsam::PriorFactor<gtsam::Pose3>(
                id, gtsam::Pose3(gtsam::Rot3(init_r), gtsam::Point3(init_t)),
                noise));

        } else {
            // 添加里程计约束
            // const KeyPoseWithCloud &last_item = m_key_poses.back();
            Eigen::Matrix3d r_between =
                last_frame->getRotation().transpose() * frame->getRotation();
            Eigen::Vector3d t_between = last_frame->getRotation().transpose() *
                            (frame->getTranslatioin() - last_frame->getTranslatioin());
            gtsam::noiseModel::Diagonal::shared_ptr noise =
                gtsam::noiseModel::Diagonal::Variances(
                    (gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-6)
                        .finished());
            m_graph.add(gtsam::BetweenFactor<gtsam::Pose3>(
                last_frame->getId(), id,
                gtsam::Pose3(gtsam::Rot3(r_between), gtsam::Point3(t_between)),
                noise));
        }

        last_frame = frame;

        m_initial_values.insert(id, gtsam::Pose3(gtsam::Rot3(init_r), gtsam::Point3(init_t)));

    }

    m_isam2->update(m_graph, m_initial_values);
    m_isam2->update();
    if (true)
    {
        m_isam2->update();
        m_isam2->update();
        m_isam2->update();
        m_isam2->update();
    }
    m_graph.resize(0);
    m_initial_values.clear();

    // update key poses
    gtsam::Values estimate_values = m_isam2->calculateBestEstimate();
    // estimate_values.print("");
    gtsam::KeyVector keys = estimate_values.keys();

    for (const gtsam::Key& key : keys) {
        // 从 key 中提取原始 ID（通常 key 就是你的 frame_id）
        size_t frame_id = key;  // 因为 GTSAM 默认用 size_t 作为 Key

        // 获取对应的 Pose3
        gtsam::Pose3 pose = estimate_values.at<gtsam::Pose3>(key);

        map->getFrame(frame_id)->setPose(pose.rotation().matrix(),
                                         pose.translation().matrix());
    }
}


};