#pragma once

#include "angles/angles.h"
#include "behaviortree_ros2/bt_service_node.hpp"
#include "tf2/utils.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "toid_bt/plugin.hpp"
#include "toid_msgs/srv/send_double.hpp"

namespace toid
{

class EndCalibNode : public BT::RosServiceNode<toid_msgs::srv::SendDouble>
{
public:
  EndCalibNode(
    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
    PoseFunc pose)
  : BT::RosServiceNode<toid_msgs::srv::SendDouble>(name, conf, params), get_pose(pose)
  {
  }

  static BT::PortsList providedPorts()
  {
    return providedBasicPorts({
      BT::InputPort<double>("width"),
      BT::InputPort<int>("count", 1, {}),
      BT::OutputPort<double>("new_width"),
      //BT::InputPort<double>("options"),
    });
  }

  bool setRequest(typename Request::SharedPtr & request) override
  {
    auto width = getInput<double>("width").value();
    auto count = getInput<double>("count").value();

    geometry_msgs::msg::PoseStamped pose;
    get_pose(pose);

    double theta = tf2::getYaw(pose.pose.orientation);
    double new_width = width * (1 + (theta / (2 * M_PI * count)));
    request->data = new_width;

    setOutput("new_width", new_width);
    return true;
  }

  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
  {
    return BT::NodeStatus::SUCCESS;
  }

  PoseFunc get_pose;
};

}  // namespace toid