#ifndef PARAMETER_H
#define PARAMETER_H

#include "../localize_utils/utils.h"
namespace juefx{

class Parameter
{
public:
    using Ptr = boost::shared_ptr<Parameter>;
    Parameter();

    void Reset();

    static Parameter::Ptr Instance();

    void init(const string &baseDir);

    string GetBaseDir(){
        return baseDir_;
    }

    string GetMapPath(){
        return mapPath_;
    }

    Isometry3d GetImu2Base(){
        return imu2base_;
    }

    Isometry3d GetCalib(){
        return calib_;
    }

    Isometry3d GetInitPose(){
        return initPose_;
    }

    Vector3d GetArm(){
        return arm_;
    }

    void SetEskfInitPose(const Isometry3f& eskf_init_pose){
        eskf_init_pose_ = eskf_init_pose;
        calibedImu_ = true;
        LOG(INFO) << "imu calibed: "
                  << RotationQuaternionToEulerVector(
                         Quaternionf(eskf_init_pose.linear())).transpose();
    }

    Isometry3f GetEskfInitPose(){
        return eskf_init_pose_;
    }

    void SetGravity(const Vector3f& gravity){
        gravity_ = gravity;
        LOG(INFO) << "gravity calibed: " << gravity_.transpose();
    }

    Vector3f GetGravity(){
        return gravity_;
    }

    void SetImuTopic(const string &topic_imu){
        topic_imu_ = topic_imu;
    }

    string GetImuTopic() {return topic_imu_;}

    void SetGnssTopic(const string &topic_gnss){
        topic_gnss_ = topic_gnss;
    }

    string GetGnssTopic() {return topic_gnss_;}

    void SetWheelTopic(const string &topic_wheel){
        topic_wheel_ = topic_wheel;
    }

    string GetWheelTopic() {return topic_wheel_;}

    void SetCloudTopic(const string &topic_cloud){
        topic_cloud_ = topic_cloud;
    }

    string GetCloudTopic() {return topic_cloud_;}

    void SetOutputCloudTopic(const string &topic_output_clouds){
        topic_output_clouds_ = topic_output_clouds;
    }

    string GetOutputCloudTopic() {return topic_output_clouds_;}

    void SetOutputOdomTopic(const string &topic_output_odom){
        topic_output_odom_ = topic_output_odom;
    }

    string GetOutputOdomTopic() {return topic_output_odom_;}

    void SetModelTopic(const string &topic_model){
        topic_model_ = topic_model;
    }

    string GetModelTopic() {return topic_model_;}

    Vector3d GetSlamStart() {return slamStart_;}

    Vector2d GetLocalToSlamstart() {return localToSlamstart_;}

    bool GetIsCalibedImu(){
        return calibedImu_;
    }

    void SetFilter2Map(const Isometry3d &filter2Map){
        filter2Map_ = filter2Map;
    }

    Isometry3d GetFilter2Map(){
        return filter2Map_;
    }

    Vector3d GetCenter(){
        return center_;
    }

private:
    string baseDir_;

    string mapPath_;

    Isometry3d imu2base_ = Isometry3d::Identity();

    Isometry3d calib_ = Isometry3d::Identity();

    Isometry3d initPose_ = Isometry3d::Identity();

    string topic_imu_ = "";

    string topic_gnss_ = "";

    string topic_wheel_ = "";

    string topic_cloud_ = "";

    string topic_output_clouds_ = "";

    string topic_output_odom_ = "";

    string topic_model_ = "";

    Vector3d slamStart_ = Vector3d::Zero();

    Vector2d offset_ = Vector2d::Zero();

    Vector2d localToSlamstart_ = Vector2d::Zero();

    Vector3d arm_ = Vector3d::Zero();

    Isometry3f eskf_init_pose_ = Isometry3f::Identity();

    Vector3f gravity_ = Vector3f(0, 0, -9.8015);

    bool calibedImu_ = false;

    Isometry3d filter2Map_ = Isometry3d::Identity();

    Vector3d center_;

};

} // end of namespace
#endif // PARAMETER_H