#include "rbs_skill_servers/pick_place_pose_loader.hpp"
#include <Eigen/src/Core/Matrix.h>
#include <Eigen/src/Geometry/AngleAxis.h>
#include <Eigen/src/Geometry/Quaternion.h>
#include <Eigen/src/Geometry/Transform.h>
#include <cmath>
#include <fstream>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <memory>
#include <rclcpp/logging.hpp>
#include <sdf/Geometry.hh>
#include <tf2_eigen/tf2_eigen.hpp>
#include <vector>

using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer;
using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses;
using namespace std::chrono_literals;
// rclcpp::Node::SharedPtr g_node = nullptr;

GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options)
    : Node("grasp_place_pose_loader",
           options.allow_undeclared_parameters(true)
               .automatically_declare_parameters_from_overrides(true)) {
  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

  srv_ = create_service<GetGraspPlacePoseService>(
      "/get_pick_place_pose_service",
      std::bind(&GetGraspPickPoseServer::handleServer, this,
                std::placeholders::_1, std::placeholders::_2));
}

void GetGraspPlacePoseServer::handleServer(
    const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
        request,
    rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
        response) {
  std::string object_name =
      request->object_name + "_place"; // TODO: replace with better name

  try {
    place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
                                                tf2::TimePointZero);
  } catch (const tf2::TransformException &ex) {
    RCLCPP_ERROR(this->get_logger(), "Could not transform %s to %s: %s",
                 object_name.c_str(), "world", ex.what());
    return;
  }
  // Load grasp pose from TF2
  try {
    grasp_pose_tf = tf_buffer_->lookupTransform(
        "world", request->object_name.c_str(), tf2::TimePointZero);
  } catch (const tf2::TransformException &ex) {
    RCLCPP_ERROR(this->get_logger(), "Could not transforms %s to %s: %s",
                 request->object_name.c_str(), "world", ex.what());
  }

  RCLCPP_DEBUG(this->get_logger(),
               "Grasp pose from tf \n\tx=%f\n\ty=%f\n\tz=%f",
               grasp_pose_tf.transform.translation.x,
               grasp_pose_tf.transform.translation.y,
               grasp_pose_tf.transform.translation.z);

  RCLCPP_DEBUG(this->get_logger(),
               "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
               request->grasp_direction.x, request->grasp_direction.y,
               request->grasp_direction.z);
  RCLCPP_DEBUG(this->get_logger(),
               "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f",
               request->place_direction.x, request->place_direction.y,
               request->place_direction.z);

  auto grasp_pose = tf2::transformToEigen(grasp_pose_tf);
  auto place_pose = tf2::transformToEigen(place_pose_tf);

  Eigen::Vector3d scale_grasp(0, 0, 0.10);
  Eigen::Vector3d scale_place(0, 0, 0.15);
  
  response->grasp =
      collectPose(grasp_pose, request->grasp_direction, scale_grasp);
  response->place =
      collectPose(place_pose, request->place_direction, scale_place);
}

std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collectPose(
    const Eigen::Isometry3d &graspPose,
    const geometry_msgs::msg::Vector3 &move_direction,
    const Eigen::Vector3d &scale_vec) {
  std::vector<geometry_msgs::msg::Pose> poses{};
  // Add GraspPose as base point
  Eigen::Isometry3d grasp_pose{graspPose};
  Eigen::Quaterniond theGraspOrientation(grasp_pose.linear());
  theGraspOrientation =
      theGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
  grasp_pose.rotate(theGraspOrientation);
  poses.push_back(tf2::toMsg(grasp_pose));
  // PreGrasp calculation
  Eigen::Vector3d preGraspTranslation =
      static_cast<Eigen::Vector3d>(graspPose.translation()) + scale_vec;
  Eigen::Quaterniond preGraspOrientation(graspPose.linear());
  preGraspOrientation =
      preGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
  Eigen::Isometry3d preGraspPose = Eigen::Isometry3d::Identity();
  preGraspPose.translate(preGraspTranslation);
  preGraspPose.rotate(preGraspOrientation);
  poses.push_back(tf2::toMsg(preGraspPose));

  // PostGrasp calculation
  Eigen::Vector3d postGraspTranslation =
      static_cast<Eigen::Vector3d>(graspPose.translation()) + scale_vec;
  Eigen::Quaterniond postGraspOrientation(graspPose.linear());
  postGraspOrientation =
      postGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());

  Eigen::Isometry3d postGraspPose = Eigen::Isometry3d::Identity();
  postGraspPose.translate(postGraspTranslation);
  postGraspPose.rotate(postGraspOrientation);
  poses.push_back(tf2::toMsg(postGraspPose));

  return poses;
}