//#include "rbs_skill_servers"
#include "rbs_skill_servers/pick_place_pose_loader.hpp"

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::handle_server, this, std::placeholders::_1, std::placeholders::_2));
}

void
GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request, 
            rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response)
{
    std::string rrr = request->object_name + "_place"; // TODO: replace with better name
    try {
        tf_data = tf_buffer_->lookupTransform(
        "world", rrr.c_str(),
        tf2::TimePointZero);
    } catch (const tf2::TransformException & ex) {
        RCLCPP_ERROR(
            this->get_logger(), "Could not transform %s to %s: %s",
            rrr.c_str(), "world", ex.what());
        return;
    }
    // TODO: Here need to check the parameter
    // We can use another parameter library from PickNik
    grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose").as_double_array();
    RCLCPP_INFO(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_INFO(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);
    Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose);
    Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data);
    //std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << std::endl << grasp_pose.linear() << std::endl;
    //std::cout << "place_pose = " << std::endl << place_pose.translation() << std::endl << place_pose.linear() << std::endl;
    Eigen::Vector3d vec_grasp(0.15,0.15,0.02);
    Eigen::Vector3d vec_place(0,0,0.15);
    response->grasp = collect_pose(grasp_pose, request->grasp_direction, vec_grasp);
    response->place = collect_pose(place_pose, request->place_direction, vec_place); 
}

std::vector<geometry_msgs::msg::Pose> 
GetGraspPlacePoseServer::collect_pose(
    const Eigen::Affine3d pose, 
    const geometry_msgs::msg::Vector3 direction,
    const Eigen::Vector3d scale_vec)
{
    std::vector<geometry_msgs::msg::Pose> pose_v_;
    pose_v_.push_back(tf2::toMsg(pose));
    Eigen::Vector3d posedir;
    Eigen::Affine3d pose_ = pose;
    tf2::fromMsg(direction, posedir);
    Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose();
    Ixy.diagonal() = posedir;
    Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero();
    Iz.diagonal() = Eigen::Vector3d::UnitZ();

    if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()).all()) // IF Direction == [1,0,0]
    {
        std::cout << "\n TBD" << std::endl;
    } 
    else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0]
    {
        // Gp -- grasp point frame
        pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y
        pose_v_.push_back(tf2::toMsg(pose_));
        pose_.pretranslate(Iz * scale_vec);     // Gp-y + z
        pose_v_.push_back(tf2::toMsg(pose_));
        pose_.pretranslate(Ixy * scale_vec);    // Gp+z
        pose_v_.push_back(tf2::toMsg(pose_));
    } 
    else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()).all()) // IF Direction == [0,0,1]
    {
        pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp
        pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp
    } 
    else 
    {
        std::cout << "\n TBD" << std::endl;
    }
    return pose_v_;
}

Eigen::Affine3d 
GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector<double> pose)
{
    Eigen::Vector3d point(std::vector<double>(pose.begin(), pose.begin()+3).data());
    Eigen::Quaterniond quat(std::vector<double>(pose.begin()+3, pose.end()).data());
    Eigen::Affine3d aff;
    aff.translation() = point;
    aff.linear() = quat.toRotationMatrix();
    return aff;
}