#!/usr/bin/env python
'''
UBlox binary protocol handling

Copyright Andrew Tridgell, October 2012
Released under GNU GPL version 3 or later
'''

import struct
from datetime import datetime
import time, os
import sys

# specify Python version
if sys.version_info[0] < 3: # we're on python 2.x.x
    PYTHON_VERSION = 2
else:
    PYTHON_VERSION = 3

# protocol constants
PREAMBLE1 = 0xb5
PREAMBLE2 = 0x62

# message classes
CLASS_NAV = 0x01
CLASS_RXM = 0x02
CLASS_INF = 0x04
CLASS_ACK = 0x05
CLASS_CFG = 0x06
CLASS_MON = 0x0A
CLASS_AID = 0x0B
CLASS_TIM = 0x0D
CLASS_ESF = 0x10

# ACK messages
MSG_ACK_NACK = 0x00
MSG_ACK_ACK = 0x01

# NAV messages
MSG_NAV_POSECEF   = 0x1
MSG_NAV_POSLLH    = 0x2
MSG_NAV_STATUS    = 0x3
MSG_NAV_DOP       = 0x4
MSG_NAV_SOL       = 0x6
MSG_NAV_POSUTM    = 0x8
MSG_NAV_VELNED    = 0x12
MSG_NAV_VELECEF   = 0x11
MSG_NAV_TIMEGPS   = 0x20
MSG_NAV_TIMEUTC   = 0x21
MSG_NAV_CLOCK     = 0x22
MSG_NAV_SVINFO    = 0x30
MSG_NAV_AOPSTATUS = 0x60
MSG_NAV_DGPS      = 0x31
MSG_NAV_DOP       = 0x04
MSG_NAV_EKFSTATUS = 0x40
MSG_NAV_SBAS      = 0x32
MSG_NAV_SOL       = 0x06

# RXM messages
MSG_RXM_RAW    = 0x10
MSG_RXM_SFRB   = 0x11
MSG_RXM_SVSI   = 0x20
MSG_RXM_EPH    = 0x31
MSG_RXM_ALM    = 0x30
MSG_RXM_PMREQ  = 0x41

# AID messages
MSG_AID_ALM    = 0x30
MSG_AID_EPH    = 0x31
MSG_AID_ALPSRV = 0x32
MSG_AID_AOP    = 0x33
MSG_AID_DATA   = 0x10
MSG_AID_ALP    = 0x50
MSG_AID_DATA   = 0x10
MSG_AID_HUI    = 0x02
MSG_AID_INI    = 0x01
MSG_AID_REQ    = 0x00

# CFG messages
MSG_CFG_PRT = 0x00
MSG_CFG_ANT = 0x13
MSG_CFG_DAT = 0x06
MSG_CFG_EKF = 0x12
MSG_CFG_ESFGWT = 0x29
MSG_CFG_CFG = 0x09
MSG_CFG_USB = 0x1b
MSG_CFG_RATE = 0x08
MSG_CFG_SET_RATE = 0x01
MSG_CFG_NAV5 = 0x24
MSG_CFG_FXN = 0x0E
MSG_CFG_INF = 0x02
MSG_CFG_ITFM = 0x39
MSG_CFG_MSG = 0x01
MSG_CFG_NAVX5 = 0x23
MSG_CFG_NMEA = 0x17
MSG_CFG_NVS = 0x22
MSG_CFG_PM2 = 0x3B
MSG_CFG_PM = 0x32
MSG_CFG_RINV = 0x34
MSG_CFG_RST = 0x04
MSG_CFG_RXM = 0x11
MSG_CFG_SBAS = 0x16
MSG_CFG_TMODE2 = 0x3D
MSG_CFG_TMODE = 0x1D
MSG_CFG_TPS = 0x31
MSG_CFG_TP = 0x07
MSG_CFG_GNSS = 0x3E

# ESF messages
MSG_ESF_MEAS   = 0x02
MSG_ESF_STATUS = 0x10

# INF messages
MSG_INF_DEBUG  = 0x04
MSG_INF_ERROR  = 0x00
MSG_INF_NOTICE = 0x02
MSG_INF_TEST   = 0x03
MSG_INF_WARNING= 0x01

# MON messages
MSG_MON_SCHD  = 0x01
MSG_MON_HW    = 0x09
MSG_MON_HW2   = 0x0B
MSG_MON_IO    = 0x02
MSG_MON_MSGPP = 0x06
MSG_MON_RXBUF = 0x07
MSG_MON_RXR   = 0x21
MSG_MON_TXBUF = 0x08
MSG_MON_VER   = 0x04

# TIM messages
MSG_TIM_TP   = 0x01
MSG_TIM_TM2  = 0x03
MSG_TIM_SVIN = 0x04
MSG_TIM_VRFY = 0x06

# port IDs
PORT_DDC    =0
PORT_SERIAL1=1
PORT_SERIAL2=2
PORT_USB    =3
PORT_SPI    =4

# dynamic models
DYNAMIC_MODEL_PORTABLE   = 0
DYNAMIC_MODEL_STATIONARY = 2
DYNAMIC_MODEL_PEDESTRIAN = 3
DYNAMIC_MODEL_AUTOMOTIVE = 4
DYNAMIC_MODEL_SEA        = 5
DYNAMIC_MODEL_AIRBORNE1G = 6
DYNAMIC_MODEL_AIRBORNE2G = 7
DYNAMIC_MODEL_AIRBORNE4G = 8

#reset items
RESET_HOT  = 0
RESET_WARM = 1
RESET_COLD = 0xFFFF

RESET_HW            = 0
RESET_SW            = 1
RESET_SW_GPS        = 2
RESET_HW_GRACEFUL   = 4
RESET_GPS_STOP      = 8
RESET_GPS_START     = 9

class UBloxError(Exception):
    '''Ublox error class'''
    def __init__(self, msg):
        Exception.__init__(self, msg)
        self.message = msg

class UBloxAttrDict(dict):
    '''allow dictionary members as attributes'''
    def __init__(self):
        dict.__init__(self)

    def __getattr__(self, name):
        try:
            return self.__getitem__(name)
        except KeyError:
            raise AttributeError(name)

    def __setattr__(self, name, value):
        if self.__dict__.has_key(name):
            # allow set on normal attributes
            dict.__setattr__(self, name, value)
        else:
            self.__setitem__(name, value)

def ArrayParse(field):
    '''parse an array descriptor'''
    arridx = field.find('[')
    if arridx == -1:
        return (field, -1)
    alen = int(field[arridx+1:-1])
    fieldname = field[:arridx]
    return (fieldname, alen)

class UBloxDescriptor:
    '''class used to describe the layout of a UBlox message'''
    def __init__(self, name, msg_format, fields=[], count_field=None, format2=None, fields2=None):
        self.name = name
        self.msg_format = msg_format
        self.fields = fields
        self.count_field = count_field
        self.format2 = format2
        self.fields2 = fields2
    
    def getf(self, fmt, buf, size):
            f = list(struct.unpack(fmt, buf[:size]))
            return f
        
    def unpack(self, msg):
        '''unpack a UBloxMessage, creating the .fields and ._recs attributes in msg'''
        msg._fields = {}

        # unpack main message blocks. A comm
        formats = self.msg_format.split(',')
        buf = msg._buf[6:-2]
        count = 0
        msg._recs = []
        fields = self.fields[:]
        
        for fmt in formats:
            size1 = struct.calcsize(fmt)
            if size1 > len(buf):
                raise UBloxError("%s INVALID_SIZE1=%u" % (self.name, len(buf)))
            f1 = self.getf(fmt, buf, size1)

            i = 0
            while i < len(f1):
                field = fields.pop(0)
                (fieldname, alen) = ArrayParse(field)
                if alen == -1:
                    msg._fields[fieldname] = f1[i]
                    if self.count_field == fieldname:
                        count = int(f1[i])
                    i += 1
                else:
                    msg._fields[fieldname] = [0]*alen
                    for a in range(alen):
                        msg._fields[fieldname][a] = f1[i]
                        i += 1
            buf = buf[size1:]
            if len(buf) == 0:
                break

        if self.count_field == '_remaining':
            count = len(buf) / struct.calcsize(self.format2)

        if count == 0:
            msg._unpacked = True
            if len(buf) != 0:
                raise UBloxError("EXTRA_BYTES=%u" % len(buf))
            return

        size2 = struct.calcsize(self.format2)
        for c in range(count):
            r = UBloxAttrDict()
            if size2 > len(buf):
                raise UBloxError("INVALID_SIZE=%u, " % len(buf))
            f2 = self.getf(self.format2, buf, size2)    
            
            for i in range(len(self.fields2)):
                r[self.fields2[i]] = f2[i]
            buf = buf[size2:]
            msg._recs.append(r)
        if len(buf) != 0:
            raise UBloxError("EXTRA_BYTES=%u" % len(buf))
        msg._unpacked = True

    def pack(self, msg, msg_class=None, msg_id=None):
        '''pack a UBloxMessage from the .fields and ._recs attributes in msg'''
        f1 = []
        if msg_class is None:
            msg_class = msg.msg_class()
        if msg_id is None:
            msg_id = msg.msg_id()
        msg._buf = ''

        fields = self.fields[:]
        for f in fields:
            (fieldname, alen) = ArrayParse(f)
            if not fieldname in msg._fields:
                break
            if alen == -1:
                f1.append(msg._fields[fieldname])
            else:
                for a in range(alen):
                    f1.append(msg._fields[fieldname][a])                    
        try:
            # try full length message
            fmt = self.msg_format.replace(',', '')
            msg._buf = struct.pack(fmt, *tuple(f1))
        except Exception as e:
            # try without optional part
            fmt = self.msg_format.split(',')[0]
            msg._buf = struct.pack(fmt, *tuple(f1))

        length = len(msg._buf)
        if msg._recs:
            length += len(msg._recs) * struct.calcsize(self.format2)
        header = struct.pack('<BBBBH', PREAMBLE1, PREAMBLE2, msg_class, msg_id, length)
        msg._buf = header + msg._buf

        for r in msg._recs:
            f2 = []
            for f in self.fields2:
                f2.append(r[f])
            msg._buf += struct.pack(self.format2, *tuple(f2))            
        msg._buf += struct.pack('<BB', *msg.checksum(data=msg._buf[2:]))

    def format(self, msg):
        '''return a formatted string for a message'''
        if not msg._unpacked:
            self.unpack(msg)
        ret = self.name + ': '
        for f in self.fields:
            (fieldname, alen) = ArrayParse(f)
            if not fieldname in msg._fields:
                continue
            v = msg._fields[fieldname]
            if isinstance(v, list):
                ret += '%s=[' % fieldname
                for a in range(alen):
                    ret += '%s, ' % v[a]
                ret = ret[:-2] + '], '
            elif isinstance(v, str):
                ret += '%s="%s", ' % (f, v.rstrip(' \0'))
            else:
                ret += '%s=%s, ' % (f, v)
        for r in msg._recs:
            ret += '[ '
            for f in self.fields2:
                v = r[f]
                ret += '%s=%s, ' % (f, v)
            ret = ret[:-2] + ' ], '
        return ret[:-2]
        

# list of supported message types.
msg_types = {
    (CLASS_ACK, MSG_ACK_ACK)    : UBloxDescriptor('ACK_ACK',
                                                  '<BB', 
                                                  ['clsID', 'msgID']),
    (CLASS_ACK, MSG_ACK_NACK)   : UBloxDescriptor('ACK_NACK',
                                                  '<BB', 
                                                  ['clsID', 'msgID']),
    (CLASS_CFG, MSG_CFG_USB)    : UBloxDescriptor('CFG_USB',
                                                  '<HHHHHH32s32s32s',
                                                  ['vendorID', 'productID', 'reserved1', 'reserved2', 'powerConsumption',
                                                   'flags', 'vendorString', 'productString', 'serialNumber']),
    (CLASS_CFG, MSG_CFG_PRT)    : UBloxDescriptor('CFG_PRT',
                                                  '<BBHIIHHHH',
                                                  ['portID', 'reserved0', 'txReady', 'mode', 'baudRate', 'inProtoMask', 
                                                   'outProtoMask', 'reserved4', 'reserved5']),
    (CLASS_CFG, MSG_CFG_CFG)    : UBloxDescriptor('CFG_CFG',
                                                  '<III,B',
                                                  ['clearMask', 'saveMask', 'loadMask', 'deviceMask']),
    (CLASS_CFG, MSG_CFG_RST)    : UBloxDescriptor('CFG_RST',
                                                  '<HBB',
                                                  ['navBbrMask ', 'resetMode', 'reserved1']),
    (CLASS_CFG, MSG_CFG_SBAS)   : UBloxDescriptor('CFG_SBAS',
                                                  '<BBBBI',
                                                  ['mode', 'usage', 'maxSBAS', 'scanmode2', 'scanmode1']),
    (CLASS_CFG, MSG_CFG_GNSS)   : UBloxDescriptor('CFG_GNSS',
                                                  '<BBBBBBBBI',
                                                  ['msgVer', 'numTrkChHw', 'numTrkChUse', 'numConfigBlocks', 'gnssId',
                                                   'resTrkCh', 'maxTrkCh', 'resetved1', 'flags']),
    (CLASS_CFG, MSG_CFG_RATE)   : UBloxDescriptor('CFG_RATE',
                                                  '<HHH',
                                                  ['measRate', 'navRate', 'timeRef']),
    (CLASS_CFG, MSG_CFG_MSG)    : UBloxDescriptor('CFG_MSG',
                                                  '<BB6B',
                                                  ['msgClass', 'msgId', 'rates[6]']),
    (CLASS_NAV, MSG_NAV_POSLLH) : UBloxDescriptor('NAV_POSLLH',
                                                  '<IiiiiII', 
                                                  ['iTOW', 'Longitude', 'Latitude', 'height', 'hMSL', 'hAcc', 'vAcc']),
    (CLASS_NAV, MSG_NAV_VELNED) : UBloxDescriptor('NAV_VELNED',
                                                  '<IiiiIIiII', 
                                                  ['iTOW', 'velN', 'velE', 'velD', 'speed', 'gSpeed', 'heading', 
                                                   'sAcc', 'cAcc']),
    (CLASS_NAV, MSG_NAV_DOP)    : UBloxDescriptor('NAV_DOP',
                                                  '<IHHHHHHH', 
                                                  ['iTOW', 'gDOP', 'pDOP', 'tDOP', 'vDOP', 'hDOP', 'nDOP', 'eDOP']),
    (CLASS_NAV, MSG_NAV_STATUS) : UBloxDescriptor('NAV_STATUS',
                                                  '<IBBBBII', 
                                                  ['iTOW', 'gpsFix', 'flags', 'fixStat', 'flags2', 'ttff', 'msss']),
    (CLASS_NAV, MSG_NAV_SOL)    : UBloxDescriptor('NAV_SOL',
                                                  '<IihBBiiiIiiiIHBBI',
                                                  ['iTOW', 'fTOW', 'week', 'gpsFix', 'flags', 'ecefX', 'ecefY', 'ecefZ',
                                                   'pAcc', 'ecefVX', 'ecefVY', 'ecefVZ', 'sAcc', 'pDOP', 'reserved1', 
                                                   'numSV', 'reserved2']),
    (CLASS_NAV, MSG_NAV_POSUTM) : UBloxDescriptor('NAV_POSUTM',
                                                  '<Iiiibb',
                                                  ['iTOW', 'East', 'North', 'Alt', 'Zone', 'Hem']),
    (CLASS_NAV, MSG_NAV_SBAS)   : UBloxDescriptor('NAV_SBAS',
                                                  '<IBBbBBBBB',
                                                  ['iTOW', 'geo', 'mode', 'sys', 'service', 'cnt', 'reserved01', 'reserved02', 'reserved03' ],
                                                  'cnt',
                                                  'BBBBBBhHh',
                                                  ['svid', 'flags', 'udre', 'svSys', 'svService', 'reserved1',
                                                   'prc', 'reserved2', 'ic']),
    (CLASS_NAV, MSG_NAV_POSECEF): UBloxDescriptor('NAV_POSECEF',
                                                  '<IiiiI',
                                                  ['iTOW', 'ecefX', 'ecefY', 'ecefZ', 'pAcc']),
    (CLASS_NAV, MSG_NAV_VELECEF): UBloxDescriptor('NAV_VELECEF',
                                                  '<IiiiI',
                                                  ['iTOW', 'ecefVX', 'ecefVY', 'ecefVZ', 'sAcc']),
    (CLASS_NAV, MSG_NAV_TIMEGPS): UBloxDescriptor('NAV_TIMEGPS',
                                                  '<IihbBI',
                                                  ['iTOW', 'fTOW', 'week', 'leapS', 'valid', 'tAcc']),
    (CLASS_NAV, MSG_NAV_TIMEUTC): UBloxDescriptor('NAV_TIMEUTC',
                                                  '<IIiHBBBBBB',
                                                  ['iTOW', 'tAcc', 'nano', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid']),
    (CLASS_NAV, MSG_NAV_CLOCK)  : UBloxDescriptor('NAV_CLOCK',
                                                  '<IiiII',
                                                  ['iTOW', 'clkB', 'clkD', 'tAcc', 'fAcc']),
    (CLASS_NAV, MSG_NAV_DGPS)   : UBloxDescriptor('NAV_DGPS',
                                                  '<IihhBBH',
                                                  ['iTOW', 'age', 'baseId', 'baseHealth', 'numCh', 'status', 'reserved1'],
                                                  'numCh',
                                                  '<BBHff',
                                                  ['svid', 'flags', 'ageC', 'prc', 'prrc']),
    (CLASS_NAV, MSG_NAV_SVINFO) : UBloxDescriptor('NAV_SVINFO',
                                                  '<IBBH',
                                                  ['iTOW', 'numCh', 'globalFlags', 'reserved2'],
                                                  'numCh',
                                                  '<BBBBBbhi',
                                                  ['chn', 'svid', 'flags', 'quality', 'cno', 'elev', 'azim', 'prRes']),
    (CLASS_RXM, MSG_RXM_SVSI)   : UBloxDescriptor('RXM_SVSI',
                                                  '<IhBB',
                                                  ['iTOW', 'week', 'numVis', 'numSV'],
                                                  'numSV',
                                                  '<BBhbB',
                                                  ['svid', 'svFlag', 'azim', 'elev', 'age']),
    (CLASS_RXM, MSG_RXM_EPH)    : UBloxDescriptor('RXM_EPH',
                                                  '<II , 8I 8I 8I',
                                                  ['svid', 'how',
                                                   'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
    (CLASS_AID, MSG_AID_EPH)    : UBloxDescriptor('AID_EPH',
                                                  '<II , 8I 8I 8I',
                                                  ['svid', 'how',
                                                   'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
    (CLASS_AID, MSG_AID_AOP)    : UBloxDescriptor('AID_AOP',
                                                  '<B47B , 48B 48B 48B',
                                                  ['svid', 'data[47]', 'optional0[48]', 'optional1[48]', 'optional1[48]']),
    (CLASS_RXM, MSG_RXM_RAW)   : UBloxDescriptor('RXM_RAW',
                                                  '<ihBB',
                                                  ['iTOW', 'week', 'numSV', 'reserved1'],
                                                  'numSV',
                                                  '<ddfBbbB',
                                                  ['cpMes', 'prMes', 'doMes', 'sv', 'mesQI', 'cno', 'lli']),
    (CLASS_RXM, MSG_RXM_SFRB)  : UBloxDescriptor('RXM_SFRB',
                                                  '<BB10I',
                                                  ['chn', 'svid', 'dwrd[10]']),
    (CLASS_AID, MSG_AID_ALM)   : UBloxDescriptor('AID_ALM',
                                                  '<II',
                                                 '_remaining',
                                                 'I',
                                                 ['dwrd']),
    (CLASS_RXM, MSG_RXM_ALM)   : UBloxDescriptor('RXM_ALM',
                                                  '<II , 8I',
                                                  ['svid', 'week', 'dwrd[8]']),
    (CLASS_CFG, MSG_CFG_NAV5)   : UBloxDescriptor('CFG_NAV5',
                                                  '<HBBiIbBHHHHBBIII',
                                                  ['mask', 'dynModel', 'fixMode', 'fixedAlt', 'fixedAltVar', 'minElev', 
                                                   'drLimit', 'pDop', 'tDop', 'pAcc', 'tAcc', 'staticHoldThresh', 
                                                   'dgpsTimeOut', 'reserved2', 'reserved3', 'reserved4']),
    (CLASS_CFG, MSG_CFG_NAVX5)   : UBloxDescriptor('CFG_NAVX5',
                                                  '<HHIBBBBBBBBBBHIBBBBBBHII',
                                                  ['version', 'mask1', 'reserved0', 'reserved1', 'reserved2',
                                                   'minSVs', 'maxSVs', 'minCNO', 'reserved5', 'iniFix3D', 
                                                   'reserved6', 'reserved7', 'reserved8', 'wknRollover',
                                                   'reserved9', 'reserved10', 'reserved11',
                                                   'usePPP', 'useAOP', 'reserved12', 'reserved13', 
                                                   'aopOrbMaxErr', 'reserved3', 'reserved4']),
    (CLASS_MON, MSG_MON_HW)     : UBloxDescriptor('MON_HW',
                                                  '<IIIIHHBBBBIB25BHIII',
                                                  ['pinSel', 'pinBank', 'pinDir', 'pinVal', 'noisePerMS', 'agcCnt', 'aStatus',
                                                   'aPower', 'flags', 'reserved1', 'usedMask', 
                                                   'VP[25]',                                                  
                                                   'jamInd', 'reserved3', 'pinInq',
                                                   'pullH', 'pullL']),
    (CLASS_MON, MSG_MON_HW2)    : UBloxDescriptor('MON_HW2',
                                                  '<bBbBB3BI8BI4B',
                                                  ['ofsI', 'magI', 'ofsQ', 'magQ', 'cfgSource', 'reserved1[3]',
                                                   'lowLevCfg', 'reserved2[8]', 'postStatus', 'reserved3[4]']),
    (CLASS_MON, MSG_MON_SCHD)   : UBloxDescriptor('MON_SCHD',
                                                  '<IIIIHHHBB',
                                                  ['tskRun', 'tskSchd', 'tskOvrr', 'tskReg', 'stack',
                                                   'stackSize', 'CPUIdle', 'flySly', 'ptlSly']),
    (CLASS_MON, MSG_MON_VER)    : UBloxDescriptor('MON_VER',
                                                  '<30s10s,30s',
                                                  ['swVersion', 'hwVersion', 'romVersion'],
                                                  '_remaining',
                                                  '30s',
                                                  ['extension']),
    (CLASS_TIM, MSG_TIM_TP)     : UBloxDescriptor('TIM_TP',
                                                  '<IIiHBB',
                                                  ['towMS', 'towSubMS', 'qErr', 'week', 'flags', 'reserved1']),
    (CLASS_TIM, MSG_TIM_TM2)    : UBloxDescriptor('TIM_TM2',
                                                  '<BBHHHIIIII',
                                                  ['ch', 'flags', 'count', 'wnR', 'wnF', 'towMsR', 'towSubMsR', 
                                                   'towMsF', 'towSubMsF', 'accEst']),
    (CLASS_TIM, MSG_TIM_SVIN)   : UBloxDescriptor('TIM_SVIN',
                                                  '<IiiiIIBBH',
                                                  ['dur', 'meanX', 'meanY', 'meanZ', 'meanV',
                                                   'obs', 'valid', 'active', 'reserved1']),
    (CLASS_INF, MSG_INF_ERROR)  : UBloxDescriptor('INF_ERR', '<18s', ['str']),
    (CLASS_INF, MSG_INF_DEBUG)  : UBloxDescriptor('INF_DEBUG', '<18s', ['str'])
}


class UBloxMessage:
    '''UBlox message class - holds a UBX binary message'''
    def __init__(self):
        self._buf = b""
        self._fields = {}
        self._recs = []
        self._unpacked = False
        self.debug_level = 0

    def __str__(self):
        '''format a message as a string'''
        if not self.valid():
            return 'UBloxMessage(INVALID)'
        type = self.msg_type()
        if type in msg_types:
            return msg_types[type].format(self)
        return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())

    def __getattr__(self, name):
        '''allow access to message fields'''
        try:
            return self._fields[name]
        except KeyError:
            if name == 'recs':
                return self._recs
            raise AttributeError(name)

    def __setattr__(self, name, value):
        '''allow access to message fields'''
        if name.startswith('_'):
            self.__dict__[name] = value
        else:
            self._fields[name] = value

    def have_field(self, name):
        '''return True if a message contains the given field'''
        return name in self._fields

    def debug(self, level, msg):
        '''write a debug message'''
        if self.debug_level >= level:
            print(msg)

    def unpack(self):
        '''unpack a message'''
        if not self.valid():
            raise UBloxError('INVALID MESSAGE')
        type = self.msg_type()
        if not type in msg_types:
            raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
        msg_types[type].unpack(self)

    def pack(self):
        '''pack a message'''
        if not self.valid():
            raise UbloxError('INVALID MESSAGE')
        type = self.msg_type()
        if not type in msg_types:
            raise UBloxError('Unknown message %s' % str(type))
        msg_types[type].pack(self)

    def name(self):
        '''return the short string name for a message'''
        if not self.valid():
            raise UbloxError('INVALID MESSAGE')
        type = self.msg_type()
        if not type in msg_types:
            raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
        return msg_types[type].name
    
    if PYTHON_VERSION == 2:
        def msg_class(self):
            '''return the message class'''
            return ord(self._buf[2])

        def msg_id(self):
            '''return the message id within the class'''
            return ord(self._buf[3])
    else:
        def msg_class(self):
            '''return the message class'''
            return (self._buf[2])

        def msg_id(self):
            '''return the message id within the class'''
            return (self._buf[3])

    def msg_type(self):
        '''return the message type tuple (class, id)'''
        return (self.msg_class(), self.msg_id())

    def msg_length(self):
        '''return the payload length'''
        (payload_length,) = struct.unpack('<H', self._buf[4:6])
        return payload_length

    def valid_so_far(self):
        '''check if the message is valid so far'''
        if PYTHON_VERSION == 2:
            if len(self._buf) > 0 and ord(self._buf[0]) != PREAMBLE1:
                return False
            if len(self._buf) > 1 and ord(self._buf[1]) != PREAMBLE2:
                self.debug(1, "bad pre2")
                return False
        else:
            if len(self._buf) > 0 and (self._buf[0]) != PREAMBLE1:
                return False
            if len(self._buf) > 1 and (self._buf[1]) != PREAMBLE2:
                self.debug(1, "bad pre2")
                return False
                
        if self.needed_bytes() == 0 and not self.valid():
            if len(self._buf) > 8:
                self.debug(1, "bad checksum len=%u needed=%u" % (len(self._buf), self.needed_bytes()))
            else:
                self.debug(1, "bad len len=%u needed=%u" % (len(self._buf), self.needed_bytes()))
            return False
        return True

    def add(self, bytes):
        '''add some bytes to a message'''

        self._buf += bytes
        while not self.valid_so_far() and len(self._buf) > 0:
            '''handle corrupted streams'''
            self._buf = self._buf[1:]
        if self.needed_bytes() < 0:
            self._buf = b""

    def checksum(self, data=None):
        '''return a checksum tuple for a message'''
        if data is None:
            data = self._buf[2:-2]
        cs = 0
        ck_a = 0
        ck_b = 0
        for i in data:
            if type(i) is str:
               
                ck_a = (ck_a + ord(i)) & 0xFF
            else:
                ck_a = (ck_a + i) & 0xFF
            ck_b = (ck_b + ck_a) & 0xFF
        return (ck_a, ck_b)

    def valid_checksum(self):
        '''check if the checksum is OK'''
        (ck_a, ck_b) = self.checksum()
        d = self._buf[2:-2]
        (ck_a2, ck_b2) = struct.unpack('<BB', self._buf[-2:])
        return ck_a == ck_a2 and ck_b == ck_b2

    def needed_bytes(self):
        '''return number of bytes still needed'''
        if len(self._buf) < 6:
            return 8 - len(self._buf)
        return self.msg_length() + 8 - len(self._buf)

    def valid(self):
        '''check if a message is valid'''
        return len(self._buf) >= 8 and self.needed_bytes() == 0 and self.valid_checksum()


class UBlox:
    '''main UBlox control class.

    port can be a file (for reading only) or a serial device
    '''
    def __init__(self, port, baudrate=115200, timeout=0):

        self.serial_device = port
        self.baudrate = baudrate
        self.use_sendrecv = False
        self.read_only = False
        self.use_xfer = False
        self.debug_level = 0

        if self.serial_device.startswith("tcp:"):
            import socket
            a = self.serial_device.split(':')
            destination_addr = (a[1], int(a[2]))
            self.dev = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.dev.connect(destination_addr)
            self.dev.setblocking(1)
            self.dev.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)            
            self.use_sendrecv = True
        elif os.path.isfile(self.serial_device):
            self.read_only = True
            self.dev = open(self.serial_device, mode='rb')
        if self.serial_device.startswith("spi:"):
            import spidev
            bus, cs = map(int, self.serial_device.split(':')[1].split('.'))
            #print(bus, cs)
            self.use_xfer = True
            self.dev = spidev.SpiDev()
            self.dev.open(bus, cs)
            #We reuse baudrate parameter but it's difficult to get default paramaters right. So it's better to specify them explicitly 
            self.dev.max_speed_hz = baudrate
        else:
            import serial
            self.dev = serial.Serial(self.serial_device, baudrate=self.baudrate,
                                     dsrdtr=False, rtscts=False, xonxoff=False, timeout=timeout)
        self.logfile = None
        self.log = None
        self.preferred_dynamic_model = None
        self.preferred_usePPP = None
        self.preferred_dgps_timeout = None

    def close(self):
        '''close the device'''
        self.dev.close()
        self.dev = None

    def set_debug(self, debug_level):
        '''set debug level'''
        self.debug_level = debug_level

    def debug(self, level, msg):
        '''write a debug message'''
        if self.debug_level >= level:
            print(msg)

    def set_logfile(self, logfile, append=False):
        '''setup logging to a file'''
        if self.log is not None:
            self.log.close()
            self.log = None
        self.logfile = logfile
        if self.logfile is not None:
            if append:
                mode = 'ab'
            else:
                mode = 'wb'
            self.log = open(self.logfile, mode=mode)

    def set_preferred_dynamic_model(self, model):
        '''set the preferred dynamic model for receiver'''
        self.preferred_dynamic_model = model
        if model is not None:
            self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)

    def set_preferred_dgps_timeout(self, timeout):
        '''set the preferred DGPS timeout for receiver'''
        self.preferred_dgps_timeout = timeout
        if timeout is not None:
            self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)

    def set_preferred_usePPP(self, usePPP):
        '''set the preferred usePPP setting for the receiver'''
        if usePPP is None:
            self.preferred_usePPP = None
            return
        self.preferred_usePPP = int(usePPP)
        self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5)

    def nmea_checksum(self, msg):
        d = msg[1:]
        cs = 0
        for i in d:
            cs ^= ord(i)
        return cs

    def write(self, buf):
        '''write some bytes'''
        if not self.read_only:
            if self.use_sendrecv:
                return self.dev.send(buf)
            elif self.use_xfer:
                spiBuf = [] # form buf
                for b in buf:
                    if type(b) is str:
                        
                        spiBuf.append(ord(b))
                    else:
                        spiBuf.append(b)
                return self.dev.xfer2(spiBuf)
            return self.dev.write(buf)

    def read(self, n):
        '''read some bytes'''
        
        if self.use_sendrecv:
            import socket
            try:
                buf = self.dev.recv(n)
                return buf
            except socket.error as e:
                return b''
        if self.use_xfer:
            buf = self.dev.readbytes(n)
            return buf
            
        buf = self.dev.read(n)
        return buf

    def send_nmea(self, msg):
        if not self.read_only:
            s = msg + "*%02X" % self.nmea_checksum(msg)
            
            if PYTHON_VERSION == 2:
                b = bytearray()
                b.extend(s)
            else:
                b = bytearray()
                b.extend(map(ord, s))
            self.write(b)

    def set_binary(self):
        '''put a UBlox into binary mode using a NMEA string'''
        if not self.read_only:
            print("try set binary at %u" % self.baudrate)
            self.send_nmea("$PUBX,41,0,0007,0001,%u,0" % self.baudrate)
            self.send_nmea("$PUBX,41,1,0007,0001,%u,0" % self.baudrate)
            self.send_nmea("$PUBX,41,2,0007,0001,%u,0" % self.baudrate)
            self.send_nmea("$PUBX,41,3,0007,0001,%u,0" % self.baudrate)
            self.send_nmea("$PUBX,41,4,0007,0001,%u,0" % self.baudrate)
            self.send_nmea("$PUBX,41,5,0007,0001,%u,0" % self.baudrate)

    def seek_percent(self, pct):
        '''seek to the given percentage of a file'''
        self.dev.seek(0, 2)
        filesize = self.dev.tell()
        self.dev.seek(pct*0.01*filesize)

    def special_handling(self, msg):
        '''handle automatic configuration changes'''
        if msg.name() == 'CFG_NAV5':
            msg.unpack()
            sendit = False
            pollit = False
            if self.preferred_dynamic_model is not None and msg.dynModel != self.preferred_dynamic_model:
                msg.dynModel = self.preferred_dynamic_model
                sendit = True
                pollit = True
            if self.preferred_dgps_timeout is not None and msg.dgpsTimeOut != self.preferred_dgps_timeout:
                msg.dgpsTimeOut = self.preferred_dgps_timeout
                self.debug(2, "Setting dgpsTimeOut=%u" % msg.dgpsTimeOut)
                sendit = True
                # we don't re-poll for this one, as some receivers refuse to set it
            if sendit:
                msg.pack()
                self.send(msg)
                if pollit:
                    self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
        if msg.name() == 'CFG_NAVX5' and self.preferred_usePPP is not None:
            msg.unpack()
            if msg.usePPP != self.preferred_usePPP:
                msg.usePPP = self.preferred_usePPP
                msg.mask = 1<<13
                msg.pack()
                self.send(msg)
                self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5)

    def receive_message_nonblocking(self, seconds=5):
        '''nonblocking receive of one ublox message'''
        with Timeout(seconds=seconds):
            return self.receive_message()

    def receive_message(self, ignore_eof=False):
        '''blocking receive of one ublox message'''
        msg = UBloxMessage()
        while True:
            n = msg.needed_bytes()
            b = self.read(n)
            if not b:
                if ignore_eof:
                    time.sleep(0.01)
                    continue
                return None
            if self.use_xfer: 
                if PYTHON_VERSION == 3:
                    bb = bytearray()
                    for c in b:
                        bb.append(c)
                    b = bb
                else:
                    b = "".join([chr(c) for c in b]) # here str
            msg.add(b)
            if self.log is not None:
                self.log.write(b)
                self.log.flush()
            if msg.valid():
                self.special_handling(msg)
                return msg

    def receive_message_noerror(self, ignore_eof=False):
        '''blocking receive of one ublox message, ignoring errors'''
        try:
            return self.receive_message(ignore_eof=ignore_eof)
        except UBloxError as e:
            print(e)
            return None
        except OSError as e:
            # Occasionally we get hit with 'resource temporarily unavailable'
            # messages here on the serial device, catch them too.
            print(e)
            return None

    def send(self, msg):
        '''send a preformatted ublox message'''
        if not msg.valid():
            self.debug(1, "invalid send")
            return
        if not self.read_only:
            self.write(msg._buf)        

    def send_message(self, msg_class, msg_id, payload):
        '''send a ublox message with class, id and payload'''
        msg = UBloxMessage()
        msg._buf = struct.pack('<BBBBH', 0xb5, 0x62, msg_class, msg_id, len(payload))

        msg._buf += payload 

        (ck_a, ck_b) = msg.checksum(msg._buf[2:])
        msg._buf += struct.pack('<BB', ck_a, ck_b)
        self.send(msg)

    def configure_solution_rate(self, rate_ms=200, nav_rate=1, timeref=0):
        '''configure the solution rate in milliseconds'''
        payload = struct.pack('<HHH', rate_ms, nav_rate, timeref)
        self.send_message(CLASS_CFG, MSG_CFG_RATE, payload)

    def configure_message_rate(self, msg_class, msg_id, rate):
        '''configure the message rate for a given message'''
        payload = struct.pack('<BBB', msg_class, msg_id, rate)
        self.send_message(CLASS_CFG, MSG_CFG_SET_RATE, payload)

    def configure_port(self, port=1, inMask=3, outMask=3, mode=2240, baudrate=None):
        '''configure a IO port'''
        if baudrate is None:
            baudrate = self.baudrate
        payload = struct.pack('<BBHIIHHHH', port, 0xff, 0, mode, baudrate, inMask, outMask, 0xFFFF, 0xFFFF)
        self.send_message(CLASS_CFG, MSG_CFG_PRT, payload)

    def configure_loadsave(self, clearMask=0, saveMask=0, loadMask=0, deviceMask=0):
        '''configure configuration load/save'''
        payload = struct.pack('<IIIB', clearMask, saveMask, loadMask, deviceMask)
        self.send_message(CLASS_CFG, MSG_CFG_CFG, payload)

    def configure_poll(self, msg_class, msg_id, payload=b''):
        '''poll a configuration message'''
        self.send_message(msg_class, msg_id, payload)

    def configure_poll_port(self, portID=None):
        '''poll a port configuration'''
        if portID is None:
            self.configure_poll(CLASS_CFG, MSG_CFG_PRT)
        else:
            self.configure_poll(CLASS_CFG, MSG_CFG_PRT, struct.pack('<B', portID))

    def configure_min_max_sats(self, min_sats=4, max_sats=32):
        '''Set the minimum/maximum number of satellites for a solution in the NAVX5 message'''
        payload = struct.pack('<HHIBBBBBBBBBBHIBBBBBBHII', 0, 4, 0, 0, 0, min_sats, max_sats, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.send_message(CLASS_CFG, MSG_CFG_NAVX5, payload)

    def module_reset(self, set, mode):
        ''' Reset the module for hot/warm/cold start'''
        payload = struct.pack('<HBB', set, mode, 0)
        self.send_message(CLASS_CFG, MSG_CFG_RST, payload)

class TimeoutError(Exception):
    pass

import signal
class Timeout:
    def __init__(self, seconds=1, msg='Timeout'):
        self.seconds = seconds
        self.msg = msg
    def handle_timeout(self, signum, frame):
        raise TimeoutError(self.msg)
    def __enter__(self):
        signal.signal(signal.SIGALRM, self.handle_timeout)
        signal.alarm(self.seconds)
    def __exit__(self, type, value, traceback):
        signal.alarm(0)
