#!/usr/bin/env python3

import os
from typing import Dict

import rclpy
import yaml
from ament_index_python.packages import get_package_share_directory
from geometry_msgs.msg import Pose
from rbs_utils_interfaces.srv import GetNamedPose
from rclpy.node import Node
from rclpy.service import Service


class GetNamedPoseService(Node):
    def __init__(self) -> None:
        super().__init__("get_named_pose_service")
        self.srv: Service = self.create_service(
            GetNamedPose, "get_named_pose", self.get_named_pose
        )
        yaml_file_path: str = os.path.join(
            get_package_share_directory("rbs_mill_assist"), "config", "key_poses.yaml"
        )
        with open(yaml_file_path, "r", encoding="utf-8") as file:
            self.key_poses: Dict = yaml.safe_load(file)

    def create_pose(self, pose_data: Dict) -> Pose:
        """
        Helper function to create a Pose from the given pose data.
        """
        pose = Pose()
        if pose_data:
            pose.position.x = pose_data.get("position", {}).get("x", 0.0)
            pose.position.y = pose_data.get("position", {}).get("y", 0.0)
            pose.position.z = pose_data.get("position", {}).get("z", 0.0)
            pose.orientation.x = pose_data.get("orientation", {}).get("x", 0.0)
            pose.orientation.y = pose_data.get("orientation", {}).get("y", 0.0)
            pose.orientation.z = pose_data.get("orientation", {}).get("z", 0.0)
            pose.orientation.w = pose_data.get("orientation", {}).get("w", 1.0)
        return pose

    def get_named_pose(
        self, request: GetNamedPose.Request, response: GetNamedPose.Response
    ) -> GetNamedPose.Response:

        key_pose = self.key_poses.get(request.pose_name, None)
        if key_pose is None:
            response.ok = False
            return response

        response.named_pose.name = request.pose_name
        response.named_pose.pose = self.create_pose(key_pose)
        response.ok = True

        return response


def main():
    rclpy.init()
    executor = rclpy.executors.SingleThreadedExecutor()
    # executor = rclpy.executors.MultiThreadedExecutor() # can't be used
    i_node = GetNamedPoseService()
    executor.add_node(i_node)
    try:
        executor.spin()
    except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
        i_node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()