#ifndef _REAL_MATCH_2D_H_
#define _REAL_MATCH_2D_H_

#include "common/common.h"
#include "map/map.h"

namespace hlzn_slam {

class RealTimeMatch2D {
    public:
        RealTimeMatch2D();
        ~RealTimeMatch2D(){}

        /**
         * @说明: 设置匹配搜索的范围
         * @param 
         *  linear_range [in] : 线性搜索范围
         *  angle_range [in] : 角度搜索范围 
         *  linear_step　[in] : 线性搜索步长
         *  angle_step [in] : 角度搜索步长
         */
        void setSearchRange(const float linear_range, const float angle_range, const float linear_step = 0.05, const float angle_step = 0.017);

        float match(const common::PointCloud& point_cloud, 
                   const common::Rigid2f& init_pose, 
                   common::Rigid2f& opt_pose,
                   Map& map,
                   bool use_smart = false);
        float towerMatch(const common::PointCloud& point_cloud, const common::Rigid2f& init_pose, 
                        common::Rigid2f& opt_pose, Map& map, const float multiple = 2.0, bool use_smart = false);
    private:
        float __match(const common::PointCloud& point_cloud, const common::Rigid2f& init_pose, 
                     common::Rigid2f& opt_pose, const std::vector<common::Rigid2f>& transforms, 
                     Map& map);
        float __smartMatch(const common::PointCloud& point_cloud, 
                                    const common::Rigid2f& init_pose, 
                                    common::Rigid2f& opt_pose, 
                                    const std::vector<common::Rigid2f>& transforms, Map& map);
        common::PointCloud __transformPointCloud(
            const common::PointCloud& point_cloud, const common::Rigid2f& transform);
        std::vector<common::Rigid2f> __generateTransfroms(const float linear_range, 
                                                          const float angle_range,
                                                          const float linear_step,
                                                          const float angle_step);
        float linear_range_,
              angle_range_,
              linear_step_,
              angle_step_;
        std::vector<common::Rigid2f> transforms_;
};

}

#endif