import math
import sys

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster


class StaticFramePublisher(Node):
    """
    Broadcast transforms that never change.

    This example publishes transforms from `world` to a static turtle frame.
    The transforms are only published once at startup, and are constant for all
    time.
    """

    def __init__(self, frame, position, quaternion, name: str = 'static_broadcaster', parent_frame: str = 'world'):
        super().__init__(name)

        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # Publish static transforms once at startup
        self.make_transforms(frame, position.split(" "), quaternion.split(" "), parent_frame)

    def make_transforms(self, frame, position, quaternion, parent_frame):
        t = TransformStamped()

        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = parent_frame
        t.child_frame_id = frame

        t.transform.translation.x = float(position[0])
        t.transform.translation.y = float(position[1])
        t.transform.translation.z = float(position[2])
        t.transform.rotation.x = float(quaternion[0])
        t.transform.rotation.y = float(quaternion[1])
        t.transform.rotation.z = float(quaternion[2])
        t.transform.rotation.w = float(quaternion[3])

        self.tf_static_broadcaster.sendTransform(t)


def main(args=None):
    logger = rclpy.logging.get_logger('logger')

    rclpy.init(args=args)

    node = rclpy.create_node('static_broadcaster')
    parent_frame = node.declare_parameter('parent_frame', 'world').value
    frame = node.declare_parameter('frame', 'object').value
    name = node.declare_parameter('name', 'static_broadcaster').value
    position = node.declare_parameter('position', '0 0 0').value
    quaternion = node.declare_parameter('quaternion', '0 0 0 1').value

    node = StaticFramePublisher(
        frame,
        position,
        quaternion,
        name,
        parent_frame)
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()