#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/OccupancyGrid.h>

bool isTrajectoryValid(costmap_2d::Costmap2DROS& costmap_ros, 
                       const std::vector<geometry_msgs::PoseStamped>& trajectory)
{
    costmap_2d::Costmap2D* costmap = costmap_ros.getCostmap();

    for (const auto& pose : trajectory)
    {
        // Get the costmap coordinates
        unsigned int x, y;
        if (!costmap->worldToMap(pose.pose.position.x, pose.pose.position.y, x, y))
        {
            ROS_WARN("Trajectory point is outside the costmap.");
            return false;
        }

        // Get the cost at the trajectory point
        unsigned char cost = costmap->getCost(x, y);

        // Check if cost is greater than threshold, indicating an obstacle
        // You can define your own threshold or create conditions based on your needs
        if (cost == costmap_2d::LETHAL_OBSTACLE || 
            cost == costmap_2d::UNKNOWN)
        {
            ROS_WARN("Trajectory point at (%f, %f) is violating the costmap region.",
                     pose.pose.position.x, pose.pose.position.y);
            return false; // Trajectory violates costmap
        }
    }
    return true; // All points are safe
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "costmap_check_node");
    ros::NodeHandle nh;

    costmap_2d::Costmap2DROS costmap_ros("costmap", nh);
    costmap_ros.start();

    std::vector<geometry_msgs::PoseStamped> trajectory; // Fill this with your trajectory data

    // Assuming the trajectory is filled with valid PoseStamped data

    if (isTrajectoryValid(costmap_ros, trajectory))
    {
        ROS_INFO("The trajectory is valid.");
    }
    else
    {
        ROS_INFO("The trajectory has invalid points.");
    }

    return 0;
}