#pragma once

#include "toid_behaviors/simple_move.hpp"
#include "toid_behaviors/scl.hpp"
#include "toid_msgs/action/simple_move_coords.hpp"

#include "geometry_msgs/msg/pose.hpp"

namespace toid
{
using MoveAction = toid_msgs::action::SimpleMoveCoords;
using namespace nav2_behaviors;

class MoveCoords : public SimpleMove<MoveAction>
{
public:
  MoveCoords();
  ~MoveCoords();

  void configureCB() override;

  ResultStatus onStart(
    const std::shared_ptr<const MoveAction::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:

  SmoothControlLaw scl;

  //Goal
  geometry_msgs::msg::Pose target_pose_;
  double target_angle_;
  double target_sign_;
  bool backwards_;

  //State
  double last_speed_;

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

  double max_angular_speed_;

  double kp_;
  double k_phi_;
  double k_delta_;
  double beta_;
  double lambda_;
  double slowdown_radius_;
};

}  // namespace toid

