#pragma once

#include "toid_behaviors/simple_move.hpp"
#include "toid_msgs/action/simple_rotate.h"

namespace toid
{
using RotateAction = toid_msgs::action::SimpleRotate;
using namespace nav2_behaviors;

class SimpleRotateBehavior : public SimpleMove<RotateAction>
{
public:
  SimpleRotateBehavior();
  ~SimpleRotateBehavior();

  void configureCB() override;

  ResultStatus onStart(
    const std::shared_ptr<const RotateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
    const geometry_msgs::msg::Twist & vel) override;

  ResultStatus updateVel(
    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
    geometry_msgs::msg::Twist & out_vel) override;
  
  virtual nav2_core::CostmapInfoType getResourceInfo() override {
    return nav2_core::CostmapInfoType::NONE;
  }

protected:

  //Goal
  double target_angle_;
  double min_turn_angle_;
  double initial_direction_;

  //State
  double angular_speed_;
  double last_angle_;
  double last_time_;

  //Config
  double max_angular_speed_;
  double min_angular_speed_;
  double max_angular_accel_;
  double max_angular_decel_;
};

}  // namespace toid
