import time
import lcm

import supervisor.Logger

logger, _ = supervisor.Logger.configure_logger(name='backseat', log_to_file=False)

from LCM.Publisher import LcmPublisher
from config.AppConfig import read_config


cfg = read_config()


def test_publish_lcm(lc=lcm.LCM(cfg['lcm_url'])):

    msg_pub_rate_sec = 0.4

    pub = LcmPublisher(lc)

    # stubs
    lrauv_data = {
        'latitude': {'val': 36.8, 'unit': 'degree'},
        'longitude': {'val': -121.8, 'unit': 'degree'},
        'depth': {'val': 1.8, 'unit': 'meter'},
        'platform_orientation': {'val': 0.1, 'unit': 'radian'},
        'platform_x_velocity_wrt_sea_water': {'val': 1.0, 'unit': 'm/s'},
        'platform_pitch_angle': {'val': 0.349066, 'unit': 'radian'},
        'platform_roll_angle': {'val': -0.01, 'unit': 'radian'}
    }

    print("\n")
    print("publishing LRAUV msgs at {0} Hz:".format(1 / msg_pub_rate_sec))
    while True:
        try:
            t = time.time()
            print("publishing msg # {}".format(pub.seq_number))

            # pub Universals
            for n, v in lrauv_data.items():
                pub.clear_msg()
                pub.add_variable(n, v['val'], v['unit'])

                pub.timestamp(time.time())
                pub.publish(cfg['lcm_lrauv_universal_channel'])

            time.sleep(max(0.0, t + msg_pub_rate_sec - time.time()))
        except IndexError:
            print("Reached EOF.")
        except KeyboardInterrupt:
            print("Aborting.")
            break

    # Send termination request
    print("Sending termination request.")
    pub.clear_msg()
    pub.add_variable('bsd_command', 'shutdown')
    pub.timestamp()
    pub.publish(cfg['lcm_bsd_command_channel'])


if __name__ == "__main__":

    test_publish_lcm()