import rclpy
from rclpy.node import Node

from std_msgs.msg import Bool
from ros2aria_msgs.msg import RestrictionsMsg
from rclpy.qos import QoSDurabilityPolicy
from rclpy.qos import QoSHistoryPolicy
from rclpy.qos import QoSLivelinessPolicy
from rclpy.qos import QoSProfile
from rclpy.qos import QoSReliabilityPolicy
import sys

class RestrictionRepublisher(Node):

    def __init__(self):

        profile = QoSProfile(
            depth=10,
            history=QoSHistoryPolicy.KEEP_ALL,
            reliability=QoSReliabilityPolicy.RELIABLE,
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
        super().__init__('restriction_republisher')
        self.restriction_pub_ = self.create_publisher(RestrictionsMsg, '/pioneers/restrictions', qos_profile=profile)
        self.master_stop_pub_ = self.create_publisher(Bool, '/pioneers/master_stop', qos_profile=profile)
        self.restriction_sub_ = self.create_subscription(RestrictionsMsg, '/pioneers/restrictions', self.restriction_callback, qos_profile=profile)
        self.master_stop_sub_ = self.create_subscription(Bool, '/pioneers/master_stop', self.master_stop_callback, qos_profile=profile)
        timer_period = 1.0  
        self.timer = self.create_timer(timer_period, self.timer_callback)

        self.master_stop_msg = Bool()
        self.restriction_msg = RestrictionsMsg()
        self.master_stop_msg. data = True
        self.restriction_msg.distance.data = sys.float_info.max
        self.restriction_msg.linear_velocity.data = 0.0
        self.restriction_msg.angular_velocity.data = 0.0

    def timer_callback(self):
        self.restriction_pub_.publish(self.restriction_msg)
        self.master_stop_pub_.publish(self.master_stop_msg)

    def restriction_callback(self, msg):
        if msg != self.restriction_msg:
            self.get_logger().info('New restricitons.')
            self.restriction_msg = msg
    
    def master_stop_callback(self, msg):
        if msg != self.master_stop_msg:
            self.get_logger().info('New master stop.')
            self.master_stop_msg = msg

def main(args=None):
    rclpy.init(args=args)

    restriction_republisher = RestrictionRepublisher()

    rclpy.spin(restriction_republisher)
    restriction_republisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()