#####################################################################
# $Id: novatel_cmds.py,v 1.1 2017/11/17 20:59:39 acartmel Exp $
# 
# Copyright Trimble Inc. 2017
#
# Novatel interface script
# Requires the pySerial package to be installed
#
# 1) Initial receiver set-up : enable ethernet ports, enable tracking
#    of all signals, enable SBAS, set QZSS elevation mask to zero
#
# 2) Enable ethernet ports only
#
# 3) Streaming of range and position data : RANGEA, BESTPOSA and
#    BESTVELA messages.
#####################################################################

import sys
import serial


#####################################################################
# Display the expected command line on errors
def expected_command_line() :
  print( 'Expected command line is:' )
  print( '  python novatel_cmds.py <port> --init_config' );
  print( '  python novatel_cmds.py <port> --get_ipaddr' );
  print( '  python novatel_cmds.py <port> <logging rate [s]>' );
  print( 'Where:' );
  print( '  port = COM port (i.e. "COM1")' )
  print( '         or TCP/IP with port (i.e. "10.1.150.48:3002")' )
  print( '  logging rate = logging rate in seconds (i.e. "1" or "0.1")' )
  print( 'The logging output stream can be piped directly to a file' )


#####################################################################
# Is the logging rate a valid number?
def is_number( val ) :
  try :
    float( val )
  except :
    return False

  return True


#####################################################################
# Write a command to the OEM7
# Waits for the new COM/USB prompt and optional user response before
# returning
# If a user response is supplied, the response containing this string
# is returned, otherwise returns an empty string
# 
# ser  - Serial port/socket object
# cmd  - Command string to send
# desc - String to display, use '' to suppress
# resp - Wait for given response before returning, use '' to suppress
def write_oem7( ser, cmd, desc, resp ) :
  # Wait for [xCOMn] or [USBn] prompt response before returning
  have_prompt = False
  # Wait for optional response string
  have_resp = False
  if ( len(resp) == 0 ) :
    have_resp = True
  # Default to empty string
  ret = ''
  # Time-out counter
  tout = 0

  # Ensure the commands are terminated
  # and converted to bytes
  if ( len(cmd) ) :
    cmd = cmd+'\n'
    cmd = cmd.encode()
    ser.write( cmd )

  # Print command description
  if ( len(desc) ) :
    print( desc )

  # Wait for the command to complete and any
  # response
  while( True ) :
    line = ser.readline().decode()
    # Expect [xCOMn] or [USBn] prompt on command completion
    if 'COM' in line or 'USB' in line :
      have_prompt = True
    # Check for expected command response
    if resp in line :
      have_resp = True
      ret = line
    # Return when prompt and any response returned
    if have_prompt and have_resp :
      return ret
    # Time-out if the response takes too long
    tout = tout + 1
    if tout >= 10 :
      print( 'Command "' + cmd[0:-1].decode() + '" response timed-out' )
      return ret


#####################################################################
# Read a response of a given type from the OEM7
# Returns 1) True/False if response is found
#         2) Response string or empty string respectively
# 
# ser  - Serial port/socket object
# resp - Wait for given one of the list of responses before returning
def read_oem7( ser, resp ) :
  # Wait for response string
  have_resp = False
  # Time-out counter
  tout = 0

  # Wait for the command to complete and any
  # response
  while( True ) :
    line = ser.readline().decode()
    for tst in resp :
      if tst in line :
        have_resp = True
        ret = line
        return ret

    tout = tout + 1
    if tout >= 10 :
      print( 'Read "' + resp + '" timed-out' )
      return ret
  


#####################################################################
# Parse the #IPSTATUS response string
# The response has been seen to fail, return an IP address of 0.0.0.0
# and needs a retry option
def parse_ipstatus( ipstatus ) :
  tmp = ipstatus.split(',')
  ipaddr = tmp[11][1:-1]
  if '0.0.0.0' in ipaddr :
    return False, ipaddr
  else :
    return True, ipaddr


#####################################################################
# Enable the ethernet controller and detect the IP address
def enable_ethernet( ser ) :
  write_oem7( ser, 'ETHCONFIG ETHA AUTO AUTO AUTO AUTO', 'Enable ethernet port', 'OK' )

  timeout = 0
  while True :
    ipstatus = write_oem7( ser, 'LOG IPSTATUSA ONCE', 'Obtaining IP address (can be slow)', '#IPSTATUS' )
    ret, ipaddr = parse_ipstatus( ipstatus )
    if  ret :
      return True, ipaddr
    else :
      timeout = timeout + 1
      if timeout < 10 :
        print( 'Retry...' )
      else :
        return False, '0.0.0.0'


#####################################################################
# Main script start point

# Check the user supplied command line
if len(sys.argv) != 3 :
  expected_command_line()
  sys.exit()

# Determine the requested port type, COM port or TCP/IP
port = sys.argv[1].lower()
if 'com' in port or 'ttyS' in port :
  is_com = True
else :
  is_com = False
  port = 'socket://'+port

# Command modes :
# --init_config
# --get_ipaddr
# logging rate in seconds
mode = sys.argv[2]
if mode != '--init_config' and mode != '--get_ipaddr' and is_number( mode ) != True :
  print( 'Invalid command line' )
  expected_command_line()
  sys.exit()

# Open the port
with serial.serial_for_url( port, timeout=1 ) as ser :
  if is_com is True :
    # Set the baudrate for serial connection
    # Not needed for TCP/IP
    # There's not always a prompt to clear to don't wait for one
    ser.baudrate = 38400
  else :
    # Wait for and clear the initial connection prompt
    write_oem7( ser, '', 'Wait for connection ...', '' )

  ############################################
  # Initial receiver configuration
  if mode == '--init_config' :
    gotip, ipaddr = enable_ethernet( ser )
    write_oem7( ser, 'SBASCONTROL enable auto', 'SBAS enabled, auto-SV selection', 'OK' )
    write_oem7( ser, 'QZSSECUTOFF 0.0', 'QZSS elevation mask 0deg', 'OK' )
    write_oem7( ser, 'SAVECONFIG', 'Saving configuration', 'OK' )
    write_oem7( ser, 'SETCHANCONFIG 5', 'QZSS elevation mask 0deg', 'OK' )
    print( 'Receiver will now reboot' )
    print( '' )
    if gotip is True :
      print( 'Receiver IP address is ' + ipaddr )
      print( 'Default IP ports ' + ipaddr + ':3001 to ' + ipaddr + ':3007' )
    else :
      print( 'Failed to get valid IP address. Is it connected?' )

  ############################################
  # Get the IP address of the receiver
  elif mode == '--get_ipaddr' :
    gotip, ipaddr = enable_ethernet( ser )
    if gotip is True :
      print( 'Receiver IP address is ' + ipaddr )
      print( 'Default IP ports ' + ipaddr + ':3001 to ' + ipaddr + ':3007' )
    else :
      print( 'Failed to get valid IP address. Is it connected?' )

  ############################################
  # Data logging
  else :
    write_oem7( ser, 'LOG RANGEA ONTIME ' + mode, '', 'OK' )
    #write_oem7( ser, 'LOG BESTPOSA ONTIME ' + mode, '', 'OK' )
    #write_oem7( ser, 'LOG BESTVELA ONTIME ' + mode, '', 'OK' )
    sys.stderr.write( 'Logging started...\n' )

    shutdown = False
    while shutdown is False :
      try :
        line = read_oem7( ser, ['#RANGEA', '#BESTPOSA', '#BESTVELA'] )
        if len(line) > 0 :
          print( line )
      except KeyboardInterrupt :
        sys.stderr.write( 'Shutdown detected...\n' )
        write_oem7( ser, 'LOG RANGEA 0', '', 'OK' )
        write_oem7( ser, 'LOG BESTPOSA 0', '', 'OK' )
        write_oem7( ser, 'LOG BESTVELA 0', '', 'OK' )
        shutdown = True
        sys.stderr.write( 'Shutdown complete\n' )

  # Done - close the port
  ser.close()