#pragma once

#include "backend.h"

#include "Map.h"
#include "map/map_manager.h"
#include <Eigen/src/Core/Matrix.h>
#include <glog/logging.h>
#include <memory>
#include <vector>


#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

namespace Dobot {


class BackEnd {

public:

    BackEnd(std::shared_ptr<Map2::MapManager> map_manager) {
        CHECK_NOTNULL(map_manager);
        map_manager_ = map_manager;

        gtsam::ISAM2Params isam2_params;
        isam2_params.relinearizeThreshold = 0.01;
        isam2_params.relinearizeSkip = 1;
        m_isam2 = std::make_shared<gtsam::ISAM2>(isam2_params);
    }

    void Optimize();

    void AddLoopInfo(const LoopPair& loop_info) {
        m_cache_pairs.emplace_back(loop_info);
    }

    std::vector<LoopPair> GetLoopInfo() {
        return m_cache_pairs;
    }


private:

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

    std::vector<LoopPair> m_cache_pairs;

    Eigen::Matrix3d m_r_offset;
    Eigen::Vector3d m_t_offset;
    std::shared_ptr<gtsam::ISAM2> m_isam2;
    gtsam::Values m_initial_values;
    gtsam::NonlinearFactorGraph m_graph;



};

}