#pragma once

#include "toid_behaviors/simple_move.hpp"
#include "toid_msgs/action/simple_translate_x.hpp"

namespace toid
{
using TranslateAction = toid_msgs::action::SimpleTranslateX;
using namespace nav2_behaviors;

class SimpleTranslateXBehavior : public SimpleMove<TranslateAction>
{
public:
  SimpleTranslateXBehavior();
  ~SimpleTranslateXBehavior();

  void configureCB() override;

  ResultStatus onStart(
    const std::shared_ptr<const TranslateAction::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_distance_;
  double target_angle_;
  double target_sign_;
  geometry_msgs::msg::Pose initial_pose_;

  //State
  double speed_;

  //Config
  double max_vel_accel_;
  double max_vel_decel_;
  double max_vel_speed_;
  double min_vel_speed_;
  double max_angular_speed_;

  double kp_;
};

}  // namespace toid

