#########################################################################
#
# Control functions for the SG6000Pro Ethernet controlled synthesizer.
#
# Provides both basic setup and a function that changes the power level
# based on distance to a simulated grid of jammers. 
#
# Copyright Trimble 2019
#
#########################################################################

import time 
import socket
import RXTools
import mutils
import numpy as np

#############################
# Setup constants
#############################

# Center the grid
all_ref_llh = [np.array([37.3344150,-121.8913375,0]), # downtown San Jose
               np.array([48.129881, 11.592890,0]),    # downtown Munich
               np.array([35.684296, 139.753109,0])    # downtown Tokyo
              ]
# Let's assume the jammers are on a 2km grid
gridSpace = 2000.0
# We're jamming L1 GPS - use this for the free space calc.
FreqFactor = 20.0*np.log10(1.57542e9)

#################################################################
# Globals - keep track of the HW to avoid duplicate writes
#################################################################

# True when the Synth's RF output is enabled
RFOutputEnabled = False

# Keep track of the output power to avoid unnecessary writes
RFProgrammedPower = -100

def find_closest_ref( llh ):
  all_dist = [mutils.comp_3d_dist(llh,ref_llh) for ref_llh in all_ref_llh]
  if min(all_dist) > 1000e3:
    print("Warning - long distance %.1f [m]" % min(all_dist))
  return all_ref_llh[mutils.argmin( all_dist )]

# Sends a command over TCP/IP to the SG60000PRO synthesizer
def sendCommand(openSocket,command,payload,debug,delay=0.05):
  localCommand = command
  if(payload):
    localCommand = command + ' ' + payload
  localCommand += '\n'
    
  openSocket.sendall(bytes(localCommand,'utf-8'))

  if(debug):
    # Get the reponse dropping the return
    myResponse = openSocket.recv(100).decode('utf-8')[:-1] 
  else:
    # some commands don't have a response or we don't care what it is
    myResponse = None

  if(delay > 0):
    time.sleep(delay)
  return(myResponse)

# Gets the position from the "RefIP" receiver, uses this to set the
# power level of the SG60000PRO Synth
def updateJamPower(openSocket,RefIP):
  global RFOutputEnabled
  global RFProgrammedPower
  
  Week = None
  Secs = None
  Lat = None
  Lon = None
  FullRange = None  # To the closest tower
  PowerRet = None # Power returned by the device

  try:
    Week,Secs,Lat,Lon,Hgt = RXTools.getLLH(RefIP,'admin','password')
    if(Lat is not None):
      llh = np.array([Lat,Lon,Hgt])
      ref_llh = find_closest_ref( llh )
      ENU = mutils.llh2enu( llh, ref_llh)

      E = ENU[0][0]
      N = ENU[1][0]

      Eerr = gridSpace * np.abs(E/gridSpace - np.round(E/gridSpace))
      Nerr = gridSpace * np.abs(N/gridSpace - np.round(N/gridSpace))
      FullRange = np.sqrt(Eerr*Eerr + Nerr*Nerr)
      Range = FullRange
        
      # Everything within a 400m radius of the jammer gets the 
      # same power - enough to completely knock out L1 GPS.
      if(Range < 400):
        Range = 400

      if(Range > 800):
        # If we exceed the maximum jam radius turn off the synthesizer
        if(RFOutputEnabled == True):
          sendCommand(openSocket,'OUTP:STAT','OFF',False,delay=0.4)
          RFOutputEnabled = False

        # set to a really low value as we aren't jamming. This does
        # not impact the HW, but will be returned to the caller
        PowerRet = -200.0 
      else:
        ################################
        # Start Power model
        ################################

        # Start with a classic free space path loss (FSPL) model as
        # a function of range to the simulated transmitter.

        # Normalize to the pass loss at 400m (-88.4391)
        FSPL = 20.0*np.log10(Range) + FreqFactor - 147.55 - 88.4391 
       
        # A little scaling to make the loss roll off quicker than
        # free space and to test the receiver over a larger dynamic
        # range
        FSPL *= 10.3
        
        # Convert the loss to a power based on a maximum of 10dBm at
        # 0 - 400m
        Power = 10.0 - FSPL
        
        ################################
        # End Power model
        ################################

        # Clip the power to keep it in range of the device
        if(Power > 10.0):
          # Maximum the SG6000PRO supports in dBm
          Power = 10.0
        elif(Power < -52.0):
          # Minimum the SG6000PRO supports in dBm
          Power = -52.0

        # Attenuator only accepts 0.25dB steps
        Power = np.round(Power*4.0) / 4.0

        # Only update the HW if we need to
        if(Power != RFProgrammedPower):
          sendCommand(openSocket,'POWER','{:.2f}'.format(Power),False)
          # Get the actual power from the instrument - it should match,
          # at least if it doesn't we have a record (we could resend the 
          # command if it doesn't match, for now don't bother).
          PowerRet = float(sendCommand(openSocket,'Power?','',True).rsplit('dBm')[0])
          RFProgrammedPower = PowerRet
        else:
          PowerRet = RFProgrammedPower 
        
        if(RFOutputEnabled == False):
          # Make sure the output is on
          sendCommand(openSocket,'OUTP:STAT','ON',False,delay=0.4)
          RFOutputEnabled = True
  except:
    print("Caught an Error!!")

  return(Week, Secs, Lat, Lon, FullRange, PowerRet)

# Initialize the synthesizer to slowly scan close to the center
# frequency of GPS L1
def initializeSynth(openSocket):
  ret = sendCommand(openSocket,'*IDN?','',True)
  print(ret)

  # Per the documentation turning off the display and buzzer speeds the access
  sendCommand(openSocket,'*DISPLAY','OFF',False)
  sendCommand(openSocket,'*BUZZER OFF','',False)
  
  # Make sure the output is disabled by default
  sendCommand(openSocket,'OUTP:STAT','OFF',False,delay=0.4)
  RFOutputEnabled = False
  
  # Make sure any sweep stops now
  sendCommand(openSocket,'ABORT','',False)

  # Set up to sweep from 1.5752 - 1.5756GHz
  # I can't get it to repeat, but I think this failed once with
  # a 50ms delay. For these important settings wait 400ms
  sendCommand(openSocket,'FREQ:START','1.5752GHZ',False,delay=0.4)
  sendCommand(openSocket,'LIST:DIR UP','',False,delay=0.4)
  sendCommand(openSocket,'FREQ:STOP','1.5756GHZ',False,delay=0.4)
  sendCommand(openSocket,'SWE:MODE','SCAN',False,delay=0.4)
  sendCommand(openSocket,'SWE:POINTS','193',False,delay=0.4)
  # Dwell time in milliseconds
  sendCommand(openSocket,'SWE:DWELL','71',False,delay=0.4)
  # Set to continuous
  sendCommand(openSocket,'INIT:CONT','1',False,delay=0.4)

  #####################################
  # Start the sweep
  sendCommand(openSocket,'INIT:IMM','',False,delay=0.4)
  # Is the sweep active?
  ret = sendCommand(openSocket,'SWE:ACTIVE?','',True)
  print('Sweep Active = ',ret)

  # Disable the power vernier
  sendCommand(openSocket,'VERNIER','0',False)
  ret = sendCommand(openSocket,'VERNIER?','',True)
  print('Vernier = ',ret)

  # Initialize to the lowest level of jamming
  Pwr = -52
  sendCommand(openSocket,'POWER',str(Pwr),False)
  # Confirm the power setting
  ret = sendCommand(openSocket,'Power?','',True)
  print('Power = ',ret)

# Reset the synthesizer - this will result in a reboot so wait 
# a while to allow the unit to recover
def resetSynth(IPAddr,port):
  tcp_client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
  tcp_client.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
  tcp_client.connect((IPAddr, port))
  sendCommand(tcp_client,'*RST','',False)
  print('Reseting Synth')
  # Wait for the synth to reboot
  time.sleep(10)


def setupJammer( IPAddr, port ):
  """ IPAddr = IP address of the RF Synth
      port = Port of the RF Synth
  Call this to set up the jammer (with the jammer off).
  Returns TCP connection for loopUpdateJammer()
  """
  # As the unit reboots we don't need to keep the TCP/IP socket
  resetSynth(IPAddr,port)

  tcp_client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
  tcp_client.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
  tcp_client.connect((IPAddr, port))

  initializeSynth(tcp_client)
  return tcp_client


def loopUpdateJammer( IPAddr, port, RXIP, f_out=None ):
  """ IPAddr = IP address of the RF Synth
      port = Port of the RF Synth
      RXIP = IP address of the receiver we'll use to get position
  Creates a jammer grid based on position from RXIP
  """
  tcp_client = setupJammer( IPAddr, port )
  while(True):
    Week, Secs, Lat, Lon, Range, Power = updateJamPower(tcp_client, RXIP)
    # We probably want to save this to a diagnostic file?
    if f_out is None:
      print(Week, Secs, Lat, Lon, Range, Power)
    else:
      print("%d %.1f %.9f %.9f %.1f %.1f" % (Week,Secs,Lat,Lon,Range,Power),file=f_out)
    # Delay a little before looping - even if the receiver is providing high
    # rate positions, the HTTP interface only appears to provide a maximum
    # of 5Hz PVT
    time.sleep(0.2)

  ########## Won't get here
  # Stop the frequency sweep
  sendCommand(tcp_client,'ABORT','',False)
  # Turn off the output
  sendCommand(tcp_client,'OUTP:STAT','OFF',False)
  tcp_client.close()

# Test execution of the code in this file
if __name__ == "__main__":
  # IP address of the RF Synth
  IPAddr = '10.1.151.98'
  # Port of the RF Synth
  port   = 10001

  # IP address of the receiver we'll use to get position
  RXIP = '10.1.151.4' #RX 7 from the RF Replay (Top CVS + Cross Aiding)

  loopUpdateJammer( IPAddr, port, RXIP )