#include <thread>
#include <mutex>
#include <deque>

#include "map/map.h"
#include "odom/odom.h"
#include "ceres_matcher/ceres_scan_matcher_2d.h"
#include "real_match_2d/real_match_2d.h"

namespace hlzn_slam {

namespace slam {

using namespace ceres_matching;

typedef struct __con con;

struct SubMap {
    SubMap(float size, float resolution) : next(nullptr)
    {
        map = std::shared_ptr<Map>(new Map(size, size, resolution));
    }
    ~SubMap()
    {
        map = nullptr;
        next = nullptr;
    }
    std::shared_ptr<Map> map;
    common::PointCloud point_cloud;

    common::Rigid2f track_pose;
    common::Rigid2f global_pose;

    std::shared_ptr<con> next;
};

struct __con {
    __con() : submap(nullptr)
    {}
    ~__con()
    {
        submap = nullptr;
    }
    std::shared_ptr<SubMap> submap;

    common::Rigid2f transform;
};

class Slam {
    public:
        Slam(std::unique_ptr<Map> global_map);
        ~Slam();
        /**
         * 说明：初始化slam模块，定义初始位置
         * @param [in] init:初始位置
         * @param [in] is_fast: 是否快速定位; true: 是  false: 否 
         */
        void init(const common::Rigid2f& init, bool is_fast, float first_search_range = 5.0, float first_search_angle = M_PI);
        
        /**
         * 说明：设置定位参数
         * @param [in] search_range: 线性搜索范围
         * @param [in] search_angle: 角度搜索范围
         * @param [in] use_towermatch: false, true
         * @param [in] probability_plus: 概率步长
         * @param [in] probability_up: 概率上限
         * @param [in] window_size: 滑动窗口数量
         * @param [in] achieve_range: 窗口分离距离值
         * @param [in] achieve_angle: 窗口分离角度值
         */
        void setParam(float search_range = 0.3, float search_angle = 0.1, bool use_towermatch = true, float probability_plus = 0.2, float probability_up = 0.8,
                        int window_size = 20, float achieve_range = 0.5, float achieve_angle = 0.4, float submap_size = 15.0, float submap_resolution = 0.05, 
                        bool use_odom = true);
        void addPointCloud(common::PointCloud point_cloud, common::laserScan scan, Eigen::Matrix<float, 4, 4> tf);
        void addOriginScan(common::laserScan& scan, Eigen::Matrix<float, 4, 4>& tf);
        void addOdom2DVelocity(float& v, float& w);
        void addOdom2D(float& x, float& y, float& theta);

        bool isInitialize();

        common::TimeRigid2f getCurrentPose();
    private:
        common::Rigid2f __Match(common::Rigid2f& pose, 
                                common::PointCloud& point_cloud, Map& map, 
                                const float& range, 
                                const float& rad, 
                                const float occupied_space_weight = 1.0, 
                                const float translation_weight = 2.0,
                                const float rotation_weight = 3.0,
                                const float self_stab_weight = 1.0);
        common::PointCloud __transformPointCloud(
            const common::PointCloud& point_cloud, const common::Rigid2f& transform);
        void __poseGraph(common::Rigid2f& global_pose, common::Rigid2f& track_pose);
        void __poseGraph2(common::Rigid2f& global_pose);
        void __createNewSubmap(common::PointCloud& point_cloud, common::Rigid2f& global_pose, common::Rigid2f& track_pose);
        void __slam(common::PointCloud point_cloud);
        void __Rigid2f2Double7(common::Rigid2f& rigid2f, double* d7);
        void __double7ToRigid2f(double* d7, common::Rigid2f& rigid2f);
        void __writeTimePose(common::TimeRigid2f time_pose);
        void __buildSubmap(common::Rigid2f pose);
        void __growGlobalmap(common::Rigid2f pose);
        
        void __Rigid2fTranslation2Double3(common::Rigid2f& rigid2f, double* d3);
        void __Rigid2fQuaterniond2Double4(common::Rigid2f& rigid2f, double* d4);
        void __double34ToRigid2f(double* d3, double* d4, common::Rigid2f& rigid2f);
        void __optimization(common::PointCloud& point_cloud, common::Rigid2f& track_pose, common::Rigid2f& global_pose);

        void __globalMapBeamMode(common::Rigid2f& global_tf);

        bool __frameParallaxAchieve(common::Rigid2f& pose, float achieve_range, float achieve_angle);

        common::PointCloud __transformScanData2PointCloudError(float error, const float angle_increment = 0.0);

        common::TimeRigid2f time_pose_;
        common::TimeRigid2f odom_pose_;

        std::deque<std::shared_ptr<SubMap>> submaps_;

        std::unique_ptr<Map> global_map_;

        std::unique_ptr<odom::odom> odom_;

        bool is_initialize_;

        std::thread matchThread_;

        common::laserScan scan_; 
        Eigen::Matrix<float, 4, 4> laser_tf_;

        std::mutex time_pose_lock_;
        #define TIME_POSE_LOCK() std::lock_guard<std::mutex> lockGuard(time_pose_lock_);
    
    private:
        bool use_towermatch_;
        bool use_odom_;

        float probability_plus_;
        float probability_up_;
        float search_range_;
        float search_angle_;
        float window_size_;
        float achieve_range_;
        float achieve_angle_;
        float submap_size_;
        float submap_resolution_;
        
        /**/
        float first_search_range_;
        float first_search_angle_;

        /**/
        float v_, w_;
};

}

}