/**
 * @file simple_action_server_example.cpp
 * @brief Example implementation of SimpleActionServer for MoveBaseAction
 * 
 * This example demonstrates how to use SimpleActionServer to implement
 * an action server without ROS dependency. It shows:
 * - How to initialize the action server
 * - How to set up callbacks for publishing messages
 * - How to handle goal execution
 * - How to publish feedback and results
 */

#include <move_base/simple_action_server.h>
#include <move_base/move_base.h>

// Message types
#include <robot_move_base_msgs/MoveBaseActionGoal.h>
#include <robot_move_base_msgs/MoveBaseActionResult.h>
#include <robot_move_base_msgs/MoveBaseActionFeedback.h>
#include <robot_actionlib_msgs/GoalID.h>
#include <robot_actionlib_msgs/GoalStatusArray.h>

#include <robot/robot.h>
#include <memory>
#include <thread>
#include <chrono>

namespace move_base
{

/**
 * @class MoveBaseActionServerExample
 * @brief Example class demonstrating SimpleActionServer usage
 */
class MoveBaseActionServerExample
{
public:
  MoveBaseActionServerExample()
    : move_base_ptr_(nullptr)
  {
    // Initialize move_base (your existing navigation system)
    // move_base_ptr_ = std::make_shared<MoveBase>(tf_);
    
    // Create SimpleActionServer with execute callback
    action_server_ = std::make_shared<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>>(
      "move_base",
      std::bind(&MoveBaseActionServerExample::executeCb, this, std::placeholders::_1),
      true  // auto_start
    );
    
    // Set up callbacks for publishing messages
    setupCallbacks();
    
    robot::log_info("[MoveBaseActionServerExample] Initialized");
  }
  
  ~MoveBaseActionServerExample()
  {
    if (action_server_)
    {
      action_server_->stop();
    }
  }
  
  /**
   * @brief Setup callbacks for SimpleActionServer
   * You can customize these callbacks to publish messages to your communication system
   */
  void setupCallbacks()
  {
    if (!action_server_)
      return;
    
    // Set callback for result publishing
    // Customize this to send result to your communication system
    action_server_->setResultCallback(
      [this](const robot_move_base_msgs::MoveBaseActionResultConstPtr& result)
      {
        // TODO: Publish result to your communication system (e.g., custom message bus, network, etc.)
        robot::log_info("[MoveBaseActionServerExample] Result: status=%d, text=%s",
                       result->status.status, result->status.text.c_str());
      }
    );
    
    // Set callback for feedback publishing
    // Customize this to send feedback to your communication system
    action_server_->setFeedbackCallback(
      [this](const robot_move_base_msgs::MoveBaseActionFeedbackConstPtr& feedback)
      {
        // TODO: Publish feedback to your communication system
        robot::log_debug("[MoveBaseActionServerExample] Published feedback");
      }
    );
    
    // Set callback for status publishing
    // Customize this to send status to your communication system
    action_server_->setStatusCallback(
      [this](const robot_actionlib_msgs::GoalStatusArrayConstPtr& status)
      {
        // TODO: Publish status to your communication system
      }
    );
    
    // Set callback for goal messages (optional - for logging)
    action_server_->setGoalCallback(
      [this](const robot_move_base_msgs::MoveBaseActionGoalConstPtr& goal)
      {
        robot::log_info("[MoveBaseActionServerExample] Goal callback: x=%.2f, y=%.2f",
                       goal->goal.target_pose.pose.position.x,
                       goal->goal.target_pose.pose.position.y);
      }
    );
    
    // Set callback for cancel messages (optional - for logging)
    action_server_->setCancelCallback(
      [this](const robot_actionlib_msgs::GoalIDConstPtr& cancel)
      {
        robot::log_info("[MoveBaseActionServerExample] Cancel callback: id=%s", cancel->id.c_str());
      }
    );
    
    // Set status publish rate (default is 10 Hz)
    action_server_->setStatusPublishRate(10.0);
    
    robot::log_info("[MoveBaseActionServerExample] Callbacks setup complete");
  }
  
  /**
   * @brief Process goal message from external source
   * Call this when you receive a goal message from your communication system
   */
  void processGoalMessage(const robot_move_base_msgs::MoveBaseActionGoalConstPtr& msg)
  {
    if (action_server_)
    {
      action_server_->processGoal(msg);
    }
  }
  
  /**
   * @brief Process cancel message from external source
   * Call this when you receive a cancel message from your communication system
   */
  void processCancelMessage(const robot_actionlib_msgs::GoalIDConstPtr& msg)
  {
    if (action_server_)
    {
      action_server_->processCancel(msg);
    }
  }
  
  /**
   * @brief Execute callback - called when a new goal is received
   * This is the main function that processes the goal
   */
  void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal)
  {
    robot::log_info("[MoveBaseActionServerExample] Execute callback started");
    robot::log_info("  Goal: x=%.2f, y=%.2f, frame_id=%s",
                   goal->target_pose.pose.position.x,
                   goal->target_pose.pose.position.y,
                   goal->target_pose.header.frame_id.c_str());
    
    // Check if we have a valid move_base instance
    if (!move_base_ptr_)
    {
      robot::log_error("[MoveBaseActionServerExample] move_base_ptr_ is null, aborting goal");
      action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(), 
                                 "MoveBase not initialized");
      return;
    }
    
    // Convert goal to PoseStamped
    robot_geometry_msgs::PoseStamped target_pose = goal->target_pose;
    
    // Send goal to move_base
    bool success = move_base_ptr_->moveTo(target_pose, 0.2, 0.2);
    
    if (!success)
    {
      robot::log_error("[MoveBaseActionServerExample] Failed to send goal to move_base");
      action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(),
                                 "Failed to send goal to move_base");
      return;
    }
    
    // Monitor execution and publish feedback
    robot::Rate rate(10.0); // 10 Hz
    robot::Time start_time = robot::Time::now();
    robot::Time last_feedback_time = start_time;
    
    while (action_server_->isActive() && !action_server_->isPreemptRequested())
    {
      // Check if goal is reached
      if (move_base_ptr_->getFeedback())
      {
        auto* feedback_ptr = move_base_ptr_->getFeedback();
        
        if (feedback_ptr->goal_checked && 
            feedback_ptr->navigation_state == robot::move_base_core::State::SUCCEEDED)
        {
          robot::log_info("[MoveBaseActionServerExample] Goal reached!");
          action_server_->setSucceeded(robot_move_base_msgs::MoveBaseResult(),
                                      "Goal reached successfully");
          return;
        }
        
        // Check for errors
        if (feedback_ptr->navigation_state == robot::move_base_core::State::ABORTED)
        {
          robot::log_error("[MoveBaseActionServerExample] Navigation aborted: %s",
                           feedback_ptr->feed_back_str.c_str());
          action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(),
                                    feedback_ptr->feed_back_str);
          return;
        }
        
        // Publish feedback periodically (every 0.5 seconds)
        robot::Time now = robot::Time::now();
        if ((now - last_feedback_time).toSec() >= 0.5)
        {
          robot_move_base_msgs::MoveBaseFeedback feedback;
          feedback.base_position.x = feedback_ptr->current_pose.x;
          feedback.base_position.y = feedback_ptr->current_pose.y;
          feedback.base_position.theta = feedback_ptr->current_pose.theta;
          feedback.base_position.header.stamp = robot::Time::now();
          feedback.base_position.header.frame_id = "map";
          
          action_server_->publishFeedback(feedback);
          last_feedback_time = now;
        }
      }
      
      // Check for preemption
      if (action_server_->isPreemptRequested())
      {
        robot::log_info("[MoveBaseActionServerExample] Goal preempted");
        move_base_ptr_->cancel();
        action_server_->setPreempted(robot_move_base_msgs::MoveBaseResult(),
                                    "Goal preempted by user");
        return;
      }
      
      // Sleep
      rate.sleep();
    }
    
    // If we exit the loop without success, check why
    if (action_server_->isPreemptRequested())
    {
      robot::log_info("[MoveBaseActionServerExample] Goal was preempted");
      move_base_ptr_->cancel();
      action_server_->setPreempted(robot_move_base_msgs::MoveBaseResult(),
                                  "Goal preempted");
    }
    else
    {
      robot::log_warning("[MoveBaseActionServerExample] Goal execution ended unexpectedly");
      action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(),
                                "Goal execution ended unexpectedly");
    }
  }
  
  /**
   * @brief Get the action server instance
   */
  std::shared_ptr<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>> getActionServer()
  {
    return action_server_;
  }
  
  /**
   * @brief Set move_base instance
   */
  void setMoveBase(robot::move_base_core::BaseNavigation::Ptr move_base)
  {
    move_base_ptr_ = move_base;
  }

private:
  // Action server
  std::shared_ptr<SimpleActionServer<robot_move_base_msgs::MoveBaseAction>> action_server_;
  
  // Move base instance
  robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
};

} // namespace move_base

/**
 * @brief Example main function showing how to use MoveBaseActionServerExample
 */
int main()
{
  try
  {
    // Create example instance
    move_base::MoveBaseActionServerExample example;
    
    // If you have a move_base instance, set it
    // example.setMoveBase(move_base_ptr);
    
    robot::log_info("[Example] Action server is running. Waiting for goals...");
    
    // Example: Create and send a goal message
    // In real usage, you would receive this from your communication system
    robot_move_base_msgs::MoveBaseActionGoalPtr goal_msg = 
        boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
    goal_msg->header.stamp = robot::Time::now();
    goal_msg->header.frame_id = "map";
    goal_msg->goal_id.stamp = robot::Time::now();
    goal_msg->goal_id.id = "example_goal_1";
    goal_msg->goal.target_pose.header.stamp = robot::Time::now();
    goal_msg->goal.target_pose.header.frame_id = "map";
    goal_msg->goal.target_pose.pose.position.x = 5.0;
    goal_msg->goal.target_pose.pose.position.y = 3.0;
    goal_msg->goal.target_pose.pose.position.z = 0.0;
    goal_msg->goal.target_pose.pose.orientation.w = 1.0;
    
    // Process the goal
    example.processGoalMessage(goal_msg);
    
    // Your event loop here
    // while (your_system_running())
    // {
    //   // Receive messages from your communication system
    //   // Process goals and cancels
    //   // example.processGoalMessage(goal_msg);
    //   // example.processCancelMessage(cancel_msg);
    // }
    
    // Keep running for a while
    robot::Time::sleep(robot::Duration(10.0));
  }
  catch (const std::exception& e)
  {
    robot::log_error("[Example] Exception: %s", e.what());
    return 1;
  }
  
  return 0;
}

