track = np.genfromtxt('traj.csv', delimiter=',', names=['time', 'lat', 'lon'], dtype=None,
    converters={0: lambda s: datetime.datetime.strptime(s, '%m/%d/%Y %H:%M')})

print track[[0, -1]]

from mpl_toolkits.basemap import Basemap

fig = figure(figsize=(15,9))
ax = fig.gca()

width = 5000000
height = 6000000
m = Basemap(width=width, height=height, projection='aeqd', lat_0=-36, lon_0=-37, resolution='h')
m.drawcoastlines(linewidth=0.25)
m.fillcontinents(color='#000000', lake_color='#666666')
m.drawmapboundary(fill_color='#666666')

color = "#b58900"
xx, yy = m(track['lon'], track['lat'])
m.plot(xx, yy, '-', color=color)
m.plot(xx, yy, '.', color=color)

plt.text(xx[0], yy[0], track['time'][0], fontsize=7, weight='bold', va='center', ha='left', color='w')
plt.text(xx[-1], yy[-1], track['time'][-1], fontsize=7, weight='bold', va='center', ha='left', color='w')

# I'm using the development version from http://code.dealmeida.net/pydap
from pydap.client import open_url

dataset = open_url("http://dap.marinexplore.com/dap/u/7b1af958cfd3/61ee8e7a-aecd-11e2-b206-22000a1c6aad")
print dataset

lon = dataset.lon0[:]
i0 = (lon <= -50).nonzero()[0].max()
i1 = (lon < -5).nonzero()[0].max() + 1
lon = lon[i0:i1]
    
lat = dataset.lat0[:]
j0 = (lat >= 0).nonzero()[0].max()
j1 = (lat > -65).nonzero()[0].max() + 1
lat = lat[j0:j1]
    
U = dataset.g_current_speed_vector_u.g_current_speed_vector_u[:,:,j0:j1,i0:i1]
V = dataset.g_current_speed_vector_v.g_current_speed_vector_v[:,:,j0:j1,i0:i1]

# remove vertical dimension and mask NaN
U = U[:,0,:,:]
U[isnan(U)] = 0.0
V = V[:,0,:,:]
V[isnan(V)] = 0.0

import coards
T = np.array([coards.format(coards.parse(value, dataset.time0.units), 'seconds since 1970-01-01') for value in dataset.time0])

fig = figure(figsize=(15,9))
ax = fig.gca()

width = 5000000
height = 6000000
m = Basemap(width=width, height=height, projection='aeqd', lat_0=-36, lon_0=-37, resolution='h')
m.drawcoastlines(linewidth=0.25)
m.fillcontinents(color='#000000', lake_color='#666666')
m.drawmapboundary(fill_color='#666666')

X, Y = np.meshgrid(lon, lat)
xx, yy = m(X, Y)
m.quiver(xx, yy, np.mean(U, axis=0), np.mean(V, axis=0), color='white')

color = "#dc322f"
xx, yy = m(track['lon'], track['lat'])
m.plot(xx, yy, '-', color=color)
m.plot(xx, yy, '.', color=color)

def distance(lat1, lon1, lat2, lon2):
    # http://www.movable-type.co.uk/scripts/latlong.html
    R = 6.371e6
    lat1 *= pi/180.
    lon1 *= pi/180.
    lat2 *= pi/180.
    lon2 *= pi/180.
    return R*np.arccos(
        np.sin(lat1)*np.sin(lat2) + 
        np.cos(lat1)*np.cos(lat2)*np.cos(lon2-lon1))

def bearing(lat1, lon1, lat2, lon2):
    # http://www.movable-type.co.uk/scripts/latlong.html
    lat1 *= pi/180.
    lon1 *= pi/180.
    lat2 *= pi/180.
    lon2 *= pi/180.
    y = np.sin(lon2-lon1)*np.cos(lat2)
    x = np.cos(lat1)*np.sin(lat2) - np.sin(lat1)*np.cos(lat2)*np.cos(lon2-lon1)
    return (pi/2) - np.arctan2(y, x)

x = lon
y = lat

G = {}
for i in range(len(x)):
    for j in range(len(y)):
        G[j, i] = []
        if i > 0:
            if j > 0:
                G[j, i].append((j-1, i-1))
            if j+1 < len(y):
                G[j, i].append((j+1, i-1))
            G[j, i].append((j, i-1))
        if i+1 < len(x):
            if j > 0:
                G[j, i].append((j-1, i+1))
            if j+1 < len(y):
                G[j, i].append((j+1, i+1))
            G[j, i].append((j, i+1))
        if j > 0:
            G[j, i].append((j-1, i))
        if j+1 < len(y):
            G[j, i].append((j+1, i))

import heapq

def shortest_path(X, Y, U, V, T, G, start, end, s0=0, t0=0):
    q = [(t0, start, ())]
    visited = {}
    while 1:
        cost, v1, path = heapq.heappop(q)
        if v1 not in visited or visited[v1] > cost:
            path = path + (v1,)
            visited[v1] = cost
            if v1 == end:
                return list(path), cost
            for v2 in G[v1]:
                cost2 = calculate_cost(X, Y, U, V, T, cost, v1, v2, s0)
                if (v2 not in visited or visited[v2] > cost2) and cost+cost2 <= T[-1]:
                    heapq.heappush(q, (cost + cost2, v2, path))

def calculate_cost(X, Y, U, V, T, t, v1, v2, s0):
    l = (T <= t).nonzero()[0].max()
    j1, i1 = v1
    j2, i2 = v2
    
    u = (U[l,j1,i1] + U[l,j2,i2])/2.
    v = (V[l,j1,i1] + V[l,j2,i2])/2.
    
    ds = distance(Y[v1], X[v1], Y[v2], X[v2])
    a = bearing(Y[v1], X[v1], Y[v2], X[v2])
    
    # velocity along track
    s = s0 + u*np.cos(a) + v*np.sin(a)
    if s < 0:
        return np.inf
    else:
        return ds/s

# get initial and final nodes
i = (x <= track['lon'][0]).nonzero()[0].max()
j = (y <= track['lat'][0]).nonzero()[0].min()
v1 = j, i
i = (x <= track['lon'][-1]).nonzero()[0].max()
j = (y <= track['lat'][-1]).nonzero()[0].min()
v2 = j, i

s0 = 0.86
t0 = coards.format(track['time'][0], 'seconds since 1970-01-01')
path, end = shortest_path(X, Y, U, V, T, G, v1, v2, s0, t0)

fig = figure(figsize=(15,9))
ax = fig.gca()

width = 5000000
height = 6000000
m = Basemap(width=width, height=height, projection='aeqd', lat_0=-36, lon_0=-37, resolution='h')
m.drawcoastlines(linewidth=0.25)
m.fillcontinents(color='#000000', lake_color='#666666')
m.drawmapboundary(fill_color='#666666')

X, Y = np.meshgrid(lon, lat)
xx, yy = m(X, Y)
m.quiver(xx, yy, np.mean(U, axis=0), np.mean(V, axis=0), color='white')

color = "#dc322f"
xx, yy = m(track['lon'], track['lat'])
m.plot(xx, yy, '-', color=color)
m.plot(xx, yy, '.', color=color)

xx = [X[s] for s in path]
yy = [Y[s] for s in path]
xx, yy = m(xx, yy)
m.plot(xx, yy, 'k-')