// 导航代价图，用于机器人路径规划和自动避障
// 作者:HeJunzhen
// 时间:2020/07/15
// 成都河狸智能科技有限责任公司

#ifndef NAVIGATION_COSTMAP_ROS_H_
#define NAVIGATION_COSTMAP_ROS_H_

#include <queue>
#include <vector>
#include <string>

#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>

#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <sensor_msgs/PointCloud.h>
#include <nav_msgs/OccupancyGrid.h>

#include "common/common.h"

#include <std_msgs/UInt32.h>


typedef uint32_t uint32;
typedef unsigned char uchar;

namespace nav
{

    namespace costmap
    {

        // 栅格
        struct Cell
        {
            Cell(float dist, int index, uint32 x, uint32 y, uint32 sx, uint32 sy) : distance_(dist), index_(index), x_(x), y_(y), sx_(sx), sy_(sy) {}

            Cell() {}

            float distance_;         // 到障碍物的距离
            uint32 index_;           // 地图数组索引号
            uint32 x_, y_, sx_, sy_; // x_:x坐标 y_:y坐标 sx_:最近障碍物x坐标 sy_:最近障碍物y坐标
        };

        inline bool operator<(const Cell &a, const Cell &b)
        {
            return a.distance_ > b.distance_;
        }

        // 代价值定义
        enum COST_VALUE
        {
            PERDEFINED_PLAN = 0,               // 预规划路径在代价地图代价值
            FREE_SPACE = 100,                  // 空闲区域代价值
            INSCRIBED_INFLATED_OBSTACLE = 253, // 地图边界代价值
            LETHAL_OBSTACLE,                   // 障碍物代价值
            NO_INFORMATION,                    // 未知区域代价值
        };

        class Pose
        {
            float x;
            float y;
            float theta;
        };

        // 机器人定位点，需要计算连续两次数据的时间差来判断当前数据是否可用
        class PoseWithTime : public Pose
        {
        public:
            ros::Time time; // 记录收到数据时的时间
            bool use;       // 数据是否可用
        };

        // 代价地图
        class CostMap
        {
        public:
            CostMap(CostmapInfo costmap_config); // 传入代价地图配置信息
            ~CostMap();

        public:
            void restart(); // 收到新的地图需要重启动代价图

            void run(); // 代价图需要接收到地图后才能启动，否则一直等待

            void stop(); // 停止

            void deleteAll(); // 释放代价图内存

            // 获取机器人当前位姿
            // 传入:pose(引用)位姿
            // 返回:true:当前数据可用 false:当前数据不可用
            bool getRobotPose(Pose &pose);

            // 预设置代价图路径
            // 输入:路径
            // 返回:true:设置成功 false:设置失败
            bool setCostPath(const Path path);

            // 设置代价地图栅格的代价值
            // 输入:x:栅格的x坐标 y:栅格的y坐标 value:设置的代价值 mandatory:是否强制设置(覆盖原先障碍物 true:强制 false:不强制)
            // 返回:true：设置成功 false:设置失败
            bool setCostValue(uint32 x, uint32 y, uchar value, bool mandatory = false);

            // 获取栅格内的代价值
            // 输入: x:栅格x坐标 y:栅格y坐标
            // 返回: 栅格的代价值
            uchar getCost(uint32 x, uint32 y);

            // 获取整个代价地图
            // 返回: 整个代价地图的指针
            uchar *getCostmap();

            // 获取代价地图X轴的长度
            // 返回: 代价地图X轴的长度
            uint32 getSizeInCellsX() { return size_x_; };

            // 获取代价地图Y轴的长度
            // 返回: 代价地图Y轴的长度
            uint32 getSizeInCellsY() { return size_y_; };

            // 获取代价地图原点坐标的X值
            // 返回: 代价地图原点坐标的X值
            float getOriginX() { return origin_x_; };

            // 获取代价地图原点坐标的Y值
            // 返回: 代价地图原点坐标的Y值
            float getOriginY() { return origin_y_; };

            // 获取代价地图的分辨率
            // 返回: 代价地图的分辨率
            float getResolution() { return resolution_; };

            // 世界坐标转地图坐标
            // 输入: wx:世界坐标X值 wy:世界坐标Y值 mx:地图坐标X值(引用) my:地图坐标Y值(引用)
            // 输出: true:转换成功 false:转换失败
            bool worldToMap(float wx, float wy, uint32 &mx, uint32 &my);

            // 地图坐标转世界坐标
            // 输入: mx:地图坐标X值 my:地图坐标Y值 wx:世界坐标X值(引用) wy:世界坐标Y值(引用)
            // 输出: true:转换成功 false:转换失败
            bool mapToWorld(uint32 mx, uint32 my, float &wx, float &wy);

        private:
            void initMap(); // 初始化代价地图

            void resetMap(); // 清空代价地图

            void pubCostmap(); // 发布代价地图

            // 计算距离内有几个栅格数
            // 输入: dist:长度距离
            // 返回： 长度距离内的栅格数
            int cellDistance(float dist);

            // 根据距离边界或障碍物的长度计算代价值
            // 输入: distance:距离边界或障碍物的长度
            // 返回: 计算的代价值
            uchar computeCost(float distance);

            // 代价区域核(原理跟高斯核一样)
            void getInflationAreaCost();

            // 将传入地图转换为代价地图
            // 输入: map:原始地图
            // 返回: 对应的代价地图
            uchar *setInflationMap(uchar *map);

            // 计算栅格代价值
            // 输入: mx:栅格x坐标 my:栅格y坐标 src_x:最近障碍物x坐标 src_y:最近障碍物y坐标
            uchar costLookup(int mx, int my, int src_x, int src_y);

            // 计算两个栅格的距离
            // 输入: mx:a栅格x坐标 mx:a栅格y坐标 mx:b栅格x坐标 mx:b栅格y坐标
            // 返回: a栅格到b栅格的距离
            float distanceLookup(int mx, int my, int src_x, int src_y);

            // 计算障碍物点在膨胀范围内的栅格,如果在则存储
            // 输入: index当前栅格在地图内的坐标 mx:栅格x值 my:栅格y值 src_x:最近障碍物x值 src_y:最近障碍物y值
            void enqueue(int index, int mx, int my, int src_x, int src_y);

            // 初始地图代价值赋值
            // 输入: value 原始地图栅格值
            // 返回: 对应的代价值
            uchar interpretValue(uchar value);

        private:
            // 接收地图
            void incomingMap(const nav_msgs::OccupancyGridConstPtr &map);

            // 接收机器人定位后坐标
            void locationCallback(const geometry_msgs::Pose2DConstPtr &pose);

            // 接收传感器点云数据
            void sensorCloudCallback(const sensor_msgs::PointCloudConstPtr &cloud);

            // 更新代价地图
            void updateCostMap();

        private:
            // 代价地图(含传感器数据)
            uchar *costmap_;

            // 原始代价地图(不含传感器数据)
            uchar *originCostmap_;

            std::vector<ros::Time> obstacleUpdateTimeMap_;

            // 原始地图
            uchar *originmap_;

            // 遍历地图
            bool *seen_;

            // 地图传入开始标志位
            bool run_;

            // 腐蚀半径(低代价值)   小于等于这个距离的代价根据距离计算
            float inflation_radius_;

            // 腐蚀半径(高代价值)    小于等于这个距离的代价 = 253
            float inscribe_radius_;

            // 腐蚀半径内的栅格数
            uint32 inflation_cells_;
            uint32 inscribe_cells_;

            float sensor_duration_;

            // 地图分辨率
            float resolution_;

            // 地图X轴长度
            uint32 size_x_;

            // 地图Y轴长度
            uint32 size_y_;

            // 代价图原点X坐标
            float origin_x_;

            // 代价图原点Y坐标
            float origin_y_;

            // 代价图更新频率
            uint32 frequency_;

            // 定位数据读写锁
            boost::shared_mutex locat_mutex_;

            // 传感器数据读写锁
            boost::shared_mutex sensor_mutex_;

            // 代价图读写锁
            boost::shared_mutex costmap_mutex_;

            // 代价地图更新线程
            boost::thread *update_cost_;

            // 机器人当前坐标
            PoseWithTime robot_pose_;

            // 腐蚀高斯核存储
            uchar **cached_cost_;
            float **cached_dist_;

            // 腐蚀距离内的栅格存储
            space::BlockQueue<Cell> bin_cells_;

            // 传感器点云数据
            std::vector<Cell> sensor_cells_;

            // 预设值的代价路径
            std::vector<int> cost_path_;

        private:
            ros::NodeHandle nh_;

            // 代价地图发布
            ros::Publisher costmap_publisher_;

            // 原始地图接收
            ros::Subscriber map_subscriber_;

            // 传感器数据接收
            ros::Subscriber sensor_subscriber_;

            // 定位坐标接收
            ros::Subscriber location_subscriber_;
        };

    }

}

#endif