#ifndef _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_
#define _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_

#include <memory>

// msgs
#include <robot_geometry_msgs/Twist.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Vector3.h>
#include <robot_geometry_msgs/PolygonStamped.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_msgs/Odometry.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <robot_sensor_msgs/DepthCameraData.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_protocol_msgs/Order.h>
#include <robot_nav_2d_utils/conversions.h>

// tf
#include <tf3/buffer_core.h>
#include <tf3/utils.h>
#include <tf3/convert.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot/time.h>
#include <move_base_core/common.h>

namespace robot
{
	namespace move_base_core
	{
		// Navigation states, including planning and controller status
		enum class State
		{
			PENDING,
			ACTIVE,
			PREEMPTED,
			SUCCEEDED,
			ABORTED,
			REJECTED,
			PREEMPTING,
			RECALLING,
			RECALLED,
			LOST,
			PLANNING,
			CONTROLLING,
			CLEARING,
			PAUSED
		};

		/**
		 * @brief Feedback structure to describe current navigation status
		 */
		struct NavFeedback
		{
			State navigation_state;										// Current navigation state
			std::string feed_back_str;								// Description or debug message
			robot_geometry_msgs::Pose2D current_pose; // Robot’s current pose in 2D
			bool goal_checked;												// Whether the current goal is verified
			bool is_ready;														// Robot is ready for commands
		};

		/**
		 * @brief Planner data output structure.
		 */
		struct PlannerDataOutput
		{
			robot_nav_2d_msgs::Path2D plan;
			robot_nav_msgs::OccupancyGrid costmap;
			robot_map_msgs::OccupancyGridUpdate costmap_update;
			bool is_costmap_updated;
			robot_geometry_msgs::PolygonStamped footprint;
		};

		/**
		 * @brief Convert a State enum to its string representation.
		 *        Useful for debugging/logging or displaying in UI.
		 *
		 * @param state Enum value of move_base_core::State
		 * @return std::string The corresponding string, e.g. "PENDING"
		 */
		inline std::string toString(move_base_core::State state)
		{
			using move_base_core::State;
			switch (state)
			{
			case State::PENDING:
				return "PENDING"; // Chờ xử lý
			case State::ACTIVE:
				return "ACTIVE"; // Đang hoạt động
			case State::PREEMPTED:
				return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới
			case State::SUCCEEDED:
				return "SUCCEEDED"; // Thành công
			case State::ABORTED:
				return "ABORTED"; // Bị lỗi
			case State::REJECTED:
				return "REJECTED"; // Từ chối bởi planner hoặc controller
			case State::PREEMPTING:
				return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu
			case State::RECALLING:
				return "RECALLING"; // Đang huỷ bỏ nội bộ
			case State::RECALLED:
				return "RECALLED"; // Đã được thu hồi
			case State::LOST:
				return "LOST"; // Mất trạng thái
			case State::PLANNING:
				return "PLANNING"; // Đang lập kế hoạch đường đi
			case State::CONTROLLING:
				return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan
			case State::CLEARING:
				return "CLEARING"; // Đang dọn dẹp bản đồ / costmap
			case State::PAUSED:
				return "PAUSED"; // Tạm dừng
			default:
				return "UNKNOWN_STATE"; // Không xác định
			}
		}

		/**
		 * @brief Creates a target pose (PoseStamped) by offsetting a given 2D pose along its heading direction.
		 *
		 * This function calculates a new position by moving forward (or backward if negative)
		 * a certain `offset_distance` from the current position, following the given heading angle (theta).
		 *
		 * @param pose             The original 2D pose (x, y, theta) in the local plane.
		 * @param frame_id         The coordinate frame in which the output PoseStamped will be expressed.
		 * @param offset_distance  The distance to offset along the heading direction, in meters.
		 *                         Positive moves forward, negative moves backward.
		 * @return                 A new PoseStamped offset from the input pose, in the given frame.
		 */
		inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance)
		{
			robot_geometry_msgs::PoseStamped goal;
			goal.header.frame_id = frame_id;
			goal.header.stamp = robot::Time::now();
			goal.pose.position.x = pose.x + offset_distance * cos(pose.theta);
			goal.pose.position.y = pose.y + offset_distance * sin(pose.theta);
			goal.pose.orientation = robot_nav_2d_utils::pose2DToPose(pose).orientation;
			return goal;
		}

		/**
		 * @brief Overloaded version: creates an offset target pose from a given PoseStamped.
		 *
		 * This function extracts the 2D position and orientation (yaw) from the given PoseStamped,
		 * offsets it forward (or backward) along the current heading direction,
		 * and returns a new PoseStamped in the same frame.
		 *
		 * @param pose             The original pose with full position and orientation.
		 * @param offset_distance  Distance to offset along the current heading direction (in meters).
		 * @return                 A new PoseStamped offset from the original pose.
		 */
		inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::PoseStamped &pose, double offset_distance)
		{
			robot_geometry_msgs::Pose2D pose2d;
			pose2d.x = pose.pose.position.x;
			pose2d.y = pose.pose.position.y;
			pose2d.theta = robot_nav_2d_utils::poseStampedToPose2D(pose).pose.theta;
			return offset_goal(pose2d, pose.header.frame_id, offset_distance);
		}

		/**
		 * @class BaseNavigation
		 * @brief Abstract interface for robot navigation modules.
		 *
		 * Provides core methods for setting goals, moving, rotating, and handling motion control.
		 * All navigation logic must implement this interface.
		 */
		class BaseNavigation
		{
		public:
			using Ptr = std::shared_ptr<BaseNavigation>;
			robot_nav_msgs::OccupancyGrid map_save_;
    		std::string map_name_save_;
			virtual ~BaseNavigation() {}

			/**
			 * @brief Initialize the navigation system.
			 * @param tf Shared pointer to the TF listener or buffer.
			 */
			virtual void initialize(TFListenerPtr tf) = 0;

			/**
			 * @brief Stop internal activity (control loop, planner...) before destruction.
			 *
			 * The host MUST be able to quiesce the navigation core at a chosen point in its
			 * own shutdown sequence, while the process is still fully alive — destroying the
			 * object from a static-destruction context after main() has returned is exactly
			 * the race that produced boost::lock_error crashes. Default is a no-op so legacy
			 * implementations remain source-compatible; after this call the object must stop
			 * producing velocity commands, and queries (getFeedback, getTwist...) must remain
			 * safe to call. Safe to call multiple times.
			 */
			virtual void shutdown() {}

			/**
			 * @brief Set the robot's footprint (outline shape) in the global frame.
			 *        This can be used for planning or collision checking.
			 *
			 * @param fprt A vector of points representing the robot's footprint polygon.
			 *             The points should be ordered counter-clockwise.
			 * Example:
			 *
				^ Y
				|
				|      P3(-0.3, 0.2)        P2(0.3, 0.2)
				|         ●---------------●
				|         |               |
				|         |     Robot     |   (view Top )
				|         |               |
				|         ●---------------●
				|      P4(-0.3, -0.2)       P1(0.3, -0.2)
				+-------------------------------> X

				std::vector<robot_geometry_msgs::Point> footprint;
				1. footprint.push_back(make_point(0.3, -0.2));
				2. footprint.push_back(make_point(0.3, 0.2));
				3. footprint.push_back(make_point(-0.3, 0.2));
				4. footprint.push_back(make_point(-0.3, -0.2));
			 */
			virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) = 0;

			/**
			 * @brief Get the robot's footprint (outline shape) in the global frame.
			 * @return A vector of points representing the robot's footprint polygon.
			 */
			virtual std::vector<robot_geometry_msgs::Point> getRobotFootprint() = 0;

			/**
			 * @brief Add a static map to the navigation system.
			 * @param map_name The name of the map.
			 * @param map The map to add.
			 */
			virtual void addStaticMap(const std::string &map_name, robot_nav_msgs::OccupancyGrid map) = 0;

			/**
			 * @brief Add a laser scan to the navigation system.
			 * @param laser_scan_name The name of the laser scan.
			 * @param laser_scan The laser scan to add.
			 */
			virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) = 0;

			/**
			 * @brief Add a point cloud to the navigation system.
			 * @param point_cloud_name The name of the point cloud.
			 * @param point_cloud The point cloud to add.
			 */
			virtual void addPointCloud(const std::string &point_cloud_name, robot_sensor_msgs::PointCloud point_cloud) = 0;

			/**
			 * @brief Add a point cloud2 to the navigation system.
			 * @param point_cloud2_name The name of the point cloud2.
			 * @param point_cloud2 The point cloud2 to add.
			 */
			virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) = 0;

			/**
			 * @brief Add one coherent depth-camera sample to the navigation system.
			 * @param topic The logical topic of the combined depth-camera stream.
			 * @param data Depth image and matching camera intrinsics.
			 */
			virtual void addDepthCameraData(const std::string &topic,
			                                robot_sensor_msgs::DepthCameraData::ConstPtr data) = 0;

			/**
			 * @brief Get a static map from the navigation system.
			 * @param map_name The name of the map.
			 * @return The map.
			 */
			virtual robot_nav_msgs::OccupancyGrid getStaticMap(const std::string &map_name) = 0;

			/**
			 * @brief Get a laser scan from the navigation system.
			 * @param laser_scan_name The name of the laser scan.
			 * @return The laser scan.
			 */
			virtual robot_sensor_msgs::LaserScan getLaserScan(const std::string &laser_scan_name) = 0;

			/**
			 * @brief Get a point cloud from the navigation system.
			 * @param point_cloud_name The name of the point cloud.
			 * @return The point cloud.
			 */
			virtual robot_sensor_msgs::PointCloud getPointCloud(const std::string &point_cloud_name) = 0;

			/**
			 * @brief Get a point cloud2 from the navigation system.
			 * @param point_cloud2_name The name of the point cloud2.
			 * @return The point cloud2.
			 */
			virtual robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string &point_cloud2_name) = 0;

			/**
			 * @brief Get all static maps from the navigation system.
			 * @return The static maps.
			 */
			virtual std::map<std::string, robot_nav_msgs::OccupancyGrid> getAllStaticMaps() = 0;

			/**
			 * @brief Get all laser scans from the navigation system.
			 * @return The laser scans.
			 */
			virtual std::map<std::string, robot_sensor_msgs::LaserScan> getAllLaserScans() = 0;

			/**
			 * @brief Get all point clouds from the navigation system.
			 * @return The point clouds.
			 */
			virtual std::map<std::string, robot_sensor_msgs::PointCloud> getAllPointClouds() = 0;

			/**
			 * @brief Get all point cloud2s from the navigation system.
			 * @return The point cloud2s.
			 */
			virtual std::map<std::string, robot_sensor_msgs::PointCloud2> getAllPointCloud2s() = 0;

			/**
			 * @brief Remove a static map from the navigation system.
			 * @param map_name The name of the map.
			 * @return True if the map was removed successfully.
			 */
			virtual bool removeStaticMap(const std::string &map_name) = 0;

			/**
			 * @brief Remove a laser scan from the navigation system.
			 * @param laser_scan_name The name of the laser scan.
			 * @return True if the laser scan was removed successfully.
			 */
			virtual bool removeLaserScan(const std::string &laser_scan_name) = 0;

			/**
			 * @brief Remove a point cloud from the navigation system.
			 * @param point_cloud_name The name of the point cloud.
			 * @return True if the point cloud was removed successfully.
			 */
			virtual bool removePointCloud(const std::string &point_cloud_name) = 0;

			/**
			 * @brief Remove a point cloud2 from the navigation system.
			 * @param point_cloud2_name The name of the point cloud2.
			 * @return True if the point cloud2 was removed successfully.
			 */
			virtual bool removePointCloud2(const std::string &point_cloud2_name) = 0;

			/**
			 * @brief Remove all static maps from the navigation system.
			 * @return True if the static maps were removed successfully.
			 */
			virtual bool removeAllStaticMaps() = 0;

			/**
			 * @brief Remove all laser scans from the navigation system.
			 * @return True if the laser scans were removed successfully.
			 */
			virtual bool removeAllLaserScans() = 0;

			/**
			 * @brief Remove all point clouds from the navigation system.
			 * @return True if the point clouds were removed successfully.
			 */
			virtual bool removeAllPointClouds() = 0;

			/**
			 * @brief Remove all point cloud2s from the navigation system.
			 * @return True if the point cloud2s were removed successfully.
			 */
			virtual bool removeAllPointCloud2s() = 0;

			/**
			 * @brief Remove all data from the navigation system.
			 * @return True if the data was removed successfully.
			 */
			virtual bool removeAllData() = 0;

			/**
			 * @brief Add an odometry to the navigation system.
			 * @param odometry_name The name of the odometry.
			 * @param odometry The odometry to add.
			 */
			virtual void addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) = 0;

			/**
			 * @brief Send a goal for the robot to navigate to.
			 * @param goal              Target pose in the global frame.
			 * @param xy_goal_tolerance Acceptable error in X/Y (meters).
			 * @param yaw_goal_tolerance Acceptable angular error (radians).
			 * @return True if goal was accepted and sent successfully.
			 */
			virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal,
													double xy_goal_tolerance = 0.0,
													double yaw_goal_tolerance = 0.0) = 0;

			/**
			 * @brief Send a goal for the robot to navigate to.
			 * @param msg               Order message.
			 * @param goal              Target pose in the global frame.
			 * @param xy_goal_tolerance Acceptable error in X/Y (meters).
			 * @param yaw_goal_tolerance Acceptable angular error (radians).
			 * @return True if goal was accepted and sent successfully.
			 */
			virtual bool moveTo(const robot_protocol_msgs::Order &msg,
													const robot_geometry_msgs::PoseStamped &goal,
													double xy_goal_tolerance = 0.0,
													double yaw_goal_tolerance = 0.0) = 0;

			/**
			 * @brief Send a docking goal to a predefined marker (e.g. charger).
			 * @param maker             Marker name or ID.
			 * @param goal              Target pose for docking.
			 * @param xy_goal_tolerance Acceptable XY error (meters).
			 * @param yaw_goal_tolerance Acceptable heading error (radians).
			 * @return True if docking command succeeded.
			 */
			virtual bool dockTo(const std::string &maker,
													const robot_geometry_msgs::PoseStamped &goal,
													double xy_goal_tolerance = 0.0,
													double yaw_goal_tolerance = 0.0) = 0;

			/**
			 * @brief Send a docking goal to a predefined marker (e.g. charger).
			 * @param msg               Order message.
			 * @param goal              Target pose for docking.
			 * @param xy_goal_tolerance Acceptable XY error (meters).
			 * @param yaw_goal_tolerance Acceptable heading error (radians).
			 * @return True if docking command succeeded.
			 */
			virtual bool dockTo(const robot_protocol_msgs::Order &msg, 
													const std::string &marker,
													const robot_geometry_msgs::PoseStamped &goal,
													double xy_goal_tolerance = 0.0,
													double yaw_goal_tolerance = 0.0) = 0;

			/**
			 * @brief Move straight toward the target position (X-axis).
			 * @param goal              Target pose.
			 * @param xy_goal_tolerance Acceptable positional error (meters).
			 * @return True if command issued successfully.
			 */
			virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal,
																	double xy_goal_tolerance = 0.0) = 0;

			/**
			 * @brief Rotate in place to align with target orientation.
			 * @param goal              Pose containing desired heading (only Z-axis used).
			 * @param yaw_goal_tolerance Acceptable angular error (radians).
			 * @return True if rotation command was sent successfully.
			 */
			virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal,
														double yaw_goal_tolerance = 0.0) = 0;

			/**
			 * @brief Pause the robot's movement (e.g. during obstacle avoidance).
			 */
			virtual void pause() = 0;

			/**
			 * @brief Resume motion after a pause.
			 */
			virtual void resume() = 0;

			/**
			 * @brief Cancel the current goal and stop the robot.
			 */
			virtual void cancel() = 0;

			/**
			 * @brief Send limited linear velocity command.
			 * @param linear Linear velocity vector (x, y, z).
			 * @return True if the command was accepted.
			 */
			virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) = 0;

			/**
			 * @brief Send limited angular velocity command.
			 * @param angular Angular velocity vector (x, y, z).
			 * @return True if the command was accepted.
			 */
			virtual bool setTwistAngular(const robot_geometry_msgs::Vector3 &angular) = 0;

			/**
			 * @brief Get the robot’s pose as a PoseStamped.
			 * @param pose Output parameter with the robot’s current pose.
			 * @return True if pose was successfully retrieved.
			 */
			virtual bool getRobotPose(robot_geometry_msgs::PoseStamped &pose) = 0;

			/**
			 * @brief Get the robot’s pose as a 2D pose.
			 * @param pose Output parameter with the robot’s current 2D pose.
			 * @return True if pose was successfully retrieved.
			 */
			virtual bool getRobotPose(robot_geometry_msgs::Pose2D &pose) = 0;

			/**
			 * @brief Get the robot’s twist.
			 * @return The robot’s current twist.
			 */
			virtual robot_nav_2d_msgs::Twist2DStamped getTwist() = 0;

			/**
			 * @brief Get the navigation feedback.
			 * @return Pointer to the navigation feedback.
			 */
			virtual NavFeedback *getFeedback() = 0;

			/**
			 * @brief Get the global data.
			 * @return The global data.
			 */
			virtual PlannerDataOutput getGlobalData() = 0;

			/**
			 * @brief Get the local data.
			 * @return The local data.
			 */
			virtual PlannerDataOutput getLocalData() = 0;

		protected:
			// Shared feedback data for navigation status tracking
			std::shared_ptr<NavFeedback> nav_feedback_;
			robot_nav_2d_msgs::Twist2DStamped twist_;
			robot_nav_msgs::Odometry odometry_;
			PlannerDataOutput global_data_;
			PlannerDataOutput local_data_;

			std::map<std::string, robot_nav_msgs::OccupancyGrid> static_maps_;
			std::map<std::string, robot_sensor_msgs::LaserScan> laser_scans_;
			std::map<std::string, robot_sensor_msgs::PointCloud> point_clouds_;
			std::map<std::string, robot_sensor_msgs::PointCloud2> point_cloud2s_;

			BaseNavigation() = default;
		};

	} // namespace move_base_core
};

#endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_
