#pragma once

#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/twist.hpp"

namespace toid
{
class SmoothControlLaw
{
public:
  double k_phi = 5.0;
  double k_delta = 2.0;
  double bbeta = 0.4;
  double lam = 2.0;

  double slowdown_radius = 0.20;
  double v_linear_min = 0.05;
  double v_linear_max = 0.1;
  double v_angular_max = 2.0;

  void egocentric_polar(
    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
    double & r, double & phi, double & delta);

  double curvature(double r, double phi, double delta);

  void calculate_vel(
    const geometry_msgs::msg::Pose & target,const geometry_msgs::msg::Pose & current,
    geometry_msgs::msg::Twist & out_speed, bool backwards = false);
};
}  // namespace toid