#ifndef MOVE_BASE_SIMPLE_ACTION_SERVER_H_
#define MOVE_BASE_SIMPLE_ACTION_SERVER_H_

#include <string>
#include <memory>
#include <functional>
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition_variable.hpp>

// Action message types
#include <robot_move_base_msgs/MoveBaseAction.h>
#include <robot_move_base_msgs/MoveBaseActionGoal.h>
#include <robot_move_base_msgs/MoveBaseActionResult.h>
#include <robot_move_base_msgs/MoveBaseActionFeedback.h>
#include <robot_move_base_msgs/MoveBaseGoal.h>
#include <robot_move_base_msgs/MoveBaseResult.h>
#include <robot_move_base_msgs/MoveBaseFeedback.h>

// Actionlib message types
#include <robot_actionlib_msgs/GoalID.h>
#include <robot_actionlib_msgs/GoalStatus.h>
#include <robot_actionlib_msgs/GoalStatusArray.h>

// Robot framework
#include <robot/robot.h>  

namespace move_base
{

/**
 * @class SimpleActionServer
 * @brief A simplified action server implementation similar to ROS actionlib::SimpleActionServer
 * 
 * This class provides a simple interface for implementing action servers without ROS dependency.
 * It handles goal requests, preemption, feedback publishing, and result publishing using callback functions.
 * 
 * @tparam ActionSpec The action message type (e.g., robot_move_base_msgs::MoveBaseAction)
 */
template<class ActionSpec>
class SimpleActionServer
{
public:
  // Typedefs for action message types
  typedef typename ActionSpec::_action_goal_type ActionGoal;
  typedef typename ActionSpec::_action_result_type ActionResult;
  typedef typename ActionSpec::_action_feedback_type ActionFeedback;
  
  typedef typename ActionGoal::_goal_type Goal;
  typedef typename ActionResult::_result_type Result;
  typedef typename ActionFeedback::_feedback_type Feedback;
  
  typedef boost::shared_ptr<const Goal> GoalConstPtr;
  typedef boost::shared_ptr<const Result> ResultConstPtr;
  typedef boost::shared_ptr<const Feedback> FeedbackConstPtr;
  typedef typename ActionGoal::ConstPtr ActionGoalConstPtr;
  typedef typename ActionResult::ConstPtr ActionResultConstPtr;
  typedef typename ActionFeedback::ConstPtr ActionFeedbackConstPtr;
  typedef robot_actionlib_msgs::GoalStatusArrayConstPtr GoalStatusArrayConstPtr;

  // Callback function types
  typedef std::function<void(const ActionGoalConstPtr&)> GoalCallback;
  typedef std::function<void(const robot_actionlib_msgs::GoalIDConstPtr&)> CancelCallback;
  typedef std::function<void(const ActionFeedbackConstPtr&)> FeedbackCallback;
  typedef std::function<void(const ActionResultConstPtr&)> ResultCallback;
  typedef std::function<void(const GoalStatusArrayConstPtr&)> StatusCallback;
  typedef std::function<void(const GoalConstPtr&)> ExecuteCallback;
  
  // Typedefs for non-const pointers
  typedef typename ActionResult::Ptr ActionResultPtr;
  typedef typename ActionFeedback::Ptr ActionFeedbackPtr;

  /**
   * @brief Constructor
   * @param name Action name (e.g., "move_base")
   * @param execute_callback Callback function to execute when a goal is received
   * @param auto_start If true, automatically start the server
   */
  SimpleActionServer(const std::string& name,
                     ExecuteCallback execute_callback,
                     bool auto_start = true);

  /**
   * @brief Destructor
   */
  ~SimpleActionServer();

  /**
   * @brief Start the action server
   */
  void start();

  /**
   * @brief Stop the action server
   */
  void stop();

  /**
   * @brief Set callback for goal messages
   * @param callback Function to call when a goal is received
   */
  void setGoalCallback(GoalCallback callback);

  /**
   * @brief Set callback for cancel messages
   * @param callback Function to call when a cancel is received
   */
  void setCancelCallback(CancelCallback callback);

  /**
   * @brief Set callback for feedback publishing
   * @param callback Function to call when feedback needs to be published
   */
  void setFeedbackCallback(FeedbackCallback callback);

  /**
   * @brief Set callback for result publishing
   * @param callback Function to call when result needs to be published
   */
  void setResultCallback(ResultCallback callback);

  /**
   * @brief Set callback for status publishing
   * @param callback Function to call when status needs to be published
   */
  void setStatusCallback(StatusCallback callback);

  /**
   * @brief Set status publish rate
   * @param rate Publishing rate in Hz
   */
  void setStatusPublishRate(double rate);

  /**
   * @brief Process an incoming goal message
   * @param goal_msg The goal message to process
   */
  void processGoal(const ActionGoalConstPtr& goal_msg);

  /**
   * @brief Process an incoming cancel message
   * @param cancel_msg The cancel message to process
   */
  void processCancel(const robot_actionlib_msgs::GoalIDConstPtr& cancel_msg);

  /**
   * @brief Check if a new goal is available
   * @return True if a new goal is available
   */
  bool isNewGoalAvailable();

  /**
   * @brief Check if preemption has been requested
   * @return True if preemption has been requested
   */
  bool isPreemptRequested();

  /**
   * @brief Accept a new goal
   * @return The new goal, or nullptr if no new goal is available
   */
  GoalConstPtr acceptNewGoal();

  /**
   * @brief Set the action as succeeded
   * @param result The result to send
   * @param text Success message
   */
  void setSucceeded(const Result& result = Result(), const std::string& text = "");

  /**
   * @brief Set the action as aborted
   * @param result The result to send
   * @param text Error message
   */
  void setAborted(const Result& result = Result(), const std::string& text = "");

  /**
   * @brief Set the action as preempted
   * @param result The result to send
   * @param text Message
   */
  void setPreempted(const Result& result = Result(), const std::string& text = "");

  /**
   * @brief Publish feedback
   * @param feedback The feedback to publish
   */
  void publishFeedback(const Feedback& feedback);

  /**
   * @brief Get the current goal
   * @return The current goal, or nullptr if no goal is active
   */
  GoalConstPtr getCurrentGoal();

  /**
   * @brief Get the current status
   * @return The current goal status
   */
  robot_actionlib_msgs::GoalStatus getCurrentStatus();

  /**
   * @brief Check if the server is active
   * @return True if the server is active
   */
  bool isActive();

  /**
   * @brief Get the action name
   * @return The action name
   */
  std::string getName() const;

private:
  /**
   * @brief Execute the goal in a separate thread
   */
  void executeGoal();

  /**
   * @brief Thread function to publish status periodically
   */
  void statusPublishThread();

  // Action name
  std::string name_;
  
  // Execute callback
  ExecuteCallback execute_callback_;
  
  // Callbacks for external communication
  GoalCallback goal_callback_;
  CancelCallback cancel_callback_;
  FeedbackCallback feedback_callback_;
  ResultCallback result_callback_;
  StatusCallback status_callback_;
  
  // State
  boost::mutex state_mutex_;
  boost::condition_variable goal_condition_;
  bool active_;
  bool preempt_requested_;
  bool new_goal_available_;
  GoalConstPtr goal_;
  robot_actionlib_msgs::GoalID current_goal_id_;
  robot_actionlib_msgs::GoalStatus current_status_;
  double status_publish_rate_;
  
  // Threads
  boost::thread status_thread_;
  boost::thread execute_thread_;
};

} // namespace move_base
#endif // MOVE_BASE_SIMPLE_ACTION_SERVER_H_

