#!/usr/bin/env python
"""

Revision History:
2014-07-08 - Initial creation B. Kieft

NOTE: THIS SCRIPT MUST BE EXECUTED FROM SMC-SCRIPT DIRECTORY.
THIS SCRIPT IS ONLY APPROPRIATE TO A BENTHOS MODEM, *NOT* A DAT.

This script is designed to get the range to a specified target.

To run on an SMC, is is recommened that pip is installed:
https://pip.pypa.io/en/latest/installing.html
to enable the installation of the following dependent packages:
pynmea

Note that VMC GPS sensor output is expected in the following form:
...
GPS,41827.86832176,36.8023927,-121.7876722
NMEA,GPVTG,,T,,M,0.034,N,0.063,K,D*24
NMEA,GPGGA,205024.00,3648.14354,N,12147.26031,W,2,08,1.27,8.7,M,-30.7,M,,0000*6F
NMEA,GPGLL,3648.14354,N,12147.26031,W,205024.00,A,D*76
NMEA,GPGST,205024.00,15,,,,1.8,2.3,3.3*5A
NMEA,GPZDA,205024.00,07,07,2014,00,00*60
NMEA,GPRMC,205025.00,A,3648.14350,N,12147.26027,W,0.128,,070714,,,D*63
GPS,41827.86833333,36.8023923,-121.7876718
... 

Additionally, Benthos output is expected to be in the following form, configured 
as shown:
16:20:33.8419 , LVL= 21216, 14545, 20466, 24051, AGC=  72, IDX=258, 0.00, 2.820, PHS= 2.918, 0.027,-3.054, |v|= 2.987, RAW=  59.0,   0.4, CAL=  33.1,   8.3, ROT= 305.8,   8.3
Range 1 to 0 : 4.2 m

@DatVerbose parameter should be set to 27505 (0x6B71 per spec).

response to "ATRCH" or "dat" command should look something like this:

Output lines prefaced with 'RANGE:' are in the following format

   target,channel,epochSec,range,bearing,lon,lat,timeString

"""
from pynmea import nmea
import time
import subprocess
import serial
import io
import calendar
import math
import geopy
import utm
from geopy.distance import VincentyDistance
import os
import sys
import argparse
import sys


# Set _simulated=False when we have actual targets to range to
_simulated = False

# Print additional messages if _debug is true
_debug = True

_hasUsbl = True

_logName = ''
_nLogEntries = 0

_prevLat = 0.
_prevLon = 0.


# GPS data file 
_gpsFile = '/mnt/XDATA/GPS/gpsdata.txt'

# String output by Benthos
RESPONSE_NOT_RECEIVED = 'Response Not Received'

RANGE_RESP_PREFIX = "Range"
RANGE_RESP_SUFFIX = "m"

# Warn if GPS time differs from system clock time by this amount
ALLOWED_CLOCK_ERR_SEC = 10

# Interval at which to check sys clock against GPS time
CLOCK_CHECK_INTERVAL_SEC = 300

# Time to pause between each loop
LOOP_PAUSE_SEC = 10

# Create some pynmea objects
_gpgga = nmea.GPGGA()
_gpzda = nmea.GPZDA()


def main(argv):

    parser = argparse.ArgumentParser(description='Log range to target')

    parser.add_argument('-a', '--append', help='append to log file', 
                        action='store_true')

    parser.add_argument('logName', 
                        help='logfile for easting, northing, and range')

    args = parser.parse_args()

    global _logName        # Defined above
    _logName = args.logName

    if args.append:
        print 'append to log file ' + _logName
    else:
        print 'create new log file ' + _logName

    if os.path.isdir(_logName):
        print 'ERROR: ' + _logName + ' is a directory'
        sys.exit(1)

    if  not args.append or not os.path.isfile(_logName):
        # File doesn't exist yet - write header
        print 'Write header to ' + _logName
        logfile = open(_logName, 'w')
        logfile.write('# epochSec  target easting  northing  range  utmZone\n')
        logfile.close()


# Print message if in debug mode
def dprint(str):
    if _debug:
        print str

# Using a decimal degree converter function from Jordan Stanway
def degrees_minutes_to_decimal_degrees(dm):
    """Convert degrees, minutes, and hemisphere to decimal degrees.
    """
    dm = dm.strip()
    dprint('dm =' + dm)
    boundary = dm.find('.') - 2
    return float(dm[:boundary]) + float(dm[boundary:]) / 60.


# Get our current latitude/longitude from GPS file, latest GPS time
# Grab the latest GGA string from the file and clean it up
# pynmea expects a GGA string in the following (and typical) format:
# '$GPGGA,205024.00,3648.14354,N,12147.26031,W,2,08,1.27,8.7,M,-30.7,M,,0000*6F'
# pynmea expects a ZDA string in the following (and typical) format:
# '$GPZDA,205056.00,07,07,2014,00,00*65'
# Return 0 if GPGGA and GPZDA sentences found, else return -1
def getGPS():
    p = subprocess.Popen('tail -30 ' + _gpsFile, \
                             shell=True, stdout=subprocess.PIPE, \
                             stderr=subprocess.STDOUT)

    gotGPGGA = False
    gotGPZDA = False

    for line in p.stdout.readlines():
        # dprint('getGPS(): line = ' + line)
        if line.find('GPGGA') > -1:    # Get the latest GGA string
            # dprint('found GPGGA: ' + line)
            line = '$' + line.lstrip('NMEA,') # Get rid of the SMC specific formatting
            # Parse the GPGGA data
            # dprint('parse line: ' + line)
            _gpgga.parse(line)
            # dprint('parsed')
            gotGPGGA = True

        if line.find('GPZDA') > -1:    # Get the latest GGA string
            line = '$' + line.lstrip('NMEA,') # Get rid of the SMC specific formatting
            # Parse the GPZDA data
            _gpzda.parse(line)
            gotGPZDA = True

    if not gotGPGGA:
        print 'No GPGGA sentences found in ' + _gpsFile

    if not gotGPZDA:
        print 'No GPZDA sentences found in ' + _gpsFile

    # Need to close subprocess stdout?
    p.stdout.close()

    if gotGPGGA and gotGPZDA:
        return 0
    else:
        return -1

# Read serial port until user prompt is found. Assume that '>' means user prompt for now
def consumePrompt(serialPort):
    n = 0 
    limit = 100
    while True:
        # Read a single character. If character is '>', assume we've got a prompt
        char = serialPort.read(size=1)
        if char == '>':
            dprint('found prompt')
            return True

        n += 1
        if n >= limit:
            print 'ERROR: Could not find Benthos prompt'
            return False



if __name__ == "__main__":
   main(sys.argv[1:])

# Define targets, their acoustic channels, and type (either "auv" or "drifter")
# (maybe eventually put these in a separate configuration file)
_targetChannels = dict([('cpf1', (11, 'drifter')), \
                            ('daphne_ac', (2, 'auv')), \
                            ('makai_ac', (3, 'auv'))])

_targetChannels = dict([('ROVER', (3, 'rover'))])

# Start GPS stream
print 'Set GPS output file'
os.system('ServerExec GPSListener \'GPS DEFAULTOUTPUT ' + _gpsFile + '\'')
print '\n'
os.system('ServerExec GPSListener \'GPS OUTPUT ' + _gpsFile + '\'')
print '\nStart GPS stream'
os.system('ServerExec GPSListener \'GPS START\'')
print '\n'

dprint('get Benthos\' serial and power port')
modemsConfigFile = '/opt/hotspot/config/modems.cfg'
powerSerial = subprocess.check_output(['python', 'modemConfig.py', modemsConfigFile, 'dat'])

dprint('powerSerial=' + powerSerial)
arr=powerSerial.split(' ')
powerPort = str(arr[0]).strip()
serialPortName = arr[1].strip()
print 'Benthos on serial port ' + serialPortName + ', power port ' + powerPort
print '(from ' + modemsConfigFile + ')' 
# Power cycle the Benthos to put device in known state
print 'Power cycle the Benthos:'
delay = 12
print 'Turn off the Benthos, wait ' + str(delay) + ' seconds for power-down'
subprocess.call(['SetPowerSwitch', powerPort, '0'])
time.sleep(delay)
print 'Turn on Benthos:'
subprocess.call(['SetPowerSwitch', powerPort, '1'])

# Open Benthos serial port
serialPort = serial.Serial(port=serialPortName, baudrate=9600, timeout=20)

print 'Wait for CONNECT message'
tries = 0
while True:
    try:
        resp = serialPort.readline()

        resp = resp.rstrip('\n')
        if resp.find('CONNECT ') != -1:
            dprint('Got connect msg: ' + resp)
            break

        if tries > 10:
            print 'ERROR: No connect message from Benthos'
            sys.exit(1)

    except OSError as e:
        print 'Caught OSError ' + str(e.errno) + \
            ' while reading acoustic port; trying again...'
        time.sleep(0.5)

    tries += 1

time.sleep(3)
serialPort.flush()

print 'Get Benthos user prompt'
serialPort.write('+++')
consumePrompt(serialPort)

lastClockCheckTime = 0

while True:

    time.sleep(3)
    serialPort.flushInput()

    targets = _targetChannels.keys()
    for target in targets:

        tuple = _targetChannels[target]
        channel, targetType = tuple

        print '\nGet range to ' + target + ' on channel ' + \
            str(channel) + ':'

        time.sleep(2)
        serialPort.flushInput()

        # Issue range command
        serialPort.write('atr' + str(channel) + '\r\n')

        resp = serialPort.readline()   # Eat echo'ed newline

        # Read command response

        """
        Try to get range response. Possible responses other than range include:
        1. Target may not respond at all
        2. May get Bearing response, if host modem has USBL
        3. May get two Bearing responses if target modem also has USBL
        """
        gotRange = False
        for n in range(1, 4):
            resp = serialPort.readline().rstrip()  
            dprint('resp: ' + resp)
            # Debug - Print hex representation of string
            print ':'.join('{:02x}'.format(ord(c)) for c in resp)
            if resp == RESPONSE_NOT_RECEIVED:
                print 'WARNING: response not received from ' + target
                break

            if resp.find(RANGE_RESP_PREFIX) != -1:
                dprint('Got range response')
                gotRange = True
                timestamp = time.time()
                break
        
        if gotRange == False:
            print 'WARNING: did not get range response from ' + target
            continue    # Try next target            
            
        dprint('resp = ' + resp)


        # Parse target's range from 'Range <ch0> to <ch1>: <distance> m'
        start = resp.find(':')
        end = resp.find(RANGE_RESP_SUFFIX, start)
        if start == -1 or end == -1:
            print 'ERROR: couldn\'t parse range from \"' + resp + '\"'
            continue

        slantRange = float(resp[start+1:end])

        dprint('slantRange: ' + str(slantRange))

        consumePrompt(serialPort)

        # Get our current location and GPS time
        if getGPS() == -1:
            print 'ERROR: missing GPGGA and/or GPZDA data in ' + _gpsFile
            continue
            
        try:
            lat0 = degrees_minutes_to_decimal_degrees(_gpgga.latitude)
            north_latitude = (_gpgga.lat_direction == 'N')*2 - 1
            lat0 *= north_latitude
            lon0 = degrees_minutes_to_decimal_degrees(_gpgga.longitude)
            east_longitude = (_gpgga.lon_direction == 'E')*2 - 1
            lon0 *= east_longitude        
            dprint('lat0=' + str(lat0) + ', lon0=' + str(lon0))

            if lat0 == _prevLat and lon0 == _prevLon:
                print 'WARNING: Location not changed; GPS not updating?'

            _prevLat = lat0
            _prevLon = lon0

            # Convert lat/lon to UTM
            tuple = utm.from_latlon(lat0, lon0)
            easting, northing, zone, zoneLetter = tuple
            

        except ValueError, e:
            print 'ERROR: Invalid lat and/or lon: ' + str(_gpgga)
            continue


        tstr = time.gmtime(timestamp)
        print 'TEST: ' + str(easting) + ' ' + str(northing) + ' ' + \
            str(slantRange)

        print '_logName: ' + _logName
        
        try:
            print 'log entry #' + str(_nLogEntries)

            logfile = open(_logName, 'a')

            logfile.write(str(int(timestamp)) + ',' + target + ',' + 
                          str(easting) + ',' + str(northing) + ',' + 
                          str(slantRange) + ',' + str(zone) + "\n")

            _nLogEntries += 1
        except KeyboardInterrupt:
            print('Got keyboard interrupt during log write - keep going')
        finally:
            logfile.close()

        # targ,chan,time,lon0,lat0,range,bearing,elev,lonTarg,latTarg,timestring
        print 'RESULT: ' + target + ',' + str(channel) + ',' + \
            str(int(timestamp)) + ',' + \
            str(round(lon0, 5)) + ',' + \
            str(round(lat0, 5)) + ',' + \
            str(round(slantRange, 2))

        # Check system clock against GPS time, at specified interval
        if timestamp - lastClockCheckTime > CLOCK_CHECK_INTERVAL_SEC:
            lastClockCheckTime = timestamp
            tmp = str(_gpzda.year) + '-' + \
                str(_gpzda.month) + '-' + \
                str(_gpzda.day) + 'T' + \
                str(_gpzda.timestamp)

            # Parse the GPS timestamp
            gpsTime = time.strptime(tmp, '%Y-%m-%dT%H%M%S.%f')
            print 'GPS time: ' + time.strftime('%Y-%m-%dT%H:%M:%S', gpsTime)
            gpsTimestamp = calendar.timegm(gpsTime)
            dprint('GPS timestamp: ' + tmp + ' struct: ' + str(gpsTime))
            dprint('gpsTimestamp=' + str(gpsTimestamp))

            if abs(timestamp - gpsTimestamp) > ALLOWED_CLOCK_ERR_SEC:
                print 'WARNING: GPS time minus system clock time = ' + \
                    str(gpsTimestamp - timestamp) + ' sec'
            else:
                print 'sys clock offset from GPS: '
                str(timestamp - gpsTimestamp) + ' sec'


    # Sleep before next loop
    print 'Wait ' + str(LOOP_PAUSE_SEC) + ' sec before next loop'
    time.sleep(LOOP_PAUSE_SEC)


# Close the Benthos serial port
serialPort.close()

dprint('Serial port closed')