#include <functional>
#include <memory>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/time_parameterization.h>
#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp"

// action libs
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/transform.hpp"

// moveit libs
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_interface.h"
#include "moveit/robot_model_loader/robot_model_loader.h"
#include "moveit/trajectory_processing/time_optimal_trajectory_generation.h"

/*
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
// #include <eigen_conversions/eigen_msg.h>
#include "moveit_msgs/action/move_group.hpp"
#include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp"

namespace rbs_skill_actions {

class MoveCartesianActionServer : public rclcpp::Node {
public:
  using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;

  // explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node)
  explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node)
      : Node("move_cartesian_action_server"), node_(node) {
    // using namespace std::placeholders;
    // this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
    //     this->get_node_base_interface(),
    //     this->get_node_clock_interface(),
    //     this->get_node_logging_interface(),
    //     this->get_node_waitables_interface(),
    //     "move_topose",
    //     std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2),
    //     std::bind(&MoveCartesianActionServer::cancel_callback, this, _1),
    //     std::bind(&MoveCartesianActionServer::accepted_callback, this, _1));
  }

  void init() {

    action_server_ = rclcpp_action::create_server<MoveitSendPose>(
        node_->get_node_base_interface(), node_->get_node_clock_interface(),
        node_->get_node_logging_interface(),
        node_->get_node_waitables_interface(), "move_cartesian",
        std::bind(&MoveCartesianActionServer::goal_callback, this,
                  std::placeholders::_1, std::placeholders::_2),
        std::bind(&MoveCartesianActionServer::cancel_callback, this,
                  std::placeholders::_1),
        std::bind(&MoveCartesianActionServer::accepted_callback, this,
                  std::placeholders::_1));
  }

private:
  rclcpp::Node::SharedPtr node_;
  rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;

  using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;

  rclcpp_action::GoalResponse
  goal_callback(const rclcpp_action::GoalUUID &uuid,
                std::shared_ptr<const MoveitSendPose::Goal> goal) {
    RCLCPP_INFO(
        this->get_logger(),
        "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, "
        "%f, %f]",
        goal->robot_name.c_str(), goal->target_pose.position.x,
        goal->target_pose.position.y, goal->target_pose.position.z,
        goal->target_pose.orientation.x, goal->target_pose.orientation.y,
        goal->target_pose.orientation.z, goal->target_pose.orientation.w);
    (void)uuid;

    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse
  cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Received cancel request");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
    using namespace std::placeholders;
    std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1),
                goal_handle)
        .detach();
    // std::thread(
    //     [this, goal_handle]() {
    //         execute(goal_handle);
    //     }).detach();
  }

  void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Executing action goal");
    const auto goal = goal_handle->get_goal();
    auto result = std::make_shared<MoveitSendPose::Result>();

    moveit::planning_interface::MoveGroupInterface move_group_interface(
        node_, goal->robot_name);
    move_group_interface.startStateMonitor();

    moveit::core::RobotState start_state(
        *move_group_interface.getCurrentState());

    std::vector<geometry_msgs::msg::Pose> waypoints;
    auto current_pose = move_group_interface.getCurrentPose();
    // waypoints.push_back(current_pose.pose);
    // geometry_msgs::msg::Pose start_pose = current_pose.pose;
    geometry_msgs::msg::Pose target_pose = goal->target_pose;
    // target_pose.position = goal->target_pose.position;
    // int num_waypoints = 100;
    // for (int i = 1; i <= num_waypoints; ++i) {
    //   geometry_msgs::msg::Pose intermediate_pose;
    //   double fraction = static_cast<double>(i) / (num_waypoints + 1);
    //
    //   intermediate_pose.position.x =
    //       start_pose.position.x +
    //       fraction * (target_pose.position.x - start_pose.position.x);
    //   intermediate_pose.position.y =
    //       start_pose.position.y +
    //       fraction * (target_pose.position.y - start_pose.position.y);
    //   intermediate_pose.position.z =
    //       start_pose.position.z +
    //       fraction * (target_pose.position.z - start_pose.position.z);
    //
    //   intermediate_pose.orientation = start_pose.orientation;
    //
    //   waypoints.push_back(intermediate_pose);
    // }
    waypoints.push_back(target_pose);

    RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
                target_pose.position.x, target_pose.position.y,
                target_pose.position.z);

    moveit_msgs::msg::RobotTrajectory trajectory;
    const double jump_threshold = 0.0;
    const double eef_step = 0.001;
    double fraction = move_group_interface.computeCartesianPath(
        waypoints, eef_step, jump_threshold, trajectory);

    robot_trajectory::RobotTrajectory rt(
        move_group_interface.getCurrentState()->getRobotModel(),
        goal->robot_name);

    rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory);

    trajectory_processing::TimeOptimalTrajectoryGeneration tp;

    bool su = tp.computeTimeStamps(rt);

    rt.getRobotTrajectoryMsg(trajectory);

    moveit::planning_interface::MoveGroupInterface::Plan plan;
    plan.trajectory = trajectory;

    if (fraction > 0) {
      RCLCPP_INFO(this->get_logger(), "Planning success");
      moveit::core::MoveItErrorCode execute_err_code =
          move_group_interface.execute(plan);
      if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
        result->success = true;
        goal_handle->succeed(result);
        RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
      } else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) {
        RCLCPP_ERROR(this->get_logger(), "Failure in move:");
      }

      // move_group_interface.move();
    } else {
      RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
      goal_handle->abort(result);
    }

    if (goal_handle->is_canceling()) {
      goal_handle->canceled(result);
      RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
      return;
    }
  }
}; // class MoveCartesianActionServer

} // namespace rbs_skill_actions

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);

  rbs_skill_actions::MoveCartesianActionServer server(node);
  std::thread run_server([&server]() {
    rclcpp::sleep_for(std::chrono::seconds(3));
    server.init();
  });

  rclcpp::spin(node);
  run_server.join();

  return 0;
}