import numpy as np
import scipy.signal as sig
import arlpy.signal as asig
from arlpy.plot import *
from unetpy import *

"""
Explanation: AUV-to-transponder range estimation application using UnetStack
It is a common practice to attach a transponder to various underwater assets (both static and mobile) for short and long term field deployments. These transponders can act as a beacon that can be utilized for localization. An underwater vehicle (e.g. AUV, ROV) will be able to do sequential ranging to find the location of the transponder during a search and rescue operation.
Normally, specialized transmitters are required to send a signal to these transponders to trigger a response. However, due to the software defined nature of Subnero modems, it is fairly easy to develop an application that will query a transponder and calculate the range information.
In this post, we showcase a fully functional transponder ranging application used to ping an Applied Acoustics 219A transponder and calculate the range to it from the recorded signal. This work has been presented in M. Chitre, R. Bhatnagar, M. Ignatius, and S. Suman, “Baseband signal processing with UnetStack,” in Underwater Communications Networking (UComms 2014), (Sestri Levante, Italy), September 2014. (Invited).
An Applied Acoustics 219A transponder is set to receive a 22 kHz tonal. We send a 5 ms long 22 kHz pulse without preamble (preambleID set to 0) using a TxBasebandSignalReq from UnetStack's Baseband service. This will trigger a response at 30 kHz after a fixed delay of 30 ms. We also initiate a 500 ms long passband signal recording using the passband streaming API (phy.pbscnt) to make sure the modem records long enough to have the response from the pinger.
The captured signal is then Hilbert transformed to do envelop detection. We use thresholding to find the start of the received signal. We also retrieves the txTime from txntf. Using these two timings, we calculate the range to the transponder.
This application is developed using Jupyter Notebook, in Python using the UnetPy framework.
End of explanation
"""


modem = UnetGateway('192.168.1.74')

phy = modem.agent('phy')
modem.subscribe(phy)

"""
Explanation: Connect to the modem
Using the unetpy gateway, connect to the modem's IP address and subscribe to physical agent.
End of explanation
"""


txFs = 192000              # Sa/s
sigLen = 5e-3              # s
timeout = 5000             # ms
soundSpeed = 1520          # m/s
triggerSig = 22000         # Hz
transponderDelay = 30e-3   # s

phy.pbsblk = 45056         # Sa
phy.signalPowerLevel = -6  # dB

"""
Explanation: Set passband block count, signal power level and other constants
Before starting passband recording, we need to set the passband block size using phy.pbsblk parameter. (Passband block size is the number of samples in a block of recorded passband data)
End of explanation
"""


def flush_modem():
    while modem.receive(timeout=1000):
        pass

"""
Explanation: Flush the modem to clear its internal buffers
End of explanation
"""


tx = asig.cw(triggerSig, sigLen, txFs).tolist()

"""
Explanation: Generate the signal to be transmitted
We use the cw function from arlpy signal processing toolbox to generate a 5 ms long 22 kHz tonal.
End of explanation
"""


flush_modem()
phy.pbscnt = 1
phy << org_arl_unet_bb.TxBasebandSignalReq(signal=tx, fc=0)
rx = modem.receive(org_arl_unet_bb.RxBasebandSignalNtf, timeout)
rxS = rx.signal
rxFs = rx.fs
txntf = modem.receive(timeout=timeout)
txTime = txntf.txTime
rxTime = rx.rxTime

"""
Explanation: Start baseband signal reception and transmit the signal
We start a passband recording by setting phy.pbscnt to 1. This will start the recording of 1 block of passband data. The next step is to transmit the 22 kHz tonal. We also note down the transmission time (start of the transmission) and reception time (start of the receiving block of pb data).
End of explanation
"""


plot(rxS, fs=rxFs, maxpts=rxFs, hold=True)
txStart=(txTime-rxTime)/1e6
vlines(txStart, color='orange')

psd(rxS, fs=rxFs)

"""
Explanation: Plot the recorded signal & PSD
Using arlpy's plotting utilities, we plot the time series and marked txTime. Note that we can see the transmitted data (saturated) in the recoded passband signal time series. In the PSD, we can see both transmission (22 kHz) and received tonal (30 kHz) clearly.
End of explanation
"""


hb = sig.firwin(127, [30000, 31000], pass_zero=False, fs=rxFs)
rxS1 = asig.lfilter0(hb, 1, rxS)
plot(rxS1, fs=rxFs, maxpts=rxFs)

"""
Explanation: In order to isolate the response signal, we pass it through a bandpass filter.
End of explanation
"""


start = txStart+transponderDelay
newRx = rxS1[int(start*rxFs):]
env = asig.envelope(newRx)
edge = np.argwhere(env> np.max(env)/2)[0,0]

plot(env, fs=rxFs, maxpts = rxFs, hold=True)
vlines(edge/rxFs, color='orange')

plot(rxS, fs=rxFs, maxpts = rxFs, hold=True)
rxStart=(start+edge/rxFs)
vlines(txStart, hold=True, color='orange')
vlines(rxStart, color='purple')

"""
Explanation: Identifying receive time
The next step is to identify the first sample from the received signal. For this, we shift the start of the received signal by txStart + transponderDelay, generate a Hilbert envelop of the remaining signal and do a threshold (which is half of sample with maximum value) detection. Both txStart and rxStart is marked in the plot.
The Hilbert envelop is generated using the envelop function from arlpy signal processing toolbox.
End of explanation
"""


print ('Tx Start = ', txStart, 's')
print ('Rx Start = ', rxStart, 's')
travelTime = rxStart-txStart-transponderDelay
print ('Round trip time = ', travelTime, 's')
print ("Range = {0:.2f} m".format(soundSpeed * travelTime/2))

"""
Explanation: Calculating the range
Now that we have the txTime and rxTime, we can calculate the range.
End of explanation
"""


modem.shutdown()

"""
Explanation: Once done, shutdown the modem
End of explanation
"""


