#!/usr/bin/env python3

# Translates from NavSatFix to GPSFix and back

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
from rclpy.qos import QoSProfile
from sensor_msgs.msg import NavSatFix
from gps_msgs.msg import GPSFix
import gps_tools.gps_message_converter as converter


class FixTranslator(Node):
    def __init__(self):
        super().__init__('fix_translator')
        qos = QoSProfile(
            durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
            reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE,
            history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
            depth=10)
        self.navsat_pub = self.create_publisher(NavSatFix, 'navsat_fix_out', qos_profile=qos)
        self.gps_pub = self.create_publisher(GPSFix, 'gps_fix_out', qos_profile=qos)
        self.navsat_sub = self.create_subscription(NavSatFix, 'navsat_fix_in', self.navsat_callback, qos_profile=qos)
        self.gps_sub = self.create_subscription(GPSFix, "gps_fix_in", self.gps_callback, qos_profile=qos)

    def navsat_callback(self, navsat_msg):
        gps_msg = converter.navsatfix_to_gpsfix(navsat_msg)
        self.gps_pub.publish(gps_msg)

    # Translates from GPSFix to NavSatFix.
    # As GPSFix can store much more information than NavSatFix,
    # a lot of this additional information might get lost.
    def gps_callback(self, gps_msg):
        navsat_msg = converter.gpsfix_to_navsatfix(gps_msg)
        self.navsat_pub.publish(navsat_msg)


if __name__ == '__main__':
    rclpy.init()

    node = FixTranslator()

    rclpy.spin(node)

    rclpy.shutdown()
