#!/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 designed to take VMC GPS data and Benthos DAT data as inputs and 
output a position of a corresponding DAT or acoustic modem. 

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, DAT 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
from geopy.distance import VincentyDistance
import os
import sys
import trackerStalker

# In general, variables named with all UPPER_CAPS are held constant during the program;
# You may want to edit these values before running


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

# Set ENABLE_ODSS_MAIL=True to enable sending mail to ODSS
ENABLE_ODSS_MAIL = True

# Print additional messages if DEBUG is true
DEBUG = True

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

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

TARGET_CHANNELS = dict([('daphne_ac', (2, 'auv')), \
                        ('makai_ac', (3, 'auv'))])

#TARGET_CHANNELS = dict([('daphne_ac', (2, 'auv')), \
#                        ('tethys_ac', (1, 'auv'))])

TARGET_CHANNELS = dict([('moored-target',( 11, 'auv'))]) 


# Stalk (i.e. follow) target with specified acoustic channel. 
# Set to negative (e.g. -1) to disable stalking
STALK_CHANNEL = 11


# Radius of stalker watch circle (meters)
STALK_CIRCLE_RADIUS = 100

# Safe operational area, defined as KML polygon
SAFE_POLYGON_FILE = '/opt/hotspot/config/CANON-ops.kml'

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

# Acoustic address of the DAT
WG_ACOUSTIC_ADDR = 10

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

# Interval at which to send locations to ODSS via email
MAIL_INTERVAL_SEC = 30

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

# Time to pause between each loop
LOOP_PAUSE_SEC = 45

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

# 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 ' + GPS_FILE, \
                             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 ' + GPS_FILE

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

    # 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 DAT prompt'
            return False



# Parse bearing and elevation from instrumnent response
def parseBearing(bearingResp):
    print 'bearingResp: ' + bearingResp
    start = bearingResp.find('Bearing ')
    if start == -1:
        raise Exception('Cannot find \'Bearing\' in \'' + bearingResp + '\'')

    comma = bearingResp.find(',', start+8)
    bearing = float(bearingResp[start+8:comma])
    elev = float(bearingResp[comma+1:])
    
    print 'bearing: ' + str(bearing) + ', elev: ' + str(elev)
    return (bearing, elev)


# Check for valid stalk channel
if STALK_CHANNEL < 0:
    print 'Stalking DISABLED (STALK_CHANNEL=' + str(STALK_CHANNEL) + ')'
else:
    # Verify that STALK_CHANNEL refers to a defined target channel
    found = False
    targets = TARGET_CHANNELS.keys()
    for target in targets:
        channel, targetType = TARGET_CHANNELS[target]
        if channel == STALK_CHANNEL:
            print 'STALK_CHANNEL=' + str(STALK_CHANNEL)
            found = True
            break

    if found == False:
        print 'ERROR: STALK_CHANNEL (' + str(STALK_CHANNEL) + ') not found in TARGET_CHANNELS'
        sys.exit(1)

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

dprint('get DAT\'s 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 'DAT on serial port ' + serialPortName + ', power port ' + powerPort
print '(from ' + modemsConfigFile + ')' 
# Power cycle the DAT to put device in known state
print 'Power cycle the DAT:'
delay = 12
print 'Turn off the DAT, wait ' + str(delay) + ' seconds for power-down'
subprocess.call(['SetPowerSwitch', powerPort, '0'])
time.sleep(delay)
print 'Turn on DAT:'
subprocess.call(['SetPowerSwitch', powerPort, '1'])

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

print 'Wait for DAT CONNECT message'
tries = 0
while True:
    resp = serialPort.readline()
    resp = resp.rstrip('\n')
    dprint('DAT msg len: ' + str(len(resp)) + ', msg: ' + resp)
    print 'from DAT: ' + resp
    if resp.find('CONNECT ') != -1:
        dprint('Got connect msg: ' + resp)
        break

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

    tries += 1

serialPort.flush()

dprint('Get DAT user prompt')
serialPort.write('+++')
consumePrompt(serialPort)

lastEmailTime = 0
lastClockCheckTime = 0

# Alarm if we lose acoustic track
trackAlarmArmed = True
nMissedAcousticResp = 0

# Consecutive acoustic misses that trigger an alarm
MISSED_RESP_THRESHOLD = 15


# Alarm if target coords are outside of safe polygon
stalkAlarmArmed = True

while True:

    emailLocation = False

    if time.time() - lastEmailTime > MAIL_INTERVAL_SEC and ENABLE_ODSS_MAIL:
        dprint('email locations to ODSS on this cycle')
        emailLocation = True
        lastEmailTime = time.time()
    elif not ENABLE_ODSS_MAIL:
        print 'ODSS mail not enabled (ENABLE_ODSS_MAIL = False)'
    else:
        dprint('do not email locations to ODSS on this cycle')
        emailLocation = False

    time.sleep(3)
    serialPort.flushInput()

    targets = TARGET_CHANNELS.keys()

    for target in targets:

        tuple = TARGET_CHANNELS[target]
        channel, targetType = tuple

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

        time.sleep(2)
        serialPort.flushInput()


        if SIMULATED:
            print 'USING SIMULATED COMMAND'
            serialPort.write('dat\r\n')
        else:
            # Issue range command
            serialPort.write('atr' + str(channel) + '\r\n')
###         serialPort.write('usbl ' + str(channel) + ' 1\r\n')

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

        bearingResp = serialPort.readline().strip()
        dprint('bearing response: ' + bearingResp)

        # Debug - Print hex representation of string
        # print ':'.join('{:02x}'.format(ord(c)) for c in bearingResp)
        if bearingResp == RESPONSE_NOT_RECEIVED:
            print 'WARNING: response not received from ' + target

            if channel == STALK_CHANNEL:
                nMissedAcousticResp = nMissedAcousticResp + 1

                if nMissedAcousticResp >= MISSED_RESP_THRESHOLD:
                    # Send alarm
                    if trackAlarmArmed:
                        trackerStalker.sendAlarm('Missed ' + str(MISSED_RESP_THRESHOLD) + \
                                                     ' acoustic responses from channel ' + \
                                                     str(channel), \
                                                     trackerStalker.LOST_ACOUSTIC_TRACK)
                        # Disarm alarm
                        trackAlarmArmed = False

            continue    # Try next target

        # Reset missed response counter, re-arm alarm
        if channel == STALK_CHANNEL:
            nMissedAcousticResp = 0
            trackAlarmArmed = True

        timestamp = time.time()

        try:
            bearElev = parseBearing(bearingResp)
            bearing = bearElev[0]
            elevDeg = bearElev[1]
            elevRad = math.radians(elevDeg)
        except Exception, e:
            print 'ERROR: ' + str(e)
            continue


        if SIMULATED:
            # TEST TEST TEST
            rangeResp = 'Range 10 to ' + str(channel) + ': 1023.9 m' 
            dprint('simulated range response: ' + rangeResp)
        else:
            # Range string is on a line of its own
            rangeResp = serialPort.readline()
            dprint('rangeResp = ' + rangeResp)
            # Range resp starts with "Bearing", try it again
            if rangeResp.find('Bearing ') != -1:
                print 'Got 2nd bearing response - use this one: ' + rangeResp;
		bearingResp = rangeResp;
                try:
                    bearElev = parseBearing(bearingResp)
                    bearing = bearElev[0]
                    elevDeg = bearElev[1]
                    elevRad = math.radians(elevDeg)
                except Exception, e:
                    print 'ERROR: ' + e
                    continue

                print 'Now get range'
                rangeResp = serialPort.readline()
                dprint('2nd rangeResp = ' + rangeResp)

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

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

        depth = slantRange * math.sin(elevRad)
        surfaceRange = slantRange * math.cos(elevRad)

        dprint('bearing: ' + str(bearing) + ', range: ' + str(slantRange))
        dprint('surfaceRange: ' + str(surfaceRange) + ', depth: ' + str(depth))

        if SIMULATED:
            serialPort.readline()          # Consume 'Ok' 

        consumePrompt(serialPort)

        # Get our current location and GPS time
        if getGPS() == -1:
            print 'ERROR: missing GPGGA and/or GPZDA data in ' + GPS_FILE
            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        
        except ValueError, e:
            print 'ERROR: Invalid lat and/or lon: ' + str(_gpgga)
            continue

        dprint('lat0=' + str(lat0) + ', lon0=' + str(lon0))

        # Use Vincenty's algorithm to compute lat/lon of target based on our 
        # lat/lon, range and bearing to target.
        origin = geopy.Point(lat0, lon0)
        print 'NOTE - using slantRange instead of surfaceRange'
        targetPos = \
            VincentyDistance(meters=slantRange).destination(origin, bearing)

        dprint('target: lat ' + str(targetPos.latitude) + \
            ', lon ' + str(targetPos.longitude))

        if channel == STALK_CHANNEL:
            dprint('Stalk target at ' + str(targetPos.latitude) + ', ' + \
                       str(targetPos.longitude))

            ret = trackerStalker.stalk(lat0, lon0, \
                                           targetPos.latitude, \
                                           targetPos.longitude, \
                                           STALK_CIRCLE_RADIUS, \
                                           SAFE_POLYGON_FILE, \
                                           stalkAlarmArmed)
            if ret != 0:
                # trackerStalker.stalk() got error - now in alarm state
                stalkAlarmArmed = False  
            else:
                # trackerStalker.stalk() ran ok - send alarm on next error
                stalkAlarmArmed = True   
                

        tstr = time.gmtime(timestamp)
        print 'ODSS format: 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)) + ',' +  \
            str(round(bearing, 2)) + ',' +  \
            str(round(elevDeg, 2)) + ',' +  \
            str(round(targetPos.longitude, 5)) + ',' +  \
            str(round(targetPos.latitude, 5)) \
            + ',' + time.strftime('%Y-%m-%dT%H:%M:%S',tstr)

        if emailLocation and ENABLE_ODSS_MAIL:

            subject = target + ',' + str(int(timestamp)) + ',' + \
                str(round(targetPos.longitude, 5)) + ',' +  \
                str(round(targetPos.latitude, 5)) 
                        
            recips = targetType + 'track@mbari.org'
            print 'Sending ' + target + ' position to ODSS'
            cmd = 'python pymail.py hotspot-noreply@myzw.com ' + \
                subject + ' ' + \
                subject + ',' +  time.strftime('%Y-%m-%dT%H:%M:%S',tstr) + \
                ' ' + \
                recips;

            # Send mail in background process ('&')
            os.system(cmd + "&")

        # 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 DAT serial port
serialPort.close()

dprint('Serial port closed')