# From: http://williams.best.vwh.net/avform.htm#GCF

import math

EPS = 0.0001
d2r = math.pi / 180.0
r2d = 180.0 / math.pi
rad2nm = (180.0 * 60.0) / math.pi
nm2rad = 1.0 / rad2nm
nm2meter = 1852
meter2nm = 1.0 / nm2meter

# p1 = (lat1(deg), lon1(deg))
# p2 = (lat2(deg), lon2(deg))
def course_and_dist(p1, p2):
    # this formulations uses postive lon = W (opposite of usual, so we
    # invert the longitude.)
    lat1 = p1[0] * d2r
    lon1 = -p1[1] * d2r
    lat2 = p2[0] * d2r
    lon2 = -p2[1] * d2r

    dist_rad = 2.0 * math.asin(math.sqrt((math.sin((lat1-lat2)/2.0))**2.0 + math.cos(lat1)*math.cos(lat2)*(math.sin((lon1-lon2)/2.0))**2))

    # if starting point is on a pole
    if math.cos(lat1) < EPS:
        # EPS a small number ~ machine precision
        if (lat1 > 0.0):
            # starting from N pole
            tc1_rad = math.pi
        else:
            # starting from S pole
            tc1_rad = 2.0 * math.pi

    # For starting points other than the poles:
    if dist_rad < 0.000000001:
        # about a cm
        tc1_rad = 0.0
    else:
        num1 = math.sin(lat2) - math.sin(lat1)*math.cos(dist_rad)
        den1 = math.sin(dist_rad) * math.cos(lat1)
        tmp1 = num1 / den1
        if tmp1 < -1.0:
            #print "CLIPPING TMP1 to -1.0!"
            tmp1 = -1.0
        if tmp1 > 1.0:
            #print "CLIPPING TMP1 to 1.0!"
            tmp1 = 1.0
        if math.sin(lon2-lon1) < 0.0:
            tc1_rad = math.acos(tmp1)
        else:
            tc1_rad = 2.0 * math.pi - math.acos(tmp1)

    dist_nm = dist_rad * rad2nm
    dist_m = dist_nm * nm2meter

    tc1_deg = tc1_rad * r2d

    return (tc1_deg, dist_m)

def project_course_distance(p1, course_deg, dist_m):
    lat1 = p1[0] * d2r
    lon1 = -p1[1] * d2r
    tc = course_deg * d2r
    d = dist_m * meter2nm * nm2rad
    lat = math.asin(math.sin(lat1)*math.cos(d)+math.cos(lat1)*math.sin(d)*math.cos(tc))
    if math.cos(lat) < EPS:
        lon = lon1      # endpoint a pole
    else:
        lon = math.fmod(lon1-math.asin(math.sin(tc)*math.sin(d)/math.cos(lat))+math.pi, 2*math.pi) - math.pi
    return (lat*r2d, -lon*r2d)

[((26, 7, 26, 21), 'math.cos', 'math.cos', ({(26, 16, 26, 20): 'lat1'}, {}), '(lat1)', False, 'import math\n'), ((67, 7, 67, 20), 'math.cos', 'math.cos', ({(67, 16, 67, 19): 'lat'}, {}), '(lat)', False, 'import math\n'), ((40, 15, 40, 29), 'math.sin', 'math.sin', ({(40, 24, 40, 28): 'lat2'}, {}), '(lat2)', False, 'import math\n'), ((41, 15, 41, 33), 'math.sin', 'math.sin', ({(41, 24, 41, 32): 'dist_rad'}, {}), '(dist_rad)', False, 'import math\n'), ((41, 36, 41, 50), 'math.cos', 'math.cos', ({(41, 45, 41, 49): 'lat1'}, {}), '(lat1)', False, 'import math\n'), ((49, 11, 49, 30), 'math.sin', 'math.sin', ({(49, 20, 49, 29): '(lon2 - lon1)'}, {}), '(lon2 - lon1)', False, 'import math\n'), ((50, 22, 50, 37), 'math.acos', 'math.acos', ({(50, 32, 50, 36): 'tmp1'}, {}), '(tmp1)', False, 'import math\n'), ((40, 32, 40, 46), 'math.sin', 'math.sin', ({(40, 41, 40, 45): 'lat1'}, {}), '(lat1)', False, 'import math\n'), ((40, 47, 40, 65), 'math.cos', 'math.cos', ({(40, 56, 40, 64): 'dist_rad'}, {}), '(dist_rad)', False, 'import math\n'), ((52, 38, 52, 53), 'math.acos', 'math.acos', ({(52, 48, 52, 52): 'tmp1'}, {}), '(tmp1)', False, 'import math\n'), ((66, 20, 66, 34), 'math.sin', 'math.sin', ({(66, 29, 66, 33): 'lat1'}, {}), '(lat1)', False, 'import math\n'), ((66, 35, 66, 46), 'math.cos', 'math.cos', ({(66, 44, 66, 45): 'd'}, {}), '(d)', False, 'import math\n'), ((66, 74, 66, 86), 'math.cos', 'math.cos', ({(66, 83, 66, 85): 'tc'}, {}), '(tc)', False, 'import math\n'), ((66, 47, 66, 61), 'math.cos', 'math.cos', ({(66, 56, 66, 60): 'lat1'}, {}), '(lat1)', False, 'import math\n'), ((66, 62, 66, 73), 'math.sin', 'math.sin', ({(66, 71, 66, 72): 'd'}, {}), '(d)', False, 'import math\n'), ((23, 42, 23, 67), 'math.sin', 'math.sin', ({(23, 51, 23, 66): '((lat1 - lat2) / 2.0)'}, {}), '((lat1 - lat2) / 2.0)', False, 'import math\n'), ((23, 76, 23, 90), 'math.cos', 'math.cos', ({(23, 85, 23, 89): 'lat1'}, {}), '(lat1)', False, 'import math\n'), ((23, 91, 23, 105), 'math.cos', 'math.cos', ({(23, 100, 23, 104): 'lat2'}, {}), '(lat2)', False, 'import math\n'), ((23, 107, 23, 132), 'math.sin', 'math.sin', ({(23, 116, 23, 131): '((lon1 - lon2) / 2.0)'}, {}), '((lon1 - lon2) / 2.0)', False, 'import math\n'), ((70, 64, 70, 77), 'math.cos', 'math.cos', ({(70, 73, 70, 76): 'lat'}, {}), '(lat)', False, 'import math\n'), ((70, 39, 70, 51), 'math.sin', 'math.sin', ({(70, 48, 70, 50): 'tc'}, {}), '(tc)', False, 'import math\n'), ((70, 52, 70, 63), 'math.sin', 'math.sin', ({(70, 61, 70, 62): 'd'}, {}), '(d)', False, 'import math\n')]