#!/usr/bin/env python
"""
This script issues navigation commands to the Wave Glider VMC, commanding to vehicle to execute 
a watch circle of specified radius about a specified latitude/longitude.
The script checks the destination coordinates for safety relative to a 
user-defined 'safe polygon'. If coordinates do not pass the safety check, no 
navigation command is issued.

NOTE: CLASSPATH must be defined to include PolyTest java application and associated library jcoord.jar
"""
import sys
import os
import math

SENDER = 'hotspot-noreply@myzw.com'

EMAIL_RECIP = 'waveglider-nav@mbari.org'

REPORTER_ID = '2001'
UNSAFE_COORDS = 1
SYSTEM_ERR = 2
LOST_ACOUSTIC_TRACK = 3
MIN_RADIUS = 25

_sendEmail = False

def main(argv):

    if len(sys.argv) != 8:
        print 'usage: currentLat currentLon targetLat targetLon watch-radius polygonKML alarmOnError'
        sys.exit(1)

    currentLat = sys.argv[1]
    currentLon = sys.argv[2]
    targetLat = sys.argv[3]
    targetLon = sys.argv[4]
    radius = sys.argv[5]
    if radius < 25:
        print str(targetLat) + ', ' + str(targetLon) + ': Unsafe coordinates'
        radius = 25.
    
    polygonFile = sys.argv[6]
    alarmOnError = sys.argv[7]
    stalk(currentLat, currentLon, targetLat, targetLon, radius, \
              polygonFile, alarmOnError)


# Send an alarm to WGMS
def sendAlarm(text, notificationID):
    cmd = \
        'ServerExec PayloadInterface \'SendAlarmSet sender=0x2000 text=\"' + \
        text + '\" reporter=' + str(REPORTER_ID) + ' notification=' + \
        str(notificationID) + '\''

    print 'sendAlarm cmd: ' + cmd

    ret = os.system(cmd)
    print '\n'
    
    mailCmd = 'python pymail.py hotspot-noreply@myzw.com \"Stalker error\" \"' + text + '\" ' + EMAIL_RECIP
    # Send mail in background process
    os.system(mailCmd + "&")

    return ret


def stalk(currentLat, currentLon, targetLat, targetLon, radius, polygonFile, \
              alarmOnError):
    # Check coordinate safety
    cmd = 'java PolyTest ' + str(currentLat) + ' ' + str(currentLon) + \
        ' ' + str(targetLat) + ' ' + str(targetLon) + ' ' + polygonFile

    ret = os.system(cmd)
    print 'Safety check result: ' + str(ret)
    if ret != 0:
        if alarmOnError:
            print str(targetLat) + ', ' + str(targetLon) + ': Unsafe coordinates'
            sendAlarm('Unsafe target coords: ' + str(targetLat) + ', ' + \
                          str(targetLon), UNSAFE_COORDS)

        return ret

    print 'Do ' + str(radius) + 'm watch-circle at ' + str(targetLat) + ', ' + str(targetLon)

    navCmd = '%ffn.gotoWatch(' + str(targetLat) + ',' + \
        str(targetLon) + ',' + str(radius) + ')'

    cmd = 'ServerExec PayloadInterface \'WCS 0 ' + navCmd + \
        ';ffn.clearHistory()\''

    print 'cmd: ' + cmd

    ret = os.system(cmd)
    print '\n'

    if ret != 0:
        print 'Navigation command error result: ' + str(ret)
        # Always send alarm in case of system error; serious problem
        sendAlarm('Error sending navigation command to CCU', SYSTEM_ERR)

    else:
        cmd2 = 'ServerExec PayloadInterface \'SendEvent sender=0x2000 text=\"issued ' + navCmd + '\"\''
        os.system(cmd2)
        print '\n'

        if _sendEmail == True:
            mailCmd = 'python pymail.py hotspot-noreply@myzw.com \"Navigation command\" \"Issued/Acked from HotSpot SMC:\n' + cmd + '\" ' + EMAIL_RECIP
            print 'mailCmd: ' + mailCmd
            # Send mail in background process
            os.system(mailCmd + "&")

    return ret



def stalkerIntro() :
    # TEST
    print 'This is stalker'


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