#ifndef ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#define ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_

#include <robot_costmap_2d/static_layer.h>
#include <robot_nav_msgs/Path.h>

namespace robot_costmap_2d
{
  class DirectionalLayer : public StaticLayer
  {
  public:
    DirectionalLayer();
    virtual ~DirectionalLayer();
    bool laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan);
    void resetMap();

	private:
    void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map);
		bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path);
    unsigned char laneFilter(unsigned int x, unsigned int y, double yaw_robot);
		void laneFilter(std::vector<unsigned int> x, std::vector<unsigned int> y, std::vector<double> yaw_robot);
    void laneFilter(std::vector<std::pair<unsigned int, double>> x, std::vector<std::pair<unsigned int, double>> y, std::vector<double> yaw_robot);
    bool inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance);
    void convertToMap(unsigned char *costmap, robot_nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th);
    bool getRobotPose(double &x, double &y, double &yaw);

    std::vector<std::array<uint16_t, 2>> directional_areas_;
    double pose_x_, pose_y_, pose_yaw_;
    double distance_skip_ = 1.0;
    // ros::Publisher lane_mask_pub_;
    robot_nav_msgs::OccupancyGrid new_map_;
	};
}

#endif // ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_H_