#!/usr/bin/env python
usage="""
Send and receive message from/to NovAtel receiver
Ports of OEM729:
  COM1,COM2,COM3,
  USB1,USB2,USB3,
  ICOM1 - ICOM7
"""

######################################################################
# Copyright Trimble 2022
######################################################################


import socket
import time


BYTE_LEN = 4096 #256
verbose = True

class Novatel_Socket:
  """\
  Wrapper to help send/recv commands to Novatel Receiver"""
  def __init__(self, IPAddr, port, timeout=1, output_format='abbreviated_ascii'):
    """\
    IPAddr = receiver address, e.g., '10.1.xxx.xxx'
    port = receiver port, e.g., 3005
    """
    self.IPAddr = IPAddr
    self.port = port
    self.selected_output_format = output_format
    self.output_format_dict = {'abbreviated_ascii':'', 'ascii':'a', 'binary':'b'}
    self.fmt = self.output_format_dict[self.selected_output_format]
    self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    self.sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
    self.sock.settimeout(timeout)
    self.sock.connect( (IPAddr,port) )
    # self.prompt_descriptor = self.recv_only().decode("utf-8") # COM1-serial, ICOM5-ethernet

  def send_only(self, cmd):
    """Returns data from command."""
    self.sock.sendall(cmd)
    return 

  def send_recv(self, cmd):
    """Returns data from command."""
    self.sock.sendall(cmd)
    return bytearray(self.sock.recv(BYTE_LEN))

  def recv_only(self, nbytes=BYTE_LEN):
    """Grab any data bytes returned to the port."""
    return bytearray(self.sock.recv(nbytes))

  def recv_all(self, nbytes=BYTE_LEN):
    """Grab any data bytes returned to the port."""
    msg = bytearray()
    try:
      while True:
        msg += bytearray(self.sock.recv(nbytes))
    except socket.timeout:
      return msg

  def close(self,sleep_time=0.5):
    """Close socket connection"""
    if self.sock is not None:
      try:
        self.sock.shutdown(socket.SHUT_RDWR)
      except:
        print('sock.shutdown() failed. Skip.')
      self.sock.close()
      time.sleep(sleep_time) # make sure socket has time to close
    self.sock = None

  def __del__(self):
    """Destructor"""
    self.close()

  def soft_reset(self):
    """Soft reset
    This cmd reboots Novatel receiver, but does not clean GPS Time
    """
    cmd = 'reset\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd)).decode("utf-8")
    return self.recv_all().decode("utf-8")

  def factory_reset(self, para='STANDARD'):
    """Soft reset
    Cautious! freset STANDARD isn't supposed to reset the Ethernet ... but it does!
    If receiver is accidently reset and ethernet port is off, try cat <(echo "ethconfig etha auto auto auto auto") | nc -w 2 10.1.xxx.xxx 10001
    10.1.xxx.xxx is Lantronix IP addr. 10001 is the port, which Novatel is connected with through RS-232.
    """
    cmd = 'freset '+ para +'\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return 

  def reset(self):
    """
    Reset each data set and performance a reboot.
    Without the above, during at least one run the Novatel unit got confused and tracked very few Galileo satellites. 
    We aren't resetting GLONASS or BDS, monitor to see if we need to add more resets
    """
    if verbose:print("Reseting... This process takes 2.5 min.")
    self.factory_reset('gpsalmanac')
    time.sleep(30)
    self.factory_reset('GALFNAV_EPH')
    time.sleep(30)
    self.factory_reset('GALINAV_EPH')
    time.sleep(30)
    self.factory_reset('GALFNAV_ALM')
    time.sleep(30)
    self.factory_reset('GALINAV_ALM')
    time.sleep(30)
    return

  def get_ip_status(self):
    """Get ethernet connection status
    """
    cmd = 'log ipstatus'+self.fmt+' once\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return self.recv_all().decode("utf-8")

  def get_recv_status(self):
    """Get USB connection status
      RXSTATUSA for ASCII msg response
      RXSTATUSB for binary return response
    """
    cmd = 'log RXSTATUS'+self.fmt+' once\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return self.recv_all().decode("utf-8")

  def get_recv_config(self):
    """Get receiver configuration
    This log is used to output a list of all current command settings
    """
    cmd = 'log RXCONFIG'+self.fmt+' once\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return self.recv_all().decode("utf-8")

  def set_it_prog_config(self, switch_on=True, filter_mode='notchfilter',cut_off_freq='1580.42',notch_width='0.15', frequency='gpsl1'):
    '''
    ITPROGFILTCONFIG
    Use this command to set the programmable filter to be either a notch filter or a bandpass filter to mitigate interference in the pass band of GNSS signals. The
    cmd: ITPROGFILTCONFIG frequency filterid switch [filtermode] [cutofffreq] [notchwidth]
    eg.  ITPROGFILTCONFIG gpsl1 pf0 enable notchfilter 1580 1
    '''
    if switch_on:
      if filter_mode == 'notchfilter':
        cmd = 'ITPROGFILTCONFIG '+frequency+' pf0 enable notchfilter '+cut_off_freq+' '+notch_width+'\n'
      elif filter_mode == 'bandpassfilter':
        cmd = 'ITPROGFILTCONFIG '+frequency+' pf0 enable bandpassfilter '+cut_off_freq+'\n'
      else:
        cmd = 'ITPROGFILTCONFIG '+frequency+' pf0 disable\n'
    else:
      cmd = 'ITPROGFILTCONFIG '+frequency+' pf0 disable\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return

  def get_it_prog_config(self):
    '''
    The ITFILTTABLE log contains the filter configuration summary for each frequency 
    '''
    cmd = 'log itfilttable once\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return self.recv_all().decode("utf-8")

  def set_ethernet_config(self):
    cmd = 'ethconfig etha auto auto auto auto\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return

  def get_ethernet_config(self):
    cmd = 'log ethconfig'+self.fmt+' once\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return self.recv_all().decode("utf-8")

  def get_version(self):
    """Get version
    """
    cmd = 'log VERSION'+self.fmt+' once\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return self.recv_all().decode("utf-8")

  def set_elev_mask(self,angle_degree=0):
    """Set elevation mask
    """
    cmd = 'ELEVATIONCUTOFF ALL '+str(angle_degree)+'\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return 

  def get_elev_mask(self):
    """Get elevation mask
    """
    cmd = 'log ELEVATIONCUTOFF'+self.fmt+' once\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return self.recv_all().decode("utf-8")

  def set_channel_config_mode(self, mode=4):
    """Set channel mode.
    If a different channel configuration is selected via the SELECTCHANCONFIG
    command, the receiver resets and starts up with the new configuration.
    """
    cmd = 'SELECTCHANCONFIG '+str(mode) + '\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return

  def get_channel_config_mode(self):
    """Get channel mode
    """
    cmd = 'log chanconfiglist'+self.fmt+' once\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return self.recv_all().decode("utf-8")

  def unlogall(self):
    """Force to unlog the RXSTATUSEVENTA messages
    Default receiver status event logging will be disable.
    '#LOGA PORT? RXSTATUSEVENTA'  is now removed from the output of 'log RXCONFIGA once'
    """
    cmd = 'unlogall true' + '\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return

  def set_dlltime(self, sig='GPSL1CA', timeconst=100):
    """This command sets the amount of carrier smoothing performed on the code measurements
    """
    cmd = "dlltimeconst "+ sig + " " + str(timeconst) + '\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return
  
  def passthrough_log(self, port, meas, interval):
    """By pass measurement to port
    """
    cmd = "log "+ port + " " + meas + " ONTIME " + str(interval) + '\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return

  def set_pdpfilter(self, on=True):
    """This command is used to enable, disable or reset the Pseudorange/Delta-Phase (PDP) filter.
    """
    setting = 'DISABLE' if on==False else 'ENABLE'
    cmd = "PDPFILTER " + setting + '\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return

  def get_rxconfig(self):
    """Get receiver all configuration
    """
    cmd = 'log rxconfig'+self.fmt+' once\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return self.recv_all().decode("utf-8")

  def set_interface_mode(self, port, rxtype, txtype, responses='OFF'):
    """This command is used to specify what type of data a particular port on the receiver can transmit and receive.
    """
    cmd = "interfacemode " + port + " " + rxtype + " " + txtype + '\n'
    if verbose: print('Send cmd:', cmd)
    self.send_only(str.encode(cmd))
    return


if __name__ == "__main__":

  import argparse

  parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter,
                                    description=usage)
  parser.add_argument('-t','--test',
                      help='If run routine receiver reset for Anti Jam Test',
                      action="store_true")
  parser.add_argument('-i','--info',
                      help='Show receiver info',
                      action="store_true")
  
  args = parser.parse_args()
  
  if args.info:
  
    Nov = Novatel_Socket('10.1.xxx.xxx', 3005, output_format='abbreviated_ascii' )
    print('-----------------------------------------')
    rt = Nov.get_ip_status()
    print(rt)

    print('-----------------------------------------')
    rt = Nov.get_ethernet_config()
    print(rt)

    print('-----------------------------------------')
    rt = Nov.get_version()
    print(rt)
  
    del(Nov)

  if args.test:
    print("Run routine receiver reset for Anti Jam Test")
    print("ICOM2 is used to stream NMEA")
    print("ICOM3 is used to receive RTCM data for RTK")
    print("ICOM4 is used to stream observables and positions")

    ###############################
    # 1-3. Reset through ethernet #
    ###############################
    # According to manual, using SAVEETHERNETDATA will prevent FRESET to erase ethernet setting
    msg = "1. Reset through ethernet. Assuming ethernet setting is saved by sending cmd SAVEETHERNETDATA."
    print(msg)
    Nov = Novatel_Socket('10.1.xxx.xxx', 3005, output_format='abbreviated_ascii' )
    Nov.factory_reset()
    del(Nov)
    print("Sleep 30 sec ...")
    time.sleep(30)

    msg = "2. Select channel configuration"
    print(msg) 
    Nov = Novatel_Socket('10.1.xxx.xxx', 3005, output_format='abbreviated_ascii' )
    Nov.set_channel_config_mode(4)
    del(Nov)
    print("Sleep 30 sec ...")
    time.sleep(30)

    # 4. Configure Receiver through ethernet
    msg = "4. Configure Receiver through ethernet"
    print(msg)    
    Nov = Novatel_Socket('10.1.xxx.xxx', 3005, output_format='abbreviated_ascii' )

    # Check channel configuration
    rt = Nov.get_channel_config_mode()
    print('\n'.join(rt.split('\r\n')[:4]))

    # Set to 100 - default CC setting
    all_sigs="""GPSL1CA GPSL2Y GPSL2P GPSL2C GPSL5 
              GLOL1CA GLOL2CA GLOL2P
              GALE1 GALE5A GALE5B GALALTBOC
              SBASL1 SBASL5
              BDSB1D1 BDSB1D2 BDSB2D1 BDSB2D2 BDSB3D1 BDSB3D2 BDSB1C BDSB2A BDSB2BI
              QZSSL1CA QZSSL2CM QZSSL5
              """

    for sig in all_sigs.replace('\n','').split(" "):
      if len(sig) > 0:
        Nov.set_dlltime(sig, 100)

    # "true" should force it to also unlog the RXSTATUSEVENTA messages
    Nov.unlogall()

    # Stream observables and positions on ICOM4 (TCP/IP port 3004)
    port = 'ICOM4'
    interval = '0.2'
    for meas in ['RANGEA', 'BESTPOSA', 'BESTVELA']:
      Nov.passthrough_log(port, meas, interval)

    # Stream NMEA on ICOM2 (TCP/IP port 3002)
    port = 'ICOM2'
    interval = '0.2'
    for meas in ['GPGGALONG','GPZDA', 'GPGST', 'GPGSV', 'GPVTG', 'GPGSA']:
      Nov.passthrough_log(port, meas, interval)

    # Set up to accept RTCM on port ICOM3 (port 3003)
    Nov.set_interface_mode('ICOM3','RTCMV3','NOVATEL', 'OFF')

    # Disable the PDP filter.
    Nov.set_pdpfilter(False)

    # Set to zero degrees to match our testing
    Nov.set_elev_mask(0)

    del(Nov)