#ifndef _MAP_H_
#define _MAP_H_

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <memory>
#include <map>
#include <string>

#include "common/common.h"

namespace hlzn_slam {
    
class Map 
{
    public:
        /**
         * @说明：初始化一张空白地图
         * @param [in] length: 地图长度(列)
         * @param [in] width: 地图宽度(行)
         * @param [in] resolution: 地图分辨率
         */
        Map(float length, float width, float resolution);

        /**
         * @说明：使用eigen矩阵初始化地图,eigen矩阵矩阵的最大值为1
         * @param [in] map: eigen矩阵
         * @param [in] resolution: 地图分辨率
         */
        Map(Eigen::MatrixXf& map, float resolution, float x_origin, float y_origin);
        ~Map(){};
        
        /**
         * @说明：添加点云数据到地图中，点云对应的像素值逐渐增加
         * @param [in] pinot_cloud: 点云数据, [in] probablity 单次增加概率
         * @param [in] upper: 像素值的上限
         */
        void pixelValuePlus(const std::vector<Eigen::Vector3f>& pinot_cloud, const float& probablity, const float& upper);
        void pixelValuePlus2(const std::vector<Eigen::Vector3f>& pinot_cloud, const float& probablity, const float& upper);
        void pixelValuePlus3(const std::vector<Eigen::Vector3f>& pinot_cloud, const float& probablity, const float& upper);
        void pixelValuePlusShadow(const std::vector<Eigen::Vector3f>& pinot_cloud, const float& probablity, const float& upper);
        void pixelValuePlusShadow2(const std::vector<Eigen::Vector3f>& pinot_cloud, const float& probablity, const float& upper);
        void out(std::string path);

        /**
         * @说明：像素值衰减，构造概率梯度，防止地图僵化
         * @param [in] attenuate_value: 单次衰减量
         */
        void mapAttenuate(const float& attenuate_value);
        void mapShadowAttenuate(const float& attenuate_value);
        
        /**
         * @说明：清除地图上添加的点云数据
         */  
        void clearMap();
        void clearShadowMap();

        /**
         * @说明：配置是否使用影子地图
         * @param [in] ok: 是否使用
         */
        void useShadow(bool ok);

        void word2Map(float x, float y, int& row, int& col);
    
        bool empty();
        
        int getRows();
        int getCols();

        float getLength();
        float getWidth();
        float getResolution();

        const float& getXorigin();
        const float& getYorigin();
        const float& getReciprocalResolution();

        /**
         * @说明：点云数据对应到地图上的代价值
         * @返回：代价（归一化）
         */  
        float getCost(const common::PointCloud& point_cloud);
        float getPixelProbablity(const int row, const int col);

        /**
         * @说明：获取地图矩阵的引用
         * @返回：矩阵的引用
         */ 
        Eigen::MatrixXf& getMapCite();
        Eigen::MatrixXf& getInverseMapCite();

        /**
         * @说明：获取地图矩阵的指针
         * @返回：矩阵的指针
         */ 
        Eigen::MatrixXf* getMapPtr();

        void __sharpenMap(Eigen::MatrixXf& map);

        void weakenPixel(float x, float y, float value);

    private:
        void __increaseProbablity(int& row, int& col, const float& probablity, const float& upper);
        void __increaseProbablity2(int& row, int& col, const float& probablity, const float& upper);
        void __gaussianIncrease(int& row, int& col, const float& probablity, const float& upper);
        void __increaseProbablityShadow(int& row, int& col, const float& probablity, const float& upper);
        void __increaseProbablityShadow2(int& row, int& col, const float& probablity, const float& upper);
        void __world2Map(float x, float y, int& row, int& col);
        void __writeMap(int row, int col, float value);
        void __normProbablity(const float& upper);
        void __clearMap();

        bool __outBound(int row, int col);
        bool __outBound(Eigen::MatrixXf& map, int row, int col);

        float& __read(int row, int col);
        float& __read(Eigen::MatrixXf& map, int row, int col);

        float __readShadow(int row, int col);

        Eigen::MatrixXf map_;
        Eigen::MatrixXf shadow_map_;
        
        std::map<const std::tuple<int /*row*/, int /*col*/>, 
            float* /*概率*/> index_table_;
        std::map<const std::tuple<int /*row*/, int /*col*/>, 
            float* /*概率*/> shadow_;
        
        bool use_shadow_;

        int rows_, cols_;
        float width_, length_, resolution_, reciprocal_resolution_;
        float x_origin_, y_origin_;
};

}


#endif