#!/usr/bin/env python3
"""Republishes a sensor_msgs/NavSatFix topic with simulated GNSS outages.

Fixes are dropped during the last OFF seconds (default 15) of every PERIOD seconds (default 45), counted from the
first received fix (or from --start, in the message clock). Used in the MOLA wheels + IMU +
GNSS fusion tutorial to show how the estimator holds the georeferenced pose without GNSS;
the dropped RTK fixes stay in the bag as independent ground truth.

Usage: gnss_outage_relay.py IN_TOPIC OUT_TOPIC [--period 45] [--off 15] [--start STAMP]
"""
import argparse

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import NavSatFix


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('in_topic')
    ap.add_argument('out_topic')
    ap.add_argument('--period', type=float, default=45.0)
    ap.add_argument('--off', type=float, default=15.0)
    ap.add_argument('--start', type=float, default=None)
    a, ros_args = ap.parse_known_args()
    rclpy.init(args=ros_args)
    node = Node('gnss_outage_relay')
    pub = node.create_publisher(NavSatFix, a.out_topic, 10)
    state = {'t0': a.start, 'dropped': 0, 'passed': 0}

    def cb(m):
        t = m.header.stamp.sec + m.header.stamp.nanosec * 1e-9
        if state['t0'] is None:
            state['t0'] = t
        if a.off > 0 and (t - state['t0']) % a.period >= a.period - a.off:
            state['dropped'] += 1
            return
        state['passed'] += 1
        pub.publish(m)

    node.create_subscription(NavSatFix, a.in_topic, cb, qos_profile_sensor_data)
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    print(f"gnss_outage_relay: passed {state['passed']}, dropped {state['dropped']}")


if __name__ == '__main__':
    main()
