root/points/trunk/points.py

#!/usr/bin/env python

"""
Generate glider goto list.

Warning: This script has only been tested for operations in
         the Northwestern hemispheric quadrant. It will most
         certainly fail for operations crossing quadrants.

Usage:

    python points.py glider_name \\
                     glider_lat glider_lon \\
                     bearing \\
                     heading [distance [steps [no_copy]]]

    where glider_name is the whoru response from the glider,

    where glider_lat and glider_lon are the present glider
        latitude and longitude in ISO format (+/-ddmm.mmmm),

    where bearing is the literal token bearing, 

    where heading is the direction of movement in degrees
        clockwise from north,

    where the optional distance is the spherical distance of       
        movement in kilometers per step, defaulting to 2km,

    where the optional steps are the number of waypoints to
        generate with distance between each step, defaulting
        to 20,

    and where if the optional no_copy is present, suppress
        writing a goto list to the to-glider folder
        (archive only);

    or

    python points.py glider_name \\
                     glider_lat glider_lon \\
                     target \\
                     target_lat target_lon \\
                     [steps [no_copy]]

    where glider_name is the whoru response from the glider,

    where glider_lat and glider_lon are the present glider
        latitude and longitude in ISO format (+/-ddmm.mmmm),

    where target is the literal token target, 

    where target_lat and target_lon are the desired glider
        latitude and longitude in ISO format (+/-ddmm.mmmm),

    where the optional steps are the number of waypoints to
        generate between present and desired glider positions,
        defaulting to 20.

    and where if the optional no_copy is present, suppress
        writing a goto list to the to-glider folder
        (archive only).
"""

import sys
import os
import math
import shutil
import time
from datetime import datetime

months = {1: 'Jan',
          2: 'Feb',
          3: 'Mar',
          4: 'Apr',
          5: 'May',
          6: 'Jun',
          7: 'Jul',
          8: 'Aug',
          9: 'Sep',
          10: 'Oct',
          11: 'Nov',
          12: 'Dec'}

LB1MOOR = [3310.353, -7819.800]
LB1ADCP = [3310.172, -7820.007]
LB2MOOR = [3256.491, -7805.667]
LB2ADCP = [3256.419, -7805.926]
LB3MOOR = [3251.142, -7800.793]
LB3ADCP = [3251.142, -7800.793]

glider_dir = '/var/opt/gmc/gliders'

goto_names = {"ramses": "goto_l10.ma",
              "pelagia": "goto_l20.ma",}

first_template = \
"""#
# Filename: %s
# File creation time: %s
# Generated by: %s
#

behavior_name=goto_list

<start:b_arg>
        b_arg: num_legs_to_run(nodim) -2 # -1 loop, -2 run once, > 0 = this m$
        b_arg: start_when(enum) 0       # 0 baw_immediately
        b_arg: list_stop_when(enum) 7   # BAW_WHEN_WPT_DIST

        # SATISFYING RADIUS
        b_arg: list_when_wpt_dist(m) 100

        # LIST PARAMETERS
        b_arg: initial_wpt(enum) 0      # 0 to n-1, -1 first after last, -2 c$
        b_arg: num_waypoints(nodim) %u
<end:b_arg>

<start:waypoints>
"""

last_template = \
"""<end:waypoints>

"""

nmperrad = 360.0 * 60.0 / (2 * math.pi)  # 1 nautical mile per minute lat
kmperrad = nmperrad * 1.852              # mean at 48 degrees
kmperdeg = kmperrad * math.pi / 180.0

def ddmm2decdeg(ddmm):
    sign = ddmm < 0
    ddmm = abs(ddmm)
    dd = (ddmm // 100)
    mm = ddmm - (dd * 100)
    dm = mm / 60
    if sign:
        decdeg = -(dd + dm)
    else:
        decdeg = dd + dm
    return decdeg

def decdeg2ddmm(decdeg):
    dd = int(decdeg)
    dm = decdeg - dd
    mm = dm * 60
    ddmm = (dd * 100) + mm
    return ddmm

def latlondict(latlonlist):
    return {'lat': latlonlist[0],
            'lon': latlonlist[1]}

def latlonlist(latlondict):
    return [latlondict['lat'], latlondict['lon']]

def line(origin, target, steps):
    vector = {'lat': target['lat'] - origin['lat'],
              'lon': target['lon'] - origin['lon']}
    divs = {'lat': vector['lat'] / steps,
            'lon': vector['lon'] / steps}
    points = [{'lat': origin['lat'] + (x * divs['lat']),
               'lon': origin['lon'] + (x * divs['lon'])}
              for x in range(1, steps + 1)]
    return points

def swap(seq):
    return [seq[1], seq[0]]

def _target(glider_name, glider_pos, target, steps, copy=False):
    goto_name = goto_names[glider_name]
    glider_pos = map(ddmm2decdeg, glider_pos)
    target = map(ddmm2decdeg, target)

    glider_pos = latlondict(glider_pos)
    target = latlondict(target)

    points = line(glider_pos, target, steps)
    points = [latlonlist(point) for point in points]
    points = [map(decdeg2ddmm, point) for point in points]

    points = [swap(point) for point in points]

    now = datetime.now()
    now_string = "%02d-%s-%04d %02d:%02d:%02d UTC" % (now.day,
                                                      months[now.month],
                                                      now.year,
                                                      now.hour,
                                                      now.minute,
                                                      now.second,
                                                     )
    now_stamp = "%04d%02d%02dT%02d%02d%02d" % (now.year,
                                               now.month,
                                               now.day,
                                               now.hour,
                                               now.minute,
                                               now.second)

    backup_dir = os.path.join(os.path.dirname(
                                  os.path.abspath(__file__)),
                              "archive",
                              glider_name)
    backup_name = "%s_%s_%s" % (glider_name,
                                goto_name,
                                now_stamp)
    goto_path = os.path.join(glider_dir,
                             glider_name,
                             "to-glider",
                             goto_name)
    try:
        os.makedirs(backup_dir)
    except:
        pass
    backup_path = os.path.join(backup_dir,
                               backup_name)

    command_line = "%s %s" % (sys.executable,
                              " ".join(sys.argv))
    if copy:
        dest = goto_path
        dupe = backup_path
    else:
        dest = backup_path
        dupe = None

    handle = open(dest, "w")
    handle.write(first_template % (goto_name,
                                   now_string,
                                   command_line,
                                   steps))
    handle.writelines(["\t%.3f\t%.3f\n" % tuple(point) for point in points])
    handle.write(last_template)
    handle.close()

    if dupe:
        shutil.copy2(dest, dupe)
        print "\nCopied from %s to %s." % (dest, dupe),
        sys.stdout.flush()

    time.sleep(1)
    return points

def _bearing(glider_name, glider_pos, heading, distance, steps, copy=False):
    glider_pos = map(ddmm2decdeg, glider_pos)
    glider_pos = map(math.radians, glider_pos)
    glider_pos = latlondict(glider_pos)
    heading = math.radians(heading)
    distance = distance / kmperrad
    dlat = distance * math.cos(heading)
    dlon = distance * math.sin(heading) / math.cos(glider_pos['lat'])
    target = {}
    target['lon'] = glider_pos['lon'] + (steps * dlon)
    target['lat'] = glider_pos['lat'] + (steps * dlat)
    target = latlonlist(target)
    target = map(math.degrees, target)
    target = map(decdeg2ddmm, target)
    glider_pos = latlonlist(glider_pos)
    glider_pos = map(math.degrees, glider_pos)
    glider_pos = map(decdeg2ddmm, glider_pos)
    return _target(glider_name, glider_pos, target, steps, copy)

if __name__ == "__main__":
    num_args = len(sys.argv) - 1
    if not (num_args == 5 or
            num_args == 6 or
            num_args == 7 or
            num_args == 8):
        sys.exit(__doc__)

    try:
        glider_name = sys.argv[1]
        glider_pos = [float(sys.argv[2]), float(sys.argv[3])]
        method = sys.argv[4].lower()
        if method == "bearing":
            heading = target = float(sys.argv[5])
            if num_args > 5:
                distance = float(sys.argv[6])
            else:
                distance = 2
            if num_args > 6:
                steps = int(sys.argv[7])
            else:
                steps = 20
            if num_args > 7:
                copy = False
            else:
                copy = True
            _bearing(glider_name, glider_pos, heading, distance, steps, copy)
        elif method == "target":
            target = [float(sys.argv[5]), float(sys.argv[6])]
            if num_args > 6:
                steps = int(sys.argv[7])
            else:
                steps = 20
            if num_args > 7:
                copy = False
            else:
                copy = True
            _target(glider_name, glider_pos, target, steps, copy)
        else:
            raise Exception("Incorrect points method type.")
    except:
        sys.exit(__doc__)