import struct

LPHEADERSIZE=17
LPMAXDATASIZE=512

kLP_NOP=0xFF


class LP:
    def __init__(self, ptype=None, packetmanager=None):
        self._readpos=0
        self._data=bytearray()
        self._datasize=0
        self._synced=False
        self._packetmanager=packetmanager
        self._session=False
        if ptype is not None:
            self.create(ptype)

    def create(self, ptype, idRequest=0):
        header=bytearray(LPHEADERSIZE)
        header[0]=0xAA
        header[11]=ptype
        if idRequest:
            header[7]=idRequest & 0xFF
            header[8]=idRequest >> 8
        header[15]=0xA5
        self.writeData(header)
        self._datasize=0

    def erase(self):
        self._data=bytearray()
        self.rewind()

    def truncate(self):
        self._data=self._data[0:LPHEADERSIZE]
        self._synced=False
        self._datasize=0
        self.rewind()

    def openSession(self):
        self._session=True

    def closeSession(self):
        self._session=False

    def calcHeaderChecksum(self):
        chksum=0
        if len(self._data)>=LPHEADERSIZE-1:
            for b in self._data[0:LPHEADERSIZE-1]:
                chksum=chksum ^ b
        return chksum

    def calcHeader(self):
        size=len(self._data)-LPHEADERSIZE
        self._data[13]=size & 0xFF
        self._data[14]=size >> 8
        self._data[16]=self.calcHeaderChecksum()

    def checkHeader(self):
        try:
            if len(self._data)<LPHEADERSIZE:
                return False
            if self._data[0]!=0xAA or self._data[15]!=0xA5:
                return False
            if self.calcHeaderChecksum()!=self._data[16]:
                return False
            return True
        except:
            pass
        return False

    def extractDataSize(self):
        try:
            self._datasize=(self._data[14] << 8) | self._data[13]
            return self._datasize
        except:
            return 0

    def sync(self):
        while self._data:
            if self.checkHeader():
                size=self.extractDataSize()
                if len(self._data)<LPHEADERSIZE+size:
                    return False
                self._synced=True
                return True
            print('OUTOFSYNC!')
            del self._data[0:1]
        return False

    def consumeData(self):
        if self._synced:
            self._data=self._data[LPHEADERSIZE+self._datasize:]
            self._datasize=0
            self.rewind()
            self._synced=False

    def writeData(self, data):
        size=len(data)
        if size:
            if len(self._data)+size>LPMAXDATASIZE:
                # print("DEBUG LP presend")
                self.send()
            if len(self._data)+size<=LPMAXDATASIZE:
                self._data.extend(data)
                self._datasize=self._datasize+size
                return True
        print("LP:writeData() error!")
        return False

    def receiveData(self, data):
        size=len(data)
        if size:
            self._data.extend(data)
            return True
        print("LP:receiveData() error!")
        return False

    def getType(self):
        try:
            return self._data[11]
        except:
            return 0

    def dataToString(self, data):
        try:
            return ':'.join('%02X' % byte for byte in data)
        except:
            return ''

    def toString(self):
        str='LP[type={0}, S={1}, {2}]={3}'.format(self.getType(),
            self._datasize,
            self.dataToString(self._data[0:LPHEADERSIZE]),
            self.dataToString(self._data[LPHEADERSIZE:LPHEADERSIZE+self.extractDataSize()]))
        return str

    def dump(self):
        print self.toString()

    def getData(self, offset=0, size=0):
        if LPHEADERSIZE+offset+size<=LPHEADERSIZE+self._datasize:
            if size>0:
                return self._data[LPHEADERSIZE+offset:LPHEADERSIZE+offset+size]
            return self._data[LPHEADERSIZE+offset:LPHEADERSIZE+offset]
        return None

    def rewind(self):
        self._readpos=0

    def readData(self, size):
        data=self.getData(self._readpos, size)
        if data:
            self._readpos+=len(data)
        return data

    def readUP(self):
        data=self.readData(2)
        if data and len(data)==2:
            up=UP(self, data[0])
            # up.dump()
            size=data[1]
            if size==0:
                return up
            else:
                data=self.readData(size)
                if data and len(data)==size:
                    up.addData(data)
                    return up
            print('UP decoding error!')
            self.dump()
        return None

    def up(self, ptype=0):
        """
        :rtype : UP
        """
        return UP(self, ptype)

    def send(self, skipEmpty=True):
        if not self._session:
            if self._packetmanager:
                return self._packetmanager.sendLP(self, skipEmpty)


# http://dabeaz.blogspot.com/2010/01/few-useful-bytearray-tricks.html
class UP:
    def __init__(self, lp, ptype=0):
        self._lp=lp
        self._readpos=0
        self._data=bytearray(struct.pack('BB', ptype, 0))

    def getType(self):
        return self._data[0]

    def setType(self, ptype):
        self._data[0]=ptype

    def erase(self):
        self._data=self._data[0:2]
        self.calcHeader()
        self.rewind()

    def getDataSize(self):
        try:
            size=len(self._data)
            return max(0, size-2)
        except:
            return 0

    def getFreeSize(self):
        return 255-len(self._data)+2

    def validateFreeSize(self, size):
        return size<self.getFreeSize()

    def calcHeader(self):
        size=len(self._data)-2
        self._data[1]=size

    def store(self):
        self.calcHeader()
        self._lp.writeData(self._data)

    def addData(self, data):
        if self.validateFreeSize(len(data)):
            self._data.extend(data)
        else:
            raise ValueError

    def writeStrField(self, str, size):
        if size>0:
            str=str[:size-1]
        self.addData(str+'\0'*(size-len(str)))

    def writeByte(self, value):
        self.addData(struct.pack('B', value))

    def writeBool(self, value):
        self.writeByte(bool(value))

    def writeWord(self, value):
        self.addData(struct.pack('<H', value))

    def writeFloat(self, value):
        self.addData(struct.pack('<f', value))

    def writeDouble(self, value):
        self.addData(struct.pack('<d', value))

    def toString(self):
        str='UP[type={0}, size={1}]={2}'.format(self.getType(),
            self.getDataSize(),
            self._lp.dataToString(self._data[2:]))
        return str

    def dump(self):
        print '  ' + self.toString()

    def getData(self, offset=0, size=0):
        if size>0:
            return self._data[2+offset:2+offset+size]
        return self._data[2+offset:2+offset]

    def rewind(self):
        self._readpos=0

    def readable(self, size=1):
        if self.getDataSize()-self._readpos>=size:
            return True
        return False

    def readData(self, size):
        if self.readable(size):
            data=self.getData(self._readpos, size)
            self._readpos+=size
            return data
        return None

    def readDataStr(self, size):
        # struct.unpack currently only support str
        return str(self.readData(size))

    def readStruct(self, format):
        size=struct.calcsize(format)
        data=self.readDataStr(size)
        if data:
            try:
                return struct.unpack(format, data)
            except:
                pass
        return None

    def readByte(self):
        if self.readable(1):
            return self.readStruct('B')[0]
        return 0

    def readBool(self):
        return bool(self.readByte())

    def readWord(self):
        if self.readable(2):
            return self.readStruct('<H')[0]
        return 0

    def readFloat(self):
        if self.readable(4):
            return self.readStruct('<f')[0]
        return 0

    def readDouble(self):
        if self.readable(4):
            return self.readStruct('<d')[0]
        return 0

    def readStrField(self, size):
        str=self.readDataStr(size).strip('\0')
        if str[-1]=='\0':
            str=str[:-1]
        return str


class PacketManager(object):
    def __init__(self, debug=False):
        self._handlers={}
        self._lp=LP()
        self._debug=debug

    def dispose(self):
        self.lpRx=None
        self._handlers=None

    def addHandler(self, lptype, uptype, handler):
        lphandler=self.lpHandler(lptype)
        if not lphandler:
            lphandler={}
            self._handlers[lptype]=lphandler
        lphandler[uptype]=handler

    def lpHandler(self, lptype):
        try:
            return self._handlers[lptype]
        except:
            pass
        return None

    def upHandler(self, lphandler, uptype):
        try:
            return lphandler[uptype]
        except:
            pass
        return None

    def receive(self, data):
        self._lp.receiveData(data)
        while self._lp.sync():
            self.onReceiveLP(self._lp)
            self._lp.consumeData()

    def sendLP(self, lp, skipEmpty=True):
        result=True
        if not skipEmpty or lp._datasize>0:
            lp.calcHeader()
            # lp.dump()
            result=self.write(str(lp._data))
        lp.truncate()
        return result

    def write(self, data):
        raise NotImplementedError

    def onReceiveLP(self, lp):
        if self._debug:
            lp.dump()
        lphandler=self.lpHandler(lp.getType())
        if lphandler:
            while 1:
                up=lp.readUP()
                if not up:
                    break
                if self._debug:
                    up.dump()
                uphandler=self.upHandler(lphandler, up.getType())
                if uphandler:
                    uphandler(up)
                else:
                    print('unhandled UP type {0}'.format(up.getType()))
                    # lp.dump()
                    up.dump()
        else:
            # avoid connexion hangup by replying to nop messages
            if lp.getType()==kLP_NOP:
                self.sendLP(LP(kLP_NOP), False)
            else:
                # print('unhandled LP type {0}'.format(lp.getType()))
                # lp.dump()
                pass
