#!/usr/bin/env python


import serial
import struct

ser = serial.Serial("/dev/ttyUSB0", 115200, timeout=2)

msgid = {
    0x10 :  "GPS",
    0x20 :  "MAG"
}

msgid_inv = {
    "GPS"   :   0x10,
    "MAG"   :   0x20
}

fixtype =   {
    0x00    :   "No Fix",
    0x02    :   "2D Lock",
    0x03    :   "3D Lock"
}

def ParseDateTime(data):
    seconds =   (data & 0x3F        )   >>  0
    minutes =   (data & 0xFC0       )   >>  6
    hours   =   (data & 0xF000      )   >>  12
    day     =   (data & 0x1F0000    )   >>  16
    month   =   (data & 0x1E00000   )   >>  21
    year    =   (data & 0xFE000000  )   >>  25
    return {"seconds":seconds,"minutes":minutes,"hours":hours,"day":day,"month":month,"year":year}

def bin(s):
    return str(s) if s<=1 else bin(s>>1) + str(s&1)

def GenMagMask(b9):
    b9b =   bin(b9).rjust(8,"0")
    _0  =   int(b9b[7])
    _1  =   int(b9b[6])
    _2  =   int(b9b[5])
    _3  =   int(b9b[4])
    _4  =   int(b9b[3])
    _5  =   int(b9b[2])
    _6  =   int(b9b[1])
    _7  =   int(b9b[0])
    mask        =   [0,0,0,0,0,0,0,0]
    mask[7]     =   _0  ^   _4
    mask[6]     =   _1  ^   _5
    mask[5]     =   _2  ^   _6
    mask[4]     =   _3  ^   _7  ^   _0
    mask[3]     =   _1
    mask[2]     =   _2
    mask[1]     =   _3
    mask[0]     =   _4  ^   _0
    mask = int("".join([str(x) for x in mask]), 2)
    return mask

while True:
    buff = ser.read(2)
    if buff == "\x55\xAA":
        #print "Received message! Decoding"
        id = ser.read(1)
        size   = ser.read(1)
        #print "Message size: %s Message ID: %s" %(hex(ord(size[0])),hex(ord(id[0])))
        buff = ser.read(ord(size[0]))
        #print "Message: %s" %buff
        checksum = ser.read(2)
        if  ord(id) == msgid_inv["GPS"]:
            xormask     =   struct.unpack("B", buff[55])[0]
            sequence    =   struct.unpack("H", buff[55:57])[0]
            numSat      =   struct.unpack("B", buff[48])[0]
            
            print "Mask: %s" %xormask
            
            buff = bytearray(buff)
            for i in range(len(buff)):
                buff[i] = buff[i] ^ xormask
            buff = str(buff)
            
            datetime    =   struct.unpack("I", buff[0:4])[0]
            
            latitude    =   struct.unpack("i", buff[4:8])[0]
            longitude   =   struct.unpack("i", buff[8:12])[0]
            altitude    =   struct.unpack("i", buff[12:16])[0]
            
            horzacc     =   struct.unpack("I", buff[16:20])[0]
            vertacc     =   struct.unpack("I", buff[20:24])[0]
            
            zero0       =   struct.unpack("I", buff[24:28])[0]
            
            nedNV       =   struct.unpack("I", buff[28:32])[0]
            nedEV       =   struct.unpack("I", buff[32:36])[0]
            nedDV       =   struct.unpack("I", buff[36:40])[0]
            
            posDOP      =   struct.unpack("H", buff[40:42])[0]
            verDOP      =   struct.unpack("H", buff[42:44])[0]
            norDOP      =   struct.unpack("H", buff[44:46])[0]
            easDOP      =   struct.unpack("H", buff[46:48])[0]
            
            zero1       =   struct.unpack("B", buff[49])[0]
            fixtype     =   struct.unpack("B", buff[50])[0]
            zero2       =   struct.unpack("B", buff[51])[0]
            fixstatus   =   struct.unpack("B", buff[52])[0]
            
            zero3       =   struct.unpack("H", buff[52:54])[0]
            
            
            print "%s %s" %(latitude/1e7,longitude/1e7)
            #print datetime
            print ParseDateTime(datetime)
        elif ord(id) == msgid_inv["MAG"]:
            buff    =   bytearray(buff)
            bmask       =   buff[4]
            xormask     =   GenMagMask(bmask)
            for i in range(len(buff)):
                if i != 4:
                    buff[i] ^= xormask
            buff    =   str(buff)
            magx    =   struct.unpack("h", buff[0:2])[0]
            magy    =   struct.unpack("h", buff[2:4])[0]
            magz    =   struct.unpack("h", buff[4:6])[0]
            print "Mag(%s,%s,%s)" %(magx,magy,magz)