import serial
import struct
import numpy as np
import queue
import time
import threading


exitFlag = False      

class GX5_25(threading.Thread):
    """
    LORD Microstrain basic module for use of a
    preconfigured 3DM-GX5-24 AHRS set to output
    only euler angles (at-present)
    """

    def __init__(self):
        threading.Thread.__init__(self)
        self.ser = serial.Serial()
        self.ser.port = 'COM4'
        self.ser.baud_rate = 115200
        self.ser.parity = serial.PARITY_NONE
        self.ser.stopbits = serial.STOPBITS_ONE
        self.ser.timeout = 5
        self.data_buff = queue.Queue(maxsize=512)
        self.data_array = np.ndarray([7,1])
        self.threadID = 1
        self.name = 'GX5-25'

    def run(self):
        print(f"Starting acquisition thread: {self.name}:{self.threadID}\r\n")
        self.enable_imu_data_stream()
        try:
            while not exitFlag:
                self.read_stream_data()
        finally:
            self.set_to_idle()
            
    def open(self):
        """
        Open the serial port
        """
        
        try:
            self.ser.open()
            
        except Exception as e:
            raise e
        
        if self.ser.is_open is True:
            print('GX5_25 serial port is open')

    def close(self):
        """
        Close the serial port
        """

        try:
            self.ser.close()
        except Exception as e:
            raise e

        if self.ser.is_open is False:
            print('GX5_25 serial port is closed')
            #raise Exception('yargh')

    def ping(self):
        """
        Send and wait for a ping message, good
        for initial testing
        """
        
        try:
            print('Issuing Ping command')
            self.ser.write(b'\x75\x65\x01\x02\x02\x01\xE0\xC6')
            self.ser.flush()
            bytes_read = self.ser.read(10)
            print(bytes_read)
        except Exception as e:
            raise e

    def set_to_idle(self):
        """
        Return the sensor to an idle state, disabling the
        streaming data
        """
        
        try:
            print('Setting to idle')
            self.ser.write(b'\x75\x65\x01\x02\x02\x02\xE1\xC7')
            self.ser.flush()
            bytes_read = self.ser.read(10)
            print(bytes_read)
        except Exception as e:
            raise e

    def enable_imu_data_stream(self):
        """
        Enable data streaming modes from the AHRS at the
        configured rate """
        
        try:
            print('Enabling imu data')
            self.ser.write(b'\x75\x65\x0C\x0A\x05\x11\x01\x01\x01\x05\x11\x01\x03\x01\x24\xCC')
            self.ser.flush()
            bytes_read = self.ser.read(14)
            print(bytes_read)
        except Exception as e:
            raise e
        
    def get_rpy_msg(self, msg_len):

        gotU = False
        gotE = False
        bytes_read = bytearray(msg_len)
        #wait for a 'ue'
        byte = 'l'
        
        while byte is not None and not gotE:

            byte = self.ser.read(1)

            if not gotU and byte == b'u':
                gotU = True
                bytes_read[0] = 117
                continue
                
            if gotU and not gotE and byte == b'e':
                gotE = True
                bytes_read[1] = 101
                
        if byte is not None:
            rest_len = msg_len-2
            rest = self.ser.read(rest_len)
            if len(rest) is (rest_len):
                bytes_read[2:msg_len-1] = rest
                return bytes_read

        return None
                

    def read_stream_data(self):
        """
        Assuming streaming data is enabled, this function
        reads data in the serial port buffer. 
        """
        
        try:
            #print(f"in: {self.ser.in_waiting}")
            
            #bytes_read = self.ser.read(20)
            bytes_read = self.get_rpy_msg(20)

            self.data_array[0] = time.time_ns()
            #roll
            self.data_array[1] = struct.unpack('>f',bytes_read[6:10])
            #pitch
            self.data_array[2] = struct.unpack('>f',bytes_read[10:14])
            #yaw
            self.data_array[3] = struct.unpack('>f',bytes_read[14:18])

            #rolld
            self.data_array[4] = np.rad2deg(self.data_array[1])
            #pitchd
            self.data_array[5] = np.rad2deg(self.data_array[2])
            #yawd
            self.data_array[6] = np.rad2deg(self.data_array[3])

            #enqueue
            if self.data_buff.full() == False:
                self.data_buff.put(self.data_array)
            else:
                print("microstrain data queue full!")
                
            #print(self.data_array)
        except Exception as e:
            raise e

    def data_available(self):
        return (self.data_buff.qsize() > 0)
    def get_data(self):
        '''
        returns a ndarray of fields of the ahrs
        sensor pulled from a FIFO queue behind the
        serial port.
        
        [0] : time in nanoseconds
        [1] : roll in radians
        [2] : pitch in radians
        [3] : yaw in radians
        [4] : roll in degrees
        [5] : pitch in radians
        [6] : yaw in radians
        '''
        data = self.data_buff.get()
        return data
        
            
       

    
            
if __name__ == "__main__":
    
    gx = GX5_25()
    gx.open()

    gx.ping()
    
    gx.start()

    #monitor the thread
    try:
        while not exitFlag:
            #this sleep is really important to making sure that
            #the polling thread gets enough time to service all
            #incoming data from the ahrs.
            time.sleep(0.1)
            while gx.data_buff.qsize() > 0:
                #empty queue
                data = gx.data_buff.get()
                print(f"{data}")
                pass
            #print(f"{gx.data_buff.qsize()}")
    except KeyboardInterrupt:
        print('Keyboard Interrupt')
        exitFlag = 1
    finally:
        gx.join()
        gx.close()