#! /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__ = '''

Assembles Simrad TSS1 Motion message.

@author: __author__
@license: __license__
'''
from app.CsvWriter import CsvWriter


class SimradTSS1:

    def __init__(self, log_path, cfg):
        # data needed for GGA NEMA sentence
        self.time = None  # epoch seconds, GMT
        self.pitch = None  # vehicle pitch angel, degrees
        self.roll = None  # vehicle roll angel, degrees

        self.csv_writer = CsvWriter('TSS1', log_path, cfg['tss1_csv_headers'], cfg['max_csv_log_duration_hr'])

    def log_to_csv(self):
        self.csv_writer.write(self.get_data())

    @staticmethod
    def assemble_tss1_angle(tss1_float: float):
        # “space” character if the value is positive, or a “–” character if it is negative
        str_out = " "
        if tss1_float < 0:
            str_out = "-"
        # four character decimal number with angle in hundreds of a degree
        str_out += "{:04d}".format(abs(int(tss1_float * 100)))

        return str_out

    def assemble(self, pitch: float, roll: float):
        pitch_str = self.assemble_tss1_angle(pitch)
        roll_str = self.assemble_tss1_angle(roll)
        return f":000000  0000U{roll_str} {pitch_str}\r\n"

    def assemble_msg(self):
        return self.assemble(self.pitch, self.roll)

    def update_time(self, new_time, max_time_diff=1.0):

        if self.time is None:
            self.time = new_time

        elif abs(self.time - new_time) >= max_time_diff:
            self.clear_data()
            self.time = new_time

    def get_data(self):
        return [self.time, self.pitch, self.roll]

    def data_ready(self):

        for val in self.get_data():
            if val is None:
                return False
        return True

    def clear_data(self):
        self.time = None
        self.pitch = None
        self.roll = None