import struct



class DataPacket:
    USART_START_BYTE0 = 	0x9C
    USART_START_BYTE1 = 	0x55

    PKRES_NO_PACKET =       0x00
    PKRES_SUCCESS =         0x01
    PKRES_BAD_START_SEQ =   0x10
    PKRES_BAD_TYPE =        0x11
    PKRES_INVALID_LENGTH =  0x12

    def __init__(self, packet_type=0, packet_data=[]):
        self.PktType = packet_type
        self.PktData = packet_data

    def create_packet(self):
        # Data packet format:
        # Bytes 0 and 1: special start bytes
        # Byte 2: packet type
        # Byte 3: bit inversion of byte 2
        # Bytes 4,5: data length (LSB first)
        # Bytes 6 to 6+length-1: Data bytes

        pkt = bytes([self.USART_START_BYTE0, self.USART_START_BYTE1])
        pkt = pkt + struct.pack('BB', self.PktType, self.PktType ^ 0xFF)
        pkt = pkt + struct.pack('<H', len(self.PktData))
        pkt = pkt + bytes(self.PktData)

        return pkt


    # Returns (Error code, position in buffer to safely delete up to, extracted DataPacket or None)
    # When calling this function, check the error code is PKRES_SUCCESS before opening the
    # packet. Otherwise you'll get a NoneType there.
    # The 2nd return value is an integer that tells you what position in the buffer may be 
    # safely deleted. There's no valid packet up to that point.
    def extract_packet(pkt):
        # Check for first start byte anywhere in this buffer
        start_pos = pkt.find(bytes([DataPacket.USART_START_BYTE0]))
        if(start_pos == -1):
            # Not found
            return (DataPacket.PKRES_NO_PACKET, -1, None)
        if(start_pos == (len(pkt)-1)):
            # Final byte in the buffer was start byte 0, more could be following
            # We should wait for more bytes
            return (DataPacket.PKRES_NO_PACKET, start_pos-1, None)
        if(start_pos + 6 > len(pkt)):
            # Not enough bytes have arrived yet
            return (DataPacket.PKRES_NO_PACKET, start_pos-1, None)
        if(pkt[start_pos+1] != DataPacket.USART_START_BYTE1):
            # Start byte 1 didn't follow start byte 0
            return (DataPacket.PKRES_BAD_START_SEQ, start_pos, None)
        if((pkt[start_pos+2] ^ pkt[start_pos+3]) != 0xFF):
            # Type and inverted type don't match
            return (DataPacket.PKRES_BAD_TYPE, start_pos, None)
        pkt_type = pkt[start_pos+2]
        pkt_length = struct.unpack('<H',pkt[(start_pos+4):(start_pos+6)])[0]
        if(start_pos + 6 + pkt_length > len(pkt)):
            # Not enough bytes have arrived yet
            return (DataPacket.PKRES_NO_PACKET, start_pos-1, None)
        # Good packet has arrived
        pkt_data = list(pkt[(start_pos+6):(start_pos+6+pkt_length)])
        return(DataPacket.PKRES_SUCCESS, start_pos+6+pkt_length-1, DataPacket(pkt_type, pkt_data))