#include "rclcpp/rclcpp.hpp"
#include "autoware_adapi_v1_msgs/srv/set_route_points.hpp"
#include "geometry_msgs/msg/pose.hpp"

using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints;

void call_change_route_points(
  rclcpp::Node::SharedPtr node,
  const geometry_msgs::msg::Pose & goal,
  const std::vector<geometry_msgs::msg::Pose> & waypoints)
{
  auto client = node->create_client<SetRoutePoints>(
    "/planning/mission_planning/change_route_points");

  if (!client->wait_for_service(std::chrono::seconds(3))) {
    RCLCPP_ERROR(node->get_logger(), "Service not available: change_route_points");
    return;
  }

  auto request = std::make_shared<SetRoutePoints::Request>();
  request->goal = goal;
  request->waypoints = waypoints;

  auto future = client->async_send_request(request);

  auto result_code = rclcpp::spin_until_future_complete(node, future);
  if (result_code == rclcpp::FutureReturnCode::SUCCESS) {
    const auto & status = future.get()->status;
    if (status.code == autoware_adapi_v1_msgs::msg::ResponseStatus::SUCCESS) {
      RCLCPP_INFO(node->get_logger(), "Route changed successfully.");
    } else {
      RCLCPP_ERROR(
        node->get_logger(),
        "Failed to change route: code=%d, message=\"%s\"",
        status.code, status.message.c_str());
    }
  } else {
    RCLCPP_ERROR(node->get_logger(), "Service call failed (code: %d)", result_code);
  }
}

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("change_route_client");

  geometry_msgs::msg::Pose goal;
  // goal.position と goal.orientation に適切な値を設定してください

  std::vector<geometry_msgs::msg::Pose> waypoints;
  // waypoints に中間地点となる pose を複数設定してください

  call_change_route_points(node, goal, waypoints);

  rclcpp::shutdown();
  return 0;
}