// Copyright 2024 RobosoftAI Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <smacc2/client_core_components/cp_action_client.hpp>
#include <smacc2/component.hpp>
#include <smacc2/smacc_signal.hpp>

#include <functional>
#include <future>

namespace cl_nav2z
{
namespace components
{

class CpNav2ActionInterface : public smacc2::ISmaccComponent
{
public:
  // Type aliases for convenience
  using ActionType = nav2_msgs::action::NavigateToPose;
  using Goal = ActionType::Goal;
  using Result = ActionType::Result;
  using Feedback = ActionType::Feedback;
  using GoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
  using WrappedResult = typename GoalHandle::WrappedResult;
  using ActionClient = smacc2::client_core_components::CpActionClient<ActionType>;

  // Nav2-specific signals for external subscribers
  smacc2::SmaccSignal<void(const WrappedResult &)> onNavigationSucceeded_;
  smacc2::SmaccSignal<void(const WrappedResult &)> onNavigationAborted_;
  smacc2::SmaccSignal<void(const WrappedResult &)> onNavigationCancelled_;
  smacc2::SmaccSignal<void(const Feedback &)> onNavigationFeedback_;

  // Nav2-specific event posting functions (set during orthogonal allocation)
  std::function<void(const WrappedResult &)> postNavigationSuccessEvent;
  std::function<void(const WrappedResult &)> postNavigationAbortedEvent;
  std::function<void(const WrappedResult &)> postNavigationCancelledEvent;
  std::function<void(const Feedback &)> postNavigationFeedbackEvent;

  CpNav2ActionInterface() = default;
  virtual ~CpNav2ActionInterface() = default;

  // Public API for behaviors and other components
  std::shared_future<typename GoalHandle::SharedPtr> sendNavigationGoal(
    const geometry_msgs::msg::PoseStamped & target)
  {
    if (!actionClient_)
    {
      RCLCPP_ERROR(getLogger(), "[CpNav2ActionInterface] Action client not available!");
      throw std::runtime_error("Action client not initialized");
    }

    auto goal = createNavigationGoal(target);

    RCLCPP_INFO_STREAM(
      getLogger(), "[CpNav2ActionInterface] Sending navigation goal to: "
                     << "x=" << target.pose.position.x << ", y=" << target.pose.position.y
                     << ", frame=" << target.header.frame_id);

    return actionClient_->sendGoal(goal);
  }

  std::shared_future<typename GoalHandle::SharedPtr> sendGoal(Goal & goal)
  {
    if (!actionClient_)
    {
      RCLCPP_ERROR(getLogger(), "[CpNav2ActionInterface] Action client not available!");
      throw std::runtime_error("Action client not initialized");
    }

    return actionClient_->sendGoal(goal);
  }

  bool cancelNavigation()
  {
    if (!actionClient_)
    {
      RCLCPP_WARN(getLogger(), "[CpNav2ActionInterface] Action client not available for cancel!");
      return false;
    }

    RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Cancelling navigation goal");
    return actionClient_->cancelGoal();
  }

  bool isNavigationServerReady() const { return actionClient_ && actionClient_->isServerReady(); }

  void waitForNavigationServer()
  {
    if (actionClient_)
    {
      actionClient_->waitForServer();
    }
  }

  // Component lifecycle
  template <typename TOrthogonal, typename TSourceObject>
  void onComponentInitialization()
  {
    // Require the underlying action client component
    this->requiresComponent(actionClient_);

    // Set up nav2-specific event posting functions
    postNavigationSuccessEvent = [this](const WrappedResult & result)
    {
      auto * ev = new smacc2::default_events::EvActionSucceeded<TSourceObject, TOrthogonal>();
      this->postEvent(ev);
    };

    postNavigationAbortedEvent = [this](const WrappedResult & result)
    {
      auto * ev = new smacc2::default_events::EvActionAborted<TSourceObject, TOrthogonal>();
      this->postEvent(ev);
    };

    postNavigationCancelledEvent = [this](const WrappedResult & result)
    {
      auto * ev = new smacc2::default_events::EvActionCancelled<TSourceObject, TOrthogonal>();
      this->postEvent(ev);
    };

    postNavigationFeedbackEvent = [this](const Feedback & feedback)
    {
      auto * ev = new smacc2::default_events::EvActionFeedback<Feedback, TOrthogonal>();
      ev->feedbackMessage = feedback;
      this->postEvent(ev);
    };

    RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Component initialized");
  }

  void onInitialize() override
  {
    // Wire up signal connections from action client to nav2-specific signals
    if (actionClient_)
    {
      // Connect action client signals to our nav2-specific signals and event posting using method pointers
      actionClient_->onSucceeded(&CpNav2ActionInterface::onNavigationSuccess, this);
      actionClient_->onAborted(&CpNav2ActionInterface::onNavigationAborted, this);
      actionClient_->onCancelled(&CpNav2ActionInterface::onNavigationCancelled, this);
      actionClient_->onFeedback(&CpNav2ActionInterface::onNavigationFeedback, this);

      RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Signal connections established");
    }
    else
    {
      RCLCPP_ERROR(
        getLogger(), "[CpNav2ActionInterface] Action client not found during initialization!");
    }
  }

  // Signal connection methods for external subscribers
  template <typename T>
  boost::signals2::connection onNavigationSucceeded(
    void (T::*callback)(const WrappedResult &), T * object)
  {
    return this->getStateMachine()->createSignalConnection(
      onNavigationSucceeded_, callback, object);
  }

  template <typename T>
  boost::signals2::connection onNavigationAborted(
    void (T::*callback)(const WrappedResult &), T * object)
  {
    return this->getStateMachine()->createSignalConnection(onNavigationAborted_, callback, object);
  }

  template <typename T>
  boost::signals2::connection onNavigationCancelled(
    void (T::*callback)(const WrappedResult &), T * object)
  {
    return this->getStateMachine()->createSignalConnection(
      onNavigationCancelled_, callback, object);
  }

  template <typename T>
  boost::signals2::connection onNavigationFeedback(
    void (T::*callback)(const Feedback &), T * object)
  {
    return this->getStateMachine()->createSignalConnection(onNavigationFeedback_, callback, object);
  }

  // Access to underlying action client for advanced usage
  ActionClient * getActionClient() const { return actionClient_; }

private:
  ActionClient * actionClient_ = nullptr;

  // Event translation callbacks
  void onNavigationSuccess(const WrappedResult & result)
  {
    RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Navigation succeeded");
    onNavigationSucceeded_(result);
    if (postNavigationSuccessEvent)
    {
      postNavigationSuccessEvent(result);
    }
  }

  void onNavigationAborted(const WrappedResult & result)
  {
    RCLCPP_WARN(getLogger(), "[CpNav2ActionInterface] Navigation aborted");
    onNavigationAborted_(result);
    if (postNavigationAbortedEvent)
    {
      postNavigationAbortedEvent(result);
    }
  }

  void onNavigationCancelled(const WrappedResult & result)
  {
    RCLCPP_INFO(getLogger(), "[CpNav2ActionInterface] Navigation cancelled");
    onNavigationCancelled_(result);
    if (postNavigationCancelledEvent)
    {
      postNavigationCancelledEvent(result);
    }
  }

  void onNavigationFeedback(const Feedback & feedback)
  {
    RCLCPP_DEBUG(getLogger(), "[CpNav2ActionInterface] Navigation feedback received");
    onNavigationFeedback_(feedback);
    if (postNavigationFeedbackEvent)
    {
      postNavigationFeedbackEvent(feedback);
    }
  }

  // Nav2-specific goal construction
  Goal createNavigationGoal(const geometry_msgs::msg::PoseStamped & target)
  {
    Goal goal;
    goal.pose = target;

    // Set default behavior_tree if not specified
    if (goal.behavior_tree.empty())
    {
      goal.behavior_tree = "";  // Let Nav2 use default behavior tree
    }

    return goal;
  }
};

}  // namespace components
}  // namespace cl_nav2z
