#! /usr/bin/env python
__author__ = "Ben Raanan"
__copyright__ = "Copyright 2021, MBARI"
__credits__ = ["MBARI"]
__license__ = "GPL-3.0"
__maintainer__ = "Ben Raanan"
__email__ = "byraanan at mbari.org"
__doc__ = '''

Runs the LRAUV backseat helper service.

@author: __author__
@license: __license__
'''

import logging
import threading
import lcm

from LCM.Listener import LcmListener
from app.BackseatProcess import BackseatProcess
from app.AltitudeBridge import AltitudeBridge
from supervisor.Supervisor import Supervisor

logger = logging.getLogger("backseat")


class LrauvBackseat(Supervisor, LcmListener):

    def __init__(self, lcm_instance, app_cfg):
        Supervisor.__init__(self, lcm_instance, app_cfg)
        LcmListener.__init__(self, lcm_instance)

        self.app = BackseatProcess(lcm_instance, self.log_path, self.cfg)

        # init LCM data requests from LRAUV
        self.request_slate(self.cfg['lrauv_data'])

        # subscribe LCM handlers
        self.set_timeout(timeout_sec=2.5)
        self.subscribe({self.cfg['lcm_bsd_command_channel']: self.lrauv_command_handler})
        self.subscribe({self.cfg['lcm_lrauv_universal_channel']: self.app.handle_nema_gga})
        self.subscribe({self.cfg['lcm_lrauv_universal_channel']: self.app.handle_nema_vtg})
        self.subscribe({self.cfg['lcm_lrauv_universal_channel']: self.app.handle_simrad_tss1})

        # configure the altitude bridge thread
        self.altitude_bridge = AltitudeBridge(lcm_instance)
        self.altitude_bridge_thread = threading.Thread(target=self.altitude_bridge.spin)

    def spin(self):
        """
        Main loop. Handle subscriber callbacks until instructed to shutdown.

        :return: 1 upon exit
        """

        self.altitude_bridge_thread.start()

        while self.run:
            try:
                # listen to incoming LCM messages with a timeout
                self.listen()
                self.strobe_heartbeat(heart_rate_sec=5.0)
                self.request_data.request()

            except KeyboardInterrupt:
                logger.error("KeyboardInterrupt: aborting.")
                break

        # stop the altitude bridge
        self.altitude_bridge.stop()
        self.altitude_bridge_thread.join()

        # power down
        self.unsubscribe()
        self.shutdown()
        logger.info("Terminating.")
        exit(1)


if __name__ == "__main__":
    import argparse
    from config.AppConfig import read_config

    # parse command-line arguments
    parser = argparse.ArgumentParser(description='Runs the LRAUV backseat helper service.')
    parser.add_argument("-c", "--config",
                        default='./config/backseat_config.yml',
                        type=str, help="set path to app config file")

    args = parser.parse_args()

    # read in app configuration
    cfg = read_config(args.config)

    # init LCM object
    lc = lcm.LCM(cfg['lcm_url'])
    # init backseat app
    backseat = LrauvBackseat(lc, cfg)
    # run
    backseat.spin()