#pragma once
#include "nav2_behaviors/timed_behavior.hpp"
#include "nav2_core/behavior.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/twist_publisher.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "rclcpp/rclcpp.hpp"
#include "toid_msgs/action/simple_rotate.hpp"

namespace toid
{

template <typename ActionT>
class SimpleMove : public nav2_behaviors::TimedBehavior<ActionT>
{
public:
  virtual void configureCB() {}
  virtual void cleanupCB() {}

  virtual void activateCB() {}
  virtual void deactivateCB() {}

  virtual nav2_behaviors::ResultStatus onStart(
    const std::shared_ptr<const typename ActionT::Goal> command,
    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel) = 0;

  virtual nav2_behaviors::ResultStatus updateVel(
    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
    geometry_msgs::msg::Twist & out_vel) = 0;

  void onConfigure() override
  {
    rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock();
    nav2_util::declare_parameter_if_not_declared(
      node, "odom_topic", rclcpp::ParameterValue("/odom"));
    std::string odom_topic_name = node->get_parameter("odom_topic").as_string();
    odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
      odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) {
        std::lock_guard lock(mut_);
        current_vel_ = msg.twist.twist;
      });
    control_duration_ = 1.0 / this->cycle_frequency_;
    configureCB();
  }

  void onCleanup() override
  {
    odom_sub_.reset();
    cleanupCB();
  }

  void activate() override
  {
    nav2_behaviors::TimedBehavior<ActionT>::activate();
    activateCB();
  }

  void deactivate() override
  {
    nav2_behaviors::TimedBehavior<ActionT>::deactivate();
    deactivateCB();
  }

  nav2_behaviors::ResultStatus onRun(
    const std::shared_ptr<const typename ActionT::Goal> command) override
  {
    geometry_msgs::msg::PoseStamped pose;
    geometry_msgs::msg::Twist vel;
    {
      std::lock_guard lock(mut_);
      vel = current_vel_;
    }

    if (!nav2_util::getCurrentPose(
          pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
          this->transform_tolerance_)) {
      return nav2_behaviors::ResultStatus{nav2_behaviors::Status::FAILED};
    }

    return onStart(command, pose.pose, vel);
  }

  nav2_behaviors::ResultStatus onCycleUpdate() override
  {
    geometry_msgs::msg::PoseStamped pose;
    geometry_msgs::msg::Twist vel;
    geometry_msgs::msg::Twist out_vel;
    auto vel_p = std::make_unique<geometry_msgs::msg::TwistStamped>();
    {
      std::lock_guard lock(mut_);
      vel = current_vel_;
    }

    if (!nav2_util::getCurrentPose(
          pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
          this->transform_tolerance_)) {
      return nav2_behaviors::ResultStatus{nav2_behaviors::Status::FAILED};
    }

    nav2_behaviors::ResultStatus r = updateVel(pose.pose, vel, out_vel);
    vel_p->twist = out_vel;
    vel_p->header.stamp = this->clock_->now();
    vel_p->header.frame_id = this->robot_base_frame_;
    this->vel_pub_->publish(std::move(vel_p));
    return r;
  }

protected:
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
  geometry_msgs::msg::Pose current_pose_;
  geometry_msgs::msg::Twist current_vel_;
  std::recursive_mutex mut_;
  double control_duration_;
};

}  // namespace toid