#ifndef __LOC_BASE_H_INCLUDED_
#define __LOC_BASE_H_INCLUDED_
#include <ros/ros.h>
#include <thread> 
#include <memory> 
#include "loc_core/localization.h"
#include "amcl/amcl.h"
#include "map_server/map_server.h"
// #include "hector_mapping/HectorMappingRos.h"
#include "slam_toolbox/slam_toolbox_async.hpp"

namespace loc_base
{
	class LocBase : public loc_core::BaseLocalization
	{
	public:
		/**
		 * @brief Constructor
		 */
		LocBase();

		/**
		 * @brief Destructor
		 */
		virtual ~LocBase();

		/**
		 * @brief Initialization function for loc base
		 * @param nh  A Node handle
		 * @param tf  A poiter tranform listen
		*/
		void initialize(ros::NodeHandle nh, std::shared_ptr<tf2_ros::Buffer> tf) override;

		/**
		 * @brief Loading a Activate Map File name 
		 * @param activated_map_filename The Map File name 
		 * @return True if activated_map_filename is exsits, else Fasle
		 */
		bool loadActivateMapFileName(std::string& activated_map_filename) override;

		/**
		 * @brief Saving a Activate Map File name 
		 * @param activated_map_filename The Map File name 
		 * @return True if activated_map_filename is exsits, else Fasle
		 */
		bool saveActivateMapFileName(const std::string& activated_map_filename) override;

		/**
		 * @brief Create any files to a folder of the path
		 * @param path The path where is be saved any files (png, yaml ...)
		 * @param map_name The name folder and files
		 */
		bool createStaticMap(const std::string map_name) override;

		/**
		* @brief Load a map given a path to a yaml file
		* @param filename The file name
		* @return True/False If load map succesed or not done
		*/
		bool changeStaticMap(const std::string filename) override;

		/**
		* @brief Load a virtual walls map given a path to a yaml file
		* @param filename The file name
		* @return True/False If load map succesed or not done
		*/
		bool changeVirtualWallsMap(const std::string filename) override;

		/**
		* @brief Get the list of map folder in the workingdirection
		* @param directories The list map name
		*/
		void listMapFiles(std::stringstream &directories) override;

		/**
		 * @brief Start the mapping process.
		 * 				When the state variable 'SlamState' is 'Ready', this function can be called.
		 * 				Upon successful invocation, the state variable 'SlamState' will change to 'Mapping'.
		 * @return True/False Call fucntion is successed/failed.
		 */
		bool startMapping() override;

		/**
		 * @brief Turn off the Localization mode.
		 * 				At this point, the robot's coordinate information will not be updated.
		 * 				If called successfully, the 'SlamState' variable will change to 'Ready'.
		 * @return True/False Call fucntion is successed/failed
		 */
		bool stopMapping() override;

		/**
		 * @brief Set the initial position of the robot on the map.
		 *        The condition to call the function is that the 'SlamState' is 'Localization'.
		 *        When the function is called successfully, the 'SlamState' will change to 'Calibrations'.
		 *        When the 'Calibrations' process ends, the 'SlamState' will change back to 'Localization'.
		 * @return True/False Call fucntion is successed/failed
		 */
		bool startLocalization() override;

		/**
		 * @brief Turn off the Localization mode.
		 * 				At this point, the robot's coordinate information will not be updated.
		 * 				If called successfully, the 'SlamState' variable will change to 'Ready'.
		 * @return True/False Call fucntion is successed/failed
		 */
		bool stopLocalization() override;

		/**
		 * @brief Get State of Loc Base
		 */
		loc_core::slam_state_en getState() override;

		// static std::string activated_map_filename_;
		// std::string map_topic_;

	protected:

		virtual bool loadParameters();

		virtual void initalPose(geometry_msgs::PoseWithCovariance init_pose);

		static bool getPose(std::string base_frame_id, std::string map_frame,
			geometry_msgs::PoseStamped& global_pose, double transform_tolerance = 2.0);

		static void savePose();

		virtual bool init_map_server();

		virtual bool close_map_server();

		virtual bool init_localization();

		virtual bool close_localization();

		virtual bool init_mapping();

		virtual bool close_mapping();

		static void signalHandler(int sign);

		virtual void savePoseCallback(const ros::WallTimerEvent& event);

		virtual void cleanup();


		bool initialized_;
		ros::NodeHandle nh_;
		ros::NodeHandle private_nh_;
		ros::Publisher init_pub_;
		static std::shared_ptr<tf2_ros::Buffer> tf_;

		XmlRpc::XmlRpcValue plugins_;
		// std::map<std::string, std::string> map_file_vt_;
		std::map<std::string, std::shared_ptr<map_server::MapServer>> static_map_vt_;

		static std::string base_frame_id_, global_frame_id_;
		static std::shared_ptr<amcl::Amcl> localization_althm_;
		std::mutex localization_mutex_;
		std::shared_ptr<ros::WallTimer> localization_timer_ptr_;
		static std::shared_ptr<slam_toolbox::AsynchronousSlamToolbox> mapping_althm_;

		loc_core::slam_state_en slam_state_{ loc_core::Ready };
		static std::string *working_dir_ptr_;
	};

} // namespace loc_base

#endif // __LOC_BASE_H_INCLUDED_