#pragma once

#include "dual_arm_action_server/real_robot_driver.hpp"
#include "interfaces/action/dual_arm.hpp"
#include "interfaces/msg/arm_error.hpp"
#include "interfaces/srv/clear_arm_error.hpp"
#include "interfaces/srv/inverse_kinematics.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <atomic>
#include <mutex>
#include <string>
#include <vector>

namespace dual_arm_action_server
{

class DualArmActionServer
{
public:
  explicit DualArmActionServer(const rclcpp::Node::SharedPtr & node);
  void initialize();

private:
  using DualArm = interfaces::action::DualArm;
  using GoalHandleDualArm = rclcpp_action::ServerGoalHandle<DualArm>;

  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);

  void clear_arm_error_callback(
    const std::shared_ptr<interfaces::srv::ClearArmError::Request> request,
    std::shared_ptr<interfaces::srv::ClearArmError::Response> response);
  void ik_service_callback(
    const std::shared_ptr<interfaces::srv::InverseKinematics::Request> request,
    std::shared_ptr<interfaces::srv::InverseKinematics::Response> response);
  void error_timer_callback();
  void joint_state_timer_callback();

  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;
  void get_target_arms(const DualArm::Goal & goal, bool & need_left, bool & need_right) const;

  rclcpp::Node::SharedPtr node_;
  rclcpp_action::Server<DualArm>::SharedPtr action_server_;

  rclcpp::Service<interfaces::srv::ClearArmError>::SharedPtr clear_error_service_;
  rclcpp::Service<interfaces::srv::InverseKinematics>::SharedPtr ik_service_;
  rclcpp::Publisher<interfaces::msg::ArmError>::SharedPtr arm_error_pub_;
  rclcpp::TimerBase::SharedPtr error_timer_;

  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_pub_;
  rclcpp::TimerBase::SharedPtr joint_state_timer_;
  sensor_msgs::msg::JointState joint_state_msg_;
  size_t left_arm_joint_start_index_;
  size_t right_arm_joint_start_index_;

  RealRobotDriver driver_;
  std::atomic<bool> executing_left_;
  std::atomic<bool> executing_right_;
  mutable std::mutex exec_state_mutex_;
};

}  // namespace dual_arm_action_server

