#! /usr/bin/env python3

import yaml
import rclpy
from rclpy.node import Node
from rbs_utils_interfaces.msg import AssemblyConfig, NamedPose, RelativeNamedPose
from geometry_msgs.msg import Point
import os


class ConfigPublisherNode(Node):

    def __init__(self):
        super().__init__('config_publisher_node')
        self.publisher_ = self.create_publisher(AssemblyConfig, '/assembly_config', 10)
        timer_period = 1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        if 'RBS_ASSEMBLY_DIR' in os.environ:
            assembly_dir = os.environ['RBS_ASSEMBLY_DIR']
        else:
            assembly_dir = '~/assembly/robossembler-stuff'
        self.assembly_dir = os.path.expanduser(assembly_dir)
        datafolder = "asp-example"
        # Read data from YAML file
        with open(f"{self.assembly_dir}/{datafolder}/rbs_db.yaml") as f:
            self.data = yaml.safe_load(f)

    def timer_callback(self):

        # Create AssemblyConfig message
        msg = AssemblyConfig()
        # Fill in the data from the YAML file
        msg.assets_db = self.data['assets_db']

        # Populate workspace
        for wp in self.data['workspace']:
            point = Point()
            point.x = wp["x"]
            point.y = wp["y"]
            point.z = wp["z"]
            msg.workspace.append(point)

        # Populate absolute_part
        for part in self.data['absolute_part']:
            named_pose = NamedPose()
            named_pose.name = part['name']
            pose = part['pose']
            named_pose.pose.position.x = pose['position']['x']
            named_pose.pose.position.y = pose['position']['y']
            named_pose.pose.position.z = pose['position']['z']
            named_pose.pose.orientation.x = pose['orientation']['x']
            named_pose.pose.orientation.y = pose['orientation']['y']
            named_pose.pose.orientation.z = pose['orientation']['z']
            named_pose.pose.orientation.w = pose['orientation']['w']
            msg.absolute_part.append(named_pose)

        # Populate relative_part
        for part in self.data['relative_part']:
            relative_named_pose = RelativeNamedPose()
            relative_named_pose.name = part['name']
            relative_named_pose.relative_at = part['relative_at']
            pose = part['pose']
            relative_named_pose.pose.position.x = pose['position']['x']
            relative_named_pose.pose.position.y = pose['position']['y']
            relative_named_pose.pose.position.z = pose['position']['z']
            relative_named_pose.pose.orientation.x = pose['orientation']['x']
            relative_named_pose.pose.orientation.y = pose['orientation']['y']
            relative_named_pose.pose.orientation.z = pose['orientation']['z']
            relative_named_pose.pose.orientation.w = pose['orientation']['w']
            msg.relative_part.append(relative_named_pose)

        # Populate grasp_pose
        for part in self.data['grasp_pose']:
            relative_named_pose = RelativeNamedPose()
            relative_named_pose.name = part['name']
            relative_named_pose.relative_at = part['relative_at']
            pose = part['pose']
            relative_named_pose.pose.position.x = pose['position']['x']
            relative_named_pose.pose.position.y = pose['position']['y']
            relative_named_pose.pose.position.z = pose['position']['z']
            relative_named_pose.pose.orientation.x = pose['orientation']['x']
            relative_named_pose.pose.orientation.y = pose['orientation']['y']
            relative_named_pose.pose.orientation.z = pose['orientation']['z']
            relative_named_pose.pose.orientation.w = pose['orientation']['w']
            msg.grasp_pose.append(relative_named_pose)

        # Populate extra_poses
        for part in self.data['extra_poses']:
            named_pose = NamedPose()
            named_pose.name = part['name']
            pose = part['pose']
            named_pose.pose.position.x = pose['position']['x']
            named_pose.pose.position.y = pose['position']['y']
            named_pose.pose.position.z = pose['position']['z']
            named_pose.pose.orientation.x = pose['orientation']['x']
            named_pose.pose.orientation.y = pose['orientation']['y']
            named_pose.pose.orientation.z = pose['orientation']['z']
            named_pose.pose.orientation.w = pose['orientation']['w']
            msg.extra_poses.append(named_pose)

        # Publish the message
        self.publisher_.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    config_publisher_node = ConfigPublisherNode()
    rclpy.spin(config_publisher_node)
    config_publisher_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()