#!/usr/bin/python3

import pynmea2
import sys

dev = "/dev/gnss0"
logfname = "/tmp/gps.log"
loops = 50


def write_cmd(port, cmd, expect=None):
    print("cmd: %s" % cmd)
    port.write(str(cmd).encode("ascii"))
    port.write(b'\r\n')
    # port.flush()
    if expect:
        for i in range(20):
            line = port.readline()[:-1]
            sys.stderr.write('read: %s\n' % line)
            if expect.encode("ascii") in line:
                sys.stderr.write('found: %s:%s\n' % (expect, line))
                return True, line

        sys.stderr.write('not found: %s:%s\n' % (expect, line))
        return False, line

    # wait for cmd completion
    for i in range(20):
        line = port.readline()[:-1]
        sys.stderr.write('read: %s\n' % line)
        if str(cmd).encode("ascii") in line:
            return True, line

    return False, line


def stm_pause(port):
    msg = pynmea2.GGA('P', 'STMGPSSUSPEND')
    write_cmd(port, msg)


def stm_restart(port):
    msg = pynmea2.GGA('P', 'STMGPSRESTART')
    write_cmd(port, msg)


def get_version(port):
    msg = pynmea2.GGA('P', 'STMGETSWVER', ("0"))
    write_cmd(port, msg, "PSTMVER")
    msg = pynmea2.GGA('P', 'STMGETSWVER', ("1"))
    write_cmd(port, msg, "PSTMVER")
    msg = pynmea2.GGA('P', 'STMGETSWVER', ("2"))
    write_cmd(port, msg, "PSTMVER")
    msg = pynmea2.GGA('P', 'STMGETSWVER', ("6"))
    write_cmd(port, msg, "PSTMVER")
    msg = pynmea2.GGA('P', 'STMGETSWVER', ("7"))
    write_cmd(port, msg, "PSTMVER")
    msg = pynmea2.GGA('P', 'STMGETSWVER', ("11",))
    write_cmd(port, msg, "PSTMVER")
    msg = pynmea2.GGA('P', 'STMGETSWVER', ("12",))
    write_cmd(port, msg, "PSTMVER")
    # dumping the configuration sometimes tries to print binary characters
    # msg = pynmea2.GGA('P', 'STMGETSWVER', ("254",))
    # write_cmd(port, msg )


def rf_test(port, sat_id, enable):
    if enable:
        msg = pynmea2.GGA('P', 'STMRFTESTON', (str(sat_id)))
    else:
        msg = pynmea2.GGA('P', 'STMRFTESTOFF')
    write_cmd(port, msg)


def set_constellation(port, cons_mask):
    '''
    bit 0: GPS constellation enabling/disabling
    bit 1: GLONASS constellation enabling/disabling
    bit 2: QZSS constellation enabling/disabling
    bit 3: GALILEO constellation enabling/disabling
    bit 7: BEIDOU constellation enabling/disabling
    '''

    msg = pynmea2.GGA('P', 'STMSETCONSTMASK', (str(cons_mask)))
    write_cmd(port, msg)


def set_notch(port, type, mode):
    '''
    type    Sat type ANF path [0 -> GPS; 1->GLONASS]
    mode    ANF operation mode [0, disable, 1 always on, 2 Auto (suggested)]
    '''
    msg = pynmea2.GGA('P', 'STMNOTCH', (str(type), str(mode)))
    write_cmd(port, msg)


def main(args):
    # try to open serial port
    sys.stderr.write('Trying port %s\n' % dev)
    try:
        ser = open(dev, "w+b", buffering=0)
        # 'warm up' with reading some input
        for i in range(10):
            line = ser.readline()
            sys.stderr.write('read: %s\n' % line)
        # try to parse (will throw an exception if input is not valid NMEA)
        pynmea2.parse(ser.readline().decode('ascii', errors='replace'))

        get_version(ser)
        set_constellation(ser, 3)
        set_notch(ser, 0, 1)

        sys.stderr.write('Logging data on %s to %s\n' % (dev, logfname))
        with open(logfname, 'wb') as f:
            # loop will exit with Ctrl-C, which raises a
            # KeyboardInterrupt
            for i in range(loops):
                line = ser.readline()
                print(line.decode('ascii', errors='replace').strip())
                f.write(line)

    except Exception as e:
        sys.stderr.write('Error reading serial port %s: %s\n' % (type(e).__name__, e))
    except KeyboardInterrupt:
        sys.stderr.write('Ctrl-C pressed, exiting log of %s to %s\n' % (dev, logfname))
        sys.exit(0)


if __name__ == "__main__":
    sys.exit(main(sys.argv))
