#ifndef COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_

#include <costmap_2d/static_layer.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Path.h>

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

	private:
    void incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map);
		bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const 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, 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_;
    nav_msgs::OccupancyGrid new_map_;
    tf::TransformListener listener_;
	};
}

#endif // COSTMAP_2D_DIRECTIONAL_LAYER_H_