###############################################################################
# Copyright 2020 - 2022 Trimble Inc.
# $Id: rt27_collector.py,v 1.4 2024/02/11 20:58:50 wlentz Exp $
# $Source: /home/wlentz/cvs_stinger/GPSTools/NoPiLive/rt27_collector.py,v $
###############################################################################

""" RT27 Measurement Collector Thread
    Thread class to collect RT27 measurements streamed to an IP port or read
    from a data file.
"""

import time
import threading
import socket
import RT27

class RT27MeasCollectorThread(threading.Thread):
    """ Thread class to collect RT27 measurements streamed to an IP port or
        read from a data file.
        The collector can be provided a name (i.e. "Base", "Rover", "BD940").
        Provide ip_addr ("10.1.150.75") and port (5017) to accept an RT27
        stream. Or provide file_name ("../data/bd940.rt27") to decode from a
        binary file containing logged RT27 data (i.e. captured using nc).
        The file_dwell value can be set to simulate the correct data rate of
        the file. This may be calculated later.
    """

    def __init__(self, name="", ip_addr=None, port_num=None, ant_num=0,
                 file_name=None, file_dwell=0.99, verbose=False):
        threading.Thread.__init__(self)

        # Thread configuration parameters
        self.name = name
        self.ip_addr = ip_addr
        self.port_num = port_num
        self.ant_num = ant_num
        self.file_name = file_name
        self.file_dwell = file_dwell
        self.run_thread = True
        self.verbose = verbose

        # Storage array for decoded RT27 measurements
        self.rt27_meas = []

        # RT27 file read or socket specific
        self.fid = None
        self.buffer = None
        self.is_socket = False
        self.is_file = False
        if ip_addr is not None and port_num is not None:
            print('Initialising connection to '+self.name)
            self.rxsocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.rxsocket.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
            self.rxsocket.settimeout(10)
            self.rxsocket.connect((ip_addr, port_num))
            self.is_socket = True
            # Not reading from a file, ensure the dwell is zero
            self.file_dwell = 0
        elif file_name is not None:
            print('Opening RT27 file '+file_name)
            self.fid = open(file_name, 'rb')
            self.buffer = bytearray(self.fid.read(1500))
            self.is_file = True
        else:
            print('rt27Thread configuration error')
            self.run_thread = False

        # RT27 data bytes parser function
        self.rt27_parser = RT27.ParseRT27()

    def quit_thread(self):
        """ Call to stop the thread. Used for clean shut-down. """
        self.run_thread = False

    def run(self):
        """ Call to run the thread. The thread will run until:
            - the IP socket closes
            - reached the end of the RT27 file
            - quit_thread() is called
        """
        while self.run_thread:
            if self.is_file:
                data = self.buffer
            else:
                data = bytearray(self.rxsocket.recv(5000))
            self.rt27_parser.process_data(data)
            # Select only data from the requested antenna
            sort_ant = []
            legacy = self.rt27_parser.get_legacy_output()
            for obs in legacy.obsEpoch:
                if obs['antNum'] == self.ant_num:
                    sort_ant.append(obs)
            if len(sort_ant)>0 and RT27.ParseRT27Legacy.ReturnType().dObs:
                if self.verbose: print("RT27 "+self.name+" ToW "+str(legacy.secs))
                # rt27_meas[][0] = Week #
                # rt27_meas[][1] = Time of week [s]
                # rt27_meas[][2] = Clock bias [ms]
                # rt27_meas[][3] = Array of RT27 measurements for this epoch
                self.rt27_meas.append([legacy.week,
                                       legacy.secs,
                                       legacy.bias,
                                       sort_ant])

                time.sleep(self.file_dwell)

            # When reading from a file, read more data when the buffer in the RT27
            # parser is geting low
            if self.is_file:
                bytes_left = self.rt27_parser.getDataLength()
                if bytes_left < 1500:
                    self.buffer = bytearray(self.fid.read(bytes_left))
                    if len(self.buffer) < bytes_left or len(self.buffer) == 0:
                        print('End of '+self.name+' RT27 data')
                        self.run_thread = False
        #print(self.name+' thread shut-down cleanly')

    def num_epochs(self):
        """ Get the number of decoder RT27 measurement epochs """
        return len(self.rt27_meas)

    def del_all_epochs(self):
        """ Delete all RT27 measurement epochs. """
        self.rt27_meas = []

    def del_epoch(self, idx):
        """ Delete RT27 measurement epoch for index idx. """
        if 0 <= idx < len(self.rt27_meas):
            self.rt27_meas.remove(idx)

    def get_time(self, idx):
        """ Get the time (Week #, ToW [s], clock bias [ms]) for the RT27
            measurement for index idx
        """
        ret = None
        if 0 <= idx < len(self.rt27_meas):
            ret = self.rt27_meas[idx][0], self.rt27_meas[idx][1], self.rt27_meas[idx][2]
        return ret

    def get_meas(self, idx):
        """ Get the RT27 measurements for index idx """
        ret = None
        if 0 <= idx < len(self.rt27_meas):
            ret = self.rt27_meas[idx][3]
        return ret

###############################################################################

def main():
    """ Run one instance of the RT27 measurement collector """
    collector = RT27MeasCollectorThread(name='tst', ip_addr='10.1.150.75', port_num=5017)
    collector.start()
    cntr = 30
    while cntr:
        print("RT27 epochs "+str(collector.num_epochs()))
        cntr -= 1
        time.sleep(1)

if __name__ == "__main__":
    main()