#include <move_base/simple_action_server.h>

namespace move_base
{

// Constructor
template<class ActionSpec>
SimpleActionServer<ActionSpec>::SimpleActionServer(const std::string& name,
                                                   ExecuteCallback execute_callback,
                                                   bool auto_start)
  : name_(name),
    execute_callback_(execute_callback),
    active_(false),
    preempt_requested_(false),
    new_goal_available_(false),
    goal_(nullptr),
    status_publish_rate_(10.0)
{
  // Initialize status
  current_status_.status = robot_actionlib_msgs::GoalStatus::PENDING;
  current_status_.goal_id.stamp = robot::Time::now();
  current_status_.goal_id.id = "";
  current_status_.text = "";
  
  if (auto_start)
  {
    start();
  }
  
  robot::log_info("[SimpleActionServer] Initialized action server: %s", name_.c_str());
}

// Destructor
template<class ActionSpec>
SimpleActionServer<ActionSpec>::~SimpleActionServer()
{
  stop();
}

// Start the action server
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::start()
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  if (!active_)
  {
    active_ = true;
    // Start status publishing thread
    status_thread_ = boost::thread(boost::bind(&SimpleActionServer::statusPublishThread, this));
    robot::log_info("[SimpleActionServer] Started action server: %s", name_.c_str());
  }
}

// Stop the action server
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::stop()
{
  {
    boost::lock_guard<boost::mutex> lock(state_mutex_);
    if (active_)
    {
      active_ = false;
    }
  }
  
  // Notify all waiting threads
  goal_condition_.notify_all();
  
  if (status_thread_.joinable())
  {
    status_thread_.join();
  }
  
  if (execute_thread_.joinable())
  {
    execute_thread_.join();
  }
  
  robot::log_info("[SimpleActionServer] Stopped action server: %s", name_.c_str());
}

// Set callback for goal messages
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::setGoalCallback(GoalCallback callback)
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  goal_callback_ = callback;
}

// Set callback for cancel messages
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::setCancelCallback(CancelCallback callback)
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  cancel_callback_ = callback;
}

// Set callback for feedback publishing
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::setFeedbackCallback(FeedbackCallback callback)
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  feedback_callback_ = callback;
}

// Set callback for result publishing
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::setResultCallback(ResultCallback callback)
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  result_callback_ = callback;
}

// Set callback for status publishing
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::setStatusCallback(StatusCallback callback)
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  status_callback_ = callback;
}

// Set status publish rate
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::setStatusPublishRate(double rate)
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  status_publish_rate_ = rate;
}

// Process an incoming goal message
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::processGoal(const ActionGoalConstPtr& goal_msg)
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  
  if (!active_)
  {
    robot::log_warning("[SimpleActionServer] Received goal but server is not active");
    return;
  }
  
  // Check if we should accept this goal
  bool accept_goal = false;
  
  if (current_status_.status == robot_actionlib_msgs::GoalStatus::PENDING ||
      current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE ||
      current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING)
  {
    // Check if this is a new goal (different ID)
    if (goal_msg->goal_id.id != current_goal_id_.id || 
        goal_msg->goal_id.stamp != current_goal_id_.stamp)
    {
      // If we have an active goal, preempt it
      if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE)
      {
        preempt_requested_ = true;
        current_status_.status = robot_actionlib_msgs::GoalStatus::PREEMPTING;
        robot::log_info("[SimpleActionServer] Preempting current goal for new goal");
      }
      
      accept_goal = true;
    }
  }
  else
  {
    // No active goal, accept this one
    accept_goal = true;
  }
  
  if (accept_goal)
  {
    // Store the new goal
    goal_ = boost::make_shared<Goal>(goal_msg->goal);
    current_goal_id_ = goal_msg->goal_id;
    new_goal_available_ = true;
    current_status_.status = robot_actionlib_msgs::GoalStatus::PENDING;
    current_status_.goal_id = current_goal_id_;
    current_status_.text = "";
    
    robot::log_info("[SimpleActionServer] Received new goal, ID: %s", current_goal_id_.id.c_str());
    
    // Notify waiting threads
    goal_condition_.notify_one();
    
    // Start execute thread if not already running
    if (execute_callback_)
    {
      if (!execute_thread_.joinable())
      {
        execute_thread_ = boost::thread(boost::bind(&SimpleActionServer::executeGoal, this));
      }
    }
    
    // Call goal callback if set
    if (goal_callback_)
    {
      goal_callback_(goal_msg);
    }
  }
}

// Process an incoming cancel message
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::processCancel(const robot_actionlib_msgs::GoalIDConstPtr& cancel_msg)
{
  robot::log_info("[SimpleActionServer::processCancel] ===== ENTRY =====");
  robot::log_info("[SimpleActionServer::processCancel] Cancel message ID: '%s'", cancel_msg->id.c_str());
  robot::log_info("[SimpleActionServer::processCancel] Cancel message stamp: %ld.%09ld", 
                  cancel_msg->stamp.sec, cancel_msg->stamp.nsec);
  
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  
  robot::log_info("[SimpleActionServer::processCancel] Current goal ID: '%s'", current_goal_id_.id.c_str());
  robot::log_info("[SimpleActionServer::processCancel] Current goal stamp: %ld.%09ld", 
                  current_goal_id_.stamp.sec, current_goal_id_.stamp.nsec);
  robot::log_info("[SimpleActionServer::processCancel] Current status: %d", current_status_.status);
  
  // Check if this cancel request is for the current goal
  bool should_cancel = false;
  bool is_empty_id = (cancel_msg->id == "");
  bool is_matching_id = (cancel_msg->id == current_goal_id_.id);
  bool is_matching_stamp = (cancel_msg->stamp == current_goal_id_.stamp);
  
  robot::log_debug("[SimpleActionServer::processCancel] Cancel check: empty_id=%s, matching_id=%s, matching_stamp=%s",
                   is_empty_id ? "true" : "false",
                   is_matching_id ? "true" : "false",
                   is_matching_stamp ? "true" : "false");
  
  if (is_empty_id || (is_matching_id && is_matching_stamp))
  {
    should_cancel = true;
    robot::log_info("[SimpleActionServer::processCancel] Cancel request matches current goal - will cancel");
    
    if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE)
    {
      robot::log_info("[SimpleActionServer::processCancel] Goal is ACTIVE - setting preemption");
      preempt_requested_ = true;
      current_status_.status = robot_actionlib_msgs::GoalStatus::PREEMPTING;
      robot::log_info("[SimpleActionServer::processCancel] Cancel requested for goal ID: %s", current_goal_id_.id.c_str());
      robot::log_info("[SimpleActionServer::processCancel] Status changed to PREEMPTING");
    }
    else if (current_status_.status == robot_actionlib_msgs::GoalStatus::PENDING)
    {
      robot::log_info("[SimpleActionServer::processCancel] Goal is PENDING - rejecting it");
      // Goal hasn't started yet, just reject it
      current_status_.status = robot_actionlib_msgs::GoalStatus::REJECTED;
      current_status_.text = "Goal cancelled before execution";
      goal_.reset();
      new_goal_available_ = false;
      robot::log_info("[SimpleActionServer::processCancel] Goal cancelled before execution");
      robot::log_info("[SimpleActionServer::processCancel] Status changed to REJECTED");
    }
    else
    {
      robot::log_info("[SimpleActionServer::processCancel] Goal status is %d - no action needed", current_status_.status);
    }
  }
  else
  {
    robot::log_info("[SimpleActionServer::processCancel] Cancel request does NOT match current goal - ignoring");
    robot::log_info("[SimpleActionServer::processCancel] Cancel ID '%s' != Current ID '%s'", 
                    cancel_msg->id.c_str(), current_goal_id_.id.c_str());
  }
  
  // Call cancel callback if set
  robot::log_debug("[SimpleActionServer::processCancel] Checking cancel_callback_: %s", 
                   cancel_callback_ ? "set" : "null");
  if (cancel_callback_)
  {
    robot::log_debug("[SimpleActionServer::processCancel] Calling cancel_callback_...");
    try
    {
      cancel_callback_(cancel_msg);
      robot::log_debug("[SimpleActionServer::processCancel] cancel_callback_ executed successfully");
    }
    catch (const std::exception& e)
    {
      robot::log_error("[SimpleActionServer::processCancel] EXCEPTION in cancel_callback_: %s", e.what());
    }
  }
  
  robot::log_info("[SimpleActionServer::processCancel] ===== EXIT =====");
}

// Check if a new goal is available
template<class ActionSpec>
bool SimpleActionServer<ActionSpec>::isNewGoalAvailable()
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  return new_goal_available_;
}

// Check if preemption has been requested
template<class ActionSpec>
bool SimpleActionServer<ActionSpec>::isPreemptRequested()
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  return preempt_requested_;
}

// Accept a new goal
template<class ActionSpec>
typename SimpleActionServer<ActionSpec>::GoalConstPtr SimpleActionServer<ActionSpec>::acceptNewGoal()
{
  robot::log_info("[SimpleActionServer::acceptNewGoal] ===== ENTRY =====");
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  
  robot::log_debug("[SimpleActionServer::acceptNewGoal] new_goal_available_: %s", 
                   new_goal_available_ ? "true" : "false");
  robot::log_debug("[SimpleActionServer::acceptNewGoal] goal_ is %s", 
                   goal_ ? "valid" : "null");
  
  if (!new_goal_available_)
  {
    robot::log_warning("[SimpleActionServer::acceptNewGoal] No new goal available - returning null");
    robot::log_info("[SimpleActionServer::acceptNewGoal] ===== EXIT (null) =====");
    return GoalConstPtr();
  }
  
  // Accept the new goal
  GoalConstPtr new_goal = goal_;
  goal_.reset();
  new_goal_available_ = false;
  preempt_requested_ = false;
  
  // Update status
  current_status_.status = robot_actionlib_msgs::GoalStatus::ACTIVE;
  current_status_.goal_id = current_goal_id_;
  current_status_.text = "";
  
  robot::log_info("[SimpleActionServer::acceptNewGoal] Goal accepted successfully");
  robot::log_info("[SimpleActionServer::acceptNewGoal] Goal ID: %s", current_goal_id_.id.c_str());
  robot::log_info("[SimpleActionServer::acceptNewGoal] Status changed to ACTIVE");
  if (new_goal)
  {
    robot::log_info("[SimpleActionServer::acceptNewGoal] Goal position: x=%.3f, y=%.3f", 
                    new_goal->target_pose.pose.position.x,
                    new_goal->target_pose.pose.position.y);
  }
  robot::log_info("[SimpleActionServer::acceptNewGoal] ===== EXIT (success) =====");
  return new_goal;
}

// Set the action as succeeded
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::setSucceeded(const Result& result, const std::string& text)
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  
  if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE ||
      current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING)
  {
    current_status_.status = robot_actionlib_msgs::GoalStatus::SUCCEEDED;
    current_status_.text = text;
    
    // Create and publish result
    ActionResultPtr action_result = boost::make_shared<ActionResult>();
    action_result->header.stamp = robot::Time::now();
    action_result->status = current_status_;
    action_result->result = result;
    
    // Call result callback if set
    if (result_callback_)
    {
      result_callback_(action_result);
    }
    
    // Clear goal
    goal_.reset();
    new_goal_available_ = false;
    preempt_requested_ = false;
    
    robot::log_info("[SimpleActionServer] Goal succeeded: %s", text.c_str());
  }
}

// Set the action as aborted
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::setAborted(const Result& result, const std::string& text)
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  
  if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE ||
      current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING)
  {
    current_status_.status = robot_actionlib_msgs::GoalStatus::ABORTED;
    current_status_.text = text;
    
    // Create and publish result
    ActionResultPtr action_result = boost::make_shared<ActionResult>();
    action_result->header.stamp = robot::Time::now();
    action_result->status = current_status_;
    action_result->result = result;
    
    // Call result callback if set
    if (result_callback_)
    {
      result_callback_(action_result);
    }
    
    // Clear goal
    goal_.reset();
    new_goal_available_ = false;
    preempt_requested_ = false;
    
    robot::log_warning("[SimpleActionServer] Goal aborted: %s", text.c_str());
  }
}

// Set the action as preempted
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::setPreempted(const Result& result, const std::string& text)
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  
  if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE ||
      current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING)
  {
    current_status_.status = robot_actionlib_msgs::GoalStatus::PREEMPTED;
    current_status_.text = text;
    
    // Create and publish result
    ActionResultPtr action_result = boost::make_shared<ActionResult>();
    action_result->header.stamp = robot::Time::now();
    action_result->status = current_status_;
    action_result->result = result;
    
    // Call result callback if set
    if (result_callback_)
    {
      result_callback_(action_result);
    }
    
    // Clear goal
    goal_.reset();
    new_goal_available_ = false;
    preempt_requested_ = false;
    
    robot::log_info("[SimpleActionServer] Goal preempted: %s", text.c_str());
  }
}

// Publish feedback
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::publishFeedback(const Feedback& feedback)
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  
  if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE)
  {
    ActionFeedbackPtr action_feedback = boost::make_shared<ActionFeedback>();
    action_feedback->header.stamp = robot::Time::now();
    action_feedback->status = current_status_;
    action_feedback->feedback = feedback;
    
    // Call feedback callback if set
    if (feedback_callback_)
    {
      feedback_callback_(action_feedback);
    }
  }
}

// Get the current goal
template<class ActionSpec>
typename SimpleActionServer<ActionSpec>::GoalConstPtr SimpleActionServer<ActionSpec>::getCurrentGoal()
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  return goal_;
}

// Get the current status
template<class ActionSpec>
robot_actionlib_msgs::GoalStatus SimpleActionServer<ActionSpec>::getCurrentStatus()
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  return current_status_;
}

// Check if the server is active
template<class ActionSpec>
bool SimpleActionServer<ActionSpec>::isActive()
{
  boost::lock_guard<boost::mutex> lock(state_mutex_);
  return active_;
}

// Get the action name
template<class ActionSpec>
std::string SimpleActionServer<ActionSpec>::getName() const
{
  return name_;
}

// Execute the goal in a separate thread
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::executeGoal()
{
  robot::log_info("[SimpleActionServer::executeGoal] ===== THREAD STARTED =====");
  robot::log_info("[SimpleActionServer::executeGoal] Action server name: %s", name_.c_str());
  robot::log_debug("[SimpleActionServer::executeGoal] execute_callback_ is %s", 
                   execute_callback_ ? "set" : "null");
  
  // Continuously process goals
  while (active_)
  {
    robot::log_debug("[SimpleActionServer::executeGoal] Waiting for new goal...");
    GoalConstPtr goal_to_execute;
    
    // Wait for a new goal to be available
    {
      boost::unique_lock<boost::mutex> lock(state_mutex_);
      robot::log_debug("[SimpleActionServer::executeGoal] Mutex lock acquired in executeGoal");
      robot::log_debug("[SimpleActionServer::executeGoal] active_=%s, new_goal_available_=%s", 
                       active_ ? "true" : "false", 
                       new_goal_available_ ? "true" : "false");
      
      // Wait until a new goal is available or server is stopped
      while (active_ && !new_goal_available_)
      {
        robot::log_debug("[SimpleActionServer::executeGoal] Waiting on condition variable...");
        goal_condition_.wait(lock);
        robot::log_debug("[SimpleActionServer::executeGoal] Woke up from condition wait");
        robot::log_debug("[SimpleActionServer::executeGoal] After wake: active_=%s, new_goal_available_=%s", 
                         active_ ? "true" : "false", 
                         new_goal_available_ ? "true" : "false");
      }
      
      if (!active_)
      {
        robot::log_info("[SimpleActionServer::executeGoal] Server is not active - exiting thread");
        break;
      }
      
      // Accept the new goal
      if (new_goal_available_)
      {
        robot::log_info("[SimpleActionServer::executeGoal] New goal available - accepting it...");
        
        // Release lock before calling acceptNewGoal() to avoid deadlock
        // (acceptNewGoal() will acquire its own lock)
        lock.unlock();
        
        goal_to_execute = acceptNewGoal();
        
        robot::log_info("[SimpleActionServer::executeGoal] Goal accepted: %s", 
                        goal_to_execute ? "valid" : "null");
      }
      else
      {
        robot::log_warning("[SimpleActionServer::executeGoal] Woke up but new_goal_available_ is false!");
        lock.unlock();
      }
    }
    
    // Execute the goal callback
    if (goal_to_execute)
    {
      robot::log_info("[SimpleActionServer::executeGoal] Executing goal callback...");
      robot::log_info("[SimpleActionServer::executeGoal] Goal position: x=%.3f, y=%.3f", 
                      goal_to_execute->target_pose.pose.position.x,
                      goal_to_execute->target_pose.pose.position.y);
      
      if (execute_callback_)
      {
        try
        {
          robot::log_info("[SimpleActionServer::executeGoal] Calling execute_callback_...");
          execute_callback_(goal_to_execute);
          robot::log_info("[SimpleActionServer::executeGoal] execute_callback_ completed");
        }
        catch (const std::exception& e)
        {
          robot::log_error("[SimpleActionServer::executeGoal] EXCEPTION in execute_callback_: %s", e.what());
        }
      }
      else
      {
        robot::log_error("[SimpleActionServer::executeGoal] execute_callback_ is null - cannot execute goal!");
      }
    }
    else
    {
      robot::log_warning("[SimpleActionServer::executeGoal] goal_to_execute is null - skipping execution");
    }
  }
  
  robot::log_info("[SimpleActionServer::executeGoal] ===== THREAD EXITING =====");
}

// Thread function to publish status periodically
template<class ActionSpec>
void SimpleActionServer<ActionSpec>::statusPublishThread()
{
  double rate = status_publish_rate_;
  robot::Duration sleep_duration(1.0 / rate);
  
  while (active_)
  {
    {
      boost::lock_guard<boost::mutex> lock(state_mutex_);
      
      // Create status array
      robot_actionlib_msgs::GoalStatusArrayPtr status_array = 
          boost::make_shared<robot_actionlib_msgs::GoalStatusArray>();
      status_array->header.stamp = robot::Time::now();
      status_array->status_list.clear();
      
      if (current_status_.status != robot_actionlib_msgs::GoalStatus::PENDING ||
          !current_goal_id_.id.empty())
      {
        status_array->status_list.push_back(current_status_);
      }
      
      // Call status callback if set
      if (status_callback_)
      {
        status_callback_(status_array);
      }
    }
    
    // Sleep
    robot::Time::sleepUntil(robot::Time::now() + sleep_duration);
  }
}

} // namespace move_base

// Explicit template instantiation for MoveBaseAction
// This is required because template method definitions are in .cpp file
template class move_base::SimpleActionServer<robot_move_base_msgs::MoveBaseAction>;

