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

Revision History:
2016-01-29 - Initial creation B. Kieft
Based on ranging.py

NOTE: THIS SCRIPT MUST BE EXECUTED FROM SMC-SCRIPT DIRECTORY.

This script is designed to "find" assets using range only pings to 
acoustic modems. This provides a yes/no answer for whether or not an 
asset is in the general area (e.g. running a canyon to search for
a missing asset).  

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 this script expects the modem to be configured prior
to use. At some point this should be automated. 
Sample Sregisters that work are shown below. Specifically
S13 = 1
S10 = 36
S7 = as short as possible
S15 = 1


Local Sregisters
S00=810  S01=000  S02=000  S03=002  S04=004  S05=000  S06=008  
S07=012  S08=060  S09=000  S10=036  S11=000  S12=000  S13=001  
S14=005  S15=001  S16=001  S17=001  S18=000  S19=000  S20=000  


"""
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
from datetime import datetime


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

# Print additional messages if _debug is true
_debug = True
_assumeLowpower = False

_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 = 2

# 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([('BED1', (0, 'BED')), \
                            ('BED2', (2, 'AMT'))]) \
#                            ('BED6', (6, 'BED'))]) 
#                            ('BED5', (5, 'BED'))])

# 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:

    minute = datetime.now().minute
    if minute <= 59 and minute >= 7:        
      time.sleep(15)
      _assumeLowpower = True;
      continue
    else:
      if _assumeLowpower:
        print 'Get Benthos user prompt'
        serialPort.write('+++')
        consumePrompt(serialPort)
        _assumeLowpower = False
    	
    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')