#pragma once

#include <atomic>
#include <memory>
#include <mutex>
#include <string>
#include <vector>

#include "geometry_msgs/msg/pose.hpp"
#include "interfaces/action/dual_arm.hpp"
#include "interfaces/msg/arm_motion_params.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/robot_state/conversions.h"
#include "moveit/robot_trajectory/robot_trajectory.h"
#include "moveit/trajectory_processing/iterative_time_parameterization.h"
#include "moveit_msgs/msg/display_trajectory.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

namespace dual_arm_moveit_action_server
{
using DualArm = interfaces::action::DualArm;
using GoalHandleDualArm = rclcpp_action::ServerGoalHandle<DualArm>;

class DualArmMoveitActionServer
{
public:
  explicit DualArmMoveitActionServer(const rclcpp::Node::SharedPtr & node);

  void initialize();

private:
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const DualArm::Goal> goal);

  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleDualArm> goal_handle);

  void handle_accepted(const std::shared_ptr<GoalHandleDualArm> goal_handle);

  void execute(const std::shared_ptr<GoalHandleDualArm> goal_handle);

  bool validate_goal(const DualArm::Goal & goal, std::string * error_msg) const;

  std::vector<interfaces::msg::ArmMotionParams> normalize_goal_params(
    const DualArm::Goal & goal) const;

  bool plan_for_arm(
    const interfaces::msg::ArmMotionParams & params,
    moveit::planning_interface::MoveGroupInterface & move_group,
    int velocity_scaling,
    moveit_msgs::msg::RobotTrajectory * out_trajectory,
    std::string * error_msg);

  bool time_parameterize_trajectory(
    moveit::planning_interface::MoveGroupInterface & move_group,
    double velocity_scaling,
    moveit_msgs::msg::RobotTrajectory & trajectory,
    std::string * error_msg);

  void publish_display_trajectory(
    const moveit_msgs::msg::RobotTrajectory & trajectory,
    moveit::planning_interface::MoveGroupInterface & move_group);

  void fill_feedback_joints(DualArm::Feedback & feedback);

  void publish_feedback_once(const std::shared_ptr<GoalHandleDualArm> & goal_handle);
  void update_feedback(uint8_t status, double progress);

  rclcpp::Node::SharedPtr node_;
  rclcpp_action::Server<DualArm>::SharedPtr action_server_;
  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> left_move_group_;
  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> right_move_group_;
  rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr display_pub_;
  std::shared_ptr<DualArm::Feedback> feedback_state_;
  std::mutex feedback_mutex_;

  std::atomic_bool planning_busy_{false};
  std::atomic_bool cancel_requested_{false};
  std::string left_group_name_;
  std::string right_group_name_;
  double planning_time_{5.0};
  double acceleration_scaling_{0.5};
  double cartesian_step_{0.01};
  double cartesian_jump_threshold_{0.0};
  double cartesian_min_fraction_{0.9};
};

}  // namespace dual_arm_moveit_action_server

