#############################################################################
# Copyright (c) 2002-2021 MBARI
# Monterey Bay Aquarium Research Institute, all rights reserved.
#############################################################################
import time
import lcm

from LCM.Publisher import LcmPublisher


def publish_heartbeat(lc=lcm.LCM()):

    pub = LcmPublisher(lc)
    pub_rate_sec = 5

    channel_name = 'BackSeatDriver_Heartbeat'
    var_name = 'heartbeat'
    cmd = 'photon'

    print("\nPublishing BSD heartbeat at {0} Hz:".format(1/pub_rate_sec))
    while True:
        try:
            t1 = time.time()

            print("Publishing BSD heartbeat # {0}".format(pub.seq_number))
            pub.clear_msg()
            pub.add_variable(var_name, cmd)
            pub.timestamp()
            pub.publish(channel_name)

            time.sleep(max(0.0, t1+pub_rate_sec - time.time()))
        except KeyboardInterrupt:
            print("Aborting.")
            break


if __name__ == "__main__":

    publish_heartbeat()