import logging
import math

from app.Nema import NemaGGA, NemaVTG
from app.SimradTSS1 import SimradTSS1
from app.UdpSocket import UdpPublisher
from LCM.HandlerBase import LcmHandlerBase
from LCM.Publisher import LcmPublisher


logger = logging.getLogger("backseat")


class BackseatProcess(LcmHandlerBase):

    def __init__(self, lcm_instance, log_path, cfg, notify=None):
        """
        Initialize AppProcess.

        :param lcm_instance: initialized LCM object lcm.LCM()
        :param cfg: pointer to backseat configuration obj
        """
        self.publisher = LcmPublisher(lcm_instance)
        self.cfg = cfg

        self.nema_pub = UdpPublisher(cfg['udp_address'], cfg['nema_udp_port'])
        self.simrad_pub = UdpPublisher(cfg['udp_address'], cfg['simrad_udp_port'])

        # GGA NEMA sentence
        self.gga = NemaGGA(log_path, cfg)
        # VTG NEMA sentence
        self.vtg = NemaVTG(log_path, cfg)
        # Simrad TSS1 motion sentence
        self.tss1 = SimradTSS1(log_path, cfg)


    def handle_nema_gga(self, channel, data):
        """
        Handles LRAUV data needed for NEMA GGA messages. Publishes formed NEMA sentences on dedicated UDP port.

        :param channel: LCM channel name string
        :param data: coded LCM packet
        :return: void
        """
        # logger.info("GGA: Handling LCM msg on channel {0}.".format(channel))
        msg = self.decode(data)

        got_data = False
        names = self.get_item_names(msg)
        timestamp = self.get_timestamp_sec(msg)

        self.gga.update_time(timestamp)

        # extract relevant data from the LCM message
        if self.cfg['lrauv_lat'] in names:
            # latitude in decimal degrees
            lat = self.get_variable(self.cfg['lrauv_lat'], msg)
            if lat:
                self.gga.lat = lat.data[0]
                got_data = True

        if self.cfg['lrauv_lon'] in names:
            # longitude in decimal degrees
            lon = self.get_variable(self.cfg['lrauv_lon'], msg)
            if lon:
                self.gga.lon = lon.data[0]
                got_data = True

        if self.cfg['lrauv_depth'] in names:
            # depth negative down
            depth = self.get_variable(self.cfg['lrauv_depth'], msg)
            if depth:
                self.gga.depth = -depth.data[0]
                got_data = True

        # assemble the NEMA sentence and ship it out once all data vars are available
        if got_data and self.gga.data_ready():
            gga_msg = self.gga.assemble_msg()

            self.nema_pub.send(gga_msg)
            self.gga.log_to_csv()
            self.gga.clear_data()


    def handle_nema_vtg(self, channel, data):
        """
        Handles LRAUV data needed for NEMA VTG messages. Publishes formed NEMA sentences on dedicated UDP port.

        :param channel: LCM channel name string
        :param data: coded LCM packet
        :return: void
        """
        # logger.info("VTG: Handling LCM msg on channel {0}.".format(channel))
        msg = self.decode(data)

        got_data = False
        names = self.get_item_names(msg)
        timestamp = self.get_timestamp_sec(msg)

        self.vtg.update_time(timestamp)

        if self.cfg['lrauv_heading'] in names:
            # vehicle heading in degrees
            heading = self.get_variable(self.cfg['lrauv_heading'], msg)
            if heading:
                self.vtg.heading = heading.data[0] * 180.0 / math.pi
                got_data = True

        if self.cfg['lrauv_speed'] in names:
            # vehicle velocity wrt to sea-water in m/s
            speed = self.get_variable(self.cfg['lrauv_speed'], msg)
            if speed:
                self.vtg.speed = speed.data[0]
                got_data = True

        if got_data and self.vtg.data_ready():
            vtg_msg = self.vtg.assemble_msg()

            self.nema_pub.send(vtg_msg)
            self.vtg.log_to_csv()
            self.vtg.clear_data()


    def handle_simrad_tss1(self, channel, data):
        """
        Handles LRAUV data needed for Simrad TSS1 motion messages.
        Publishes formed TSS1 msgs on dedicated UDP port.

        :param channel: LCM channel name string
        :param data: coded LCM packet
        :return: void
        """
        # logger.info("TSS1: Handling LCM msg on channel {0}.".format(channel))
        msg = self.decode(data)

        got_data = False
        names = self.get_item_names(msg)
        timestamp = self.get_timestamp_sec(msg)

        self.tss1.update_time(timestamp)

        if self.cfg['lrauv_pitch'] in names:
            # vehicle pitch angle in degrees
            pitch = self.get_variable(self.cfg['lrauv_pitch'], msg)
            if pitch:
                self.tss1.pitch = pitch.data[0] * 180.0 / math.pi
                got_data = True

        if self.cfg['lrauv_roll'] in names:
            # vehicle roll angle in degrees
            roll = self.get_variable(self.cfg['lrauv_roll'], msg)
            if roll:
                self.tss1.roll = roll.data[0] * 180.0 / math.pi
                got_data = True

        if got_data and self.tss1.data_ready():
            tss1_msg = self.tss1.assemble_msg()

            self.simrad_pub.send(tss1_msg)
            self.tss1.log_to_csv()
            self.tss1.clear_data()