#!/usr/bin/env python3
# WGS 84
#a = 6378137  # meters
#f = 1 / 298.257223563
#b = 6356752.314245  # meters; b = (1 - f)a
#f: flattening of the ellipsoid
#a: radius of the ellipsoid, meteres
#phi1: latitude of the start point, decimal degrees
#lembda1: longitude of the start point, decimal degrees
#alpha12: bearing, decimal degrees
#s: Distance to endpoint, meters
import sys
from math import atan
from math import atan2
from math import cos
from math import radians
from math import degrees
from math import sin
from math import sqrt
from math import tan
from math import pow

a=6378137.0                             # radius at equator in meters (WGS-84)
f=1/298.257223563                       # flattening of the ellipsoid (WGS-84)
b=(1-f)*a

def get_heading(coord1, coord2):
     lat1 = radians(coord1[0])
     lon1 = radians(coord1[1])
     lat2 = radians(coord2[0])
     lon2 = radians(coord2[1])
     bearing_plot_X =  cos(lat2) * sin(lon2 - lon1)
     bearing_plot_Y = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon1 - lon2)
     heading = degrees(atan2(bearing_plot_X,bearing_plot_Y))
     if heading <0: heading += 360
     return heading

def inverse(coord1,coord2,maxIter=200,tol=10**-12):
    global a
    global f
    global b

    phi_1,L_1,=coord1
    phi_2,L_2,=coord2

    u_1=atan((1-f)*tan(radians(phi_1)))
    u_2=atan((1-f)*tan(radians(phi_2)))

    L=radians(L_2-L_1)

    Lambda=L                                # set initial value of lambda to L

    sin_u1=sin(u_1)
    cos_u1=cos(u_1)
    sin_u2=sin(u_2)
    cos_u2=cos(u_2)

    try:
        iters=0
        for i in range(0,maxIter):
            iters+=1

            cos_lambda=cos(Lambda)
            sin_lambda=sin(Lambda)
            sin_sigma=sqrt((cos_u2*sin(Lambda))**2+(cos_u1*sin_u2-sin_u1*cos_u2*cos_lambda)**2)
            cos_sigma=sin_u1*sin_u2+cos_u1*cos_u2*cos_lambda
            sigma=atan2(sin_sigma,cos_sigma)
            sin_alpha=(cos_u1*cos_u2*sin_lambda)/sin_sigma
            cos_sq_alpha=1-sin_alpha**2
            cos2_sigma_m=cos_sigma-((2*sin_u1*sin_u2)/cos_sq_alpha)
            C=(f/16)*cos_sq_alpha*(4+f*(4-3*cos_sq_alpha))
            Lambda_prev=Lambda
            Lambda=L+(1-C)*f*sin_alpha*(sigma+C*sin_sigma*(cos2_sigma_m+C*cos_sigma*(-1+2*cos2_sigma_m**2)))

            # successful convergence
            diff=abs(Lambda_prev-Lambda)
            if diff<=tol:
                break

        u_sq=cos_sq_alpha*((a**2-b**2)/b**2)
        A=1+(u_sq/16384)*(4096+u_sq*(-768+u_sq*(320-175*u_sq)))
        B=(u_sq/1024)*(256+u_sq*(-128+u_sq*(74-47*u_sq)))
        delta_sig=B*sin_sigma*(cos2_sigma_m+0.25*B*(cos_sigma*(-1+2*cos2_sigma_m**2)-(1/6)*B*cos2_sigma_m*(-3+4*sin_sigma**2)*(-3+4*cos2_sigma_m**2)))

        alpha12 = get_heading(coord1, coord2)
        m=(b*A*(sigma-delta_sig))#/1000                # output distance in m
        return (m,alpha12)
    except ZeroDivisionError:
        return (0,0)


def direct(phi1, lembda1, alpha12, s): #lat, lon, bearing, distance
    global a
    global f
    global b

    piD4 = atan( 1.0 )
    two_pi = piD4 * 8.0
    phi1    = phi1    * piD4 / 45.0
    lembda1 = lembda1 * piD4 / 45.0
    alpha12 = alpha12 * piD4 / 45.0
    if ( alpha12 < 0.0 ) :
        alpha12 = alpha12 + two_pi
    if ( alpha12 > two_pi ) :
        alpha12 = alpha12 - two_pi
    TanU1 = (1-f) * tan(phi1)
    U1 = atan( TanU1 )
    sigma1 = atan2( TanU1, cos(alpha12) )
    Sinalpha = cos(U1) * sin(alpha12)
    cosalpha_sq = 1.0 - Sinalpha * Sinalpha
    u2 = cosalpha_sq * (a * a - b * b ) / (b * b)
    A = 1.0 + (u2 / 16384) * (4096 + u2 * (-768 + u2 * \
        (320 - 175 * u2) ) )
    B = (u2 / 1024) * (256 + u2 * (-128 + u2 * (74 - 47 * u2) ) )
    # Starting with the approx
    sigma = (s / (b * A))
    last_sigma = 2.0 * sigma + 2.0   # something impossible

    # Iterate the following 3 eqs unitl no sig change in sigma
    # two_sigma_m , delta_sigma
    while ( abs( (last_sigma - sigma) / sigma) > 1.0e-9 ):
        two_sigma_m = 2 * sigma1 + sigma
        delta_sigma = B * sin(sigma) * ( cos(two_sigma_m) \
                + (B/4) * (cos(sigma) * \
                (-1 + 2 * pow( cos(two_sigma_m), 2 ) -  \
                (B/6) * cos(two_sigma_m) * \
                (-3 + 4 * pow(sin(sigma), 2 )) *  \
                (-3 + 4 * pow( cos (two_sigma_m), 2 )))))
        last_sigma = sigma
        sigma = (s / (b * A)) + delta_sigma
    phi2 = atan2 ( (sin(U1) * cos(sigma) +\
        cos(U1) * sin(sigma) * cos(alpha12) ), \
        ((1-f) * sqrt( pow(Sinalpha, 2) +  \
        pow(sin(U1) * sin(sigma) - cos(U1) * \
        cos(sigma) * cos(alpha12), 2))))
    lembda = atan2( (sin(sigma) * sin(alpha12 )),\
        (cos(U1) * cos(sigma) -  \
        sin(U1) *  sin(sigma) * cos(alpha12)))
    C = (f/16) * cosalpha_sq * (4 + f * (4 - 3 * cosalpha_sq ))
    omega = lembda - (1-C) * f * Sinalpha *  \
        (sigma + C * sin(sigma) * (cos(two_sigma_m) + \
        C * cos(sigma) * (-1 + 2 *\
        pow(cos(two_sigma_m), 2) )))
    lembda2 = lembda1 + omega
    alpha21 = atan2 ( Sinalpha, (-sin(U1) * \
        sin(sigma) +
        cos(U1) * cos(sigma) * cos(alpha12)))
    alpha21 = alpha21 + two_pi / 2.0
    if ( alpha21 < 0.0 ) :
        alpha21 = alpha21 + two_pi
    if ( alpha21 > two_pi ) :
        alpha21 = alpha21 - two_pi
    phi2 = phi2 * 45.0 / piD4
    lembda2 = lembda2 * 45.0 / piD4
    alpha21 = alpha21 * 45.0 / piD4
    return (phi2, lembda2)#, alpha21

if __name__ == '__main__':
    help = """Usage:
    vincenty.py [option] [value1] [value2] [value3] [value4]
    Get distance between two points in meters:
        vincenty.py inverse lat1 lon1 lat2 lon2
    Get coordinate at a given distance and heading:
        vincenty.py direct lat1 lon1 heading distance
    Get the heading between two coordinates:
        vincenty.py heading lat1 lon1 lat2 lon2"""
    try:
        op1 = float(sys.argv[2])
        op2 = float(sys.argv[3])
        op3 = float(sys.argv[4])
        op4 = float(sys.argv[5])

        if sys.argv[1] == "inverse":
            output = inverse((op1, op2),(op3, op4))[0]
        elif sys.argv[1] == "direct":
            output = str(direct(op1, op2, op3, op4))[1:-1]
        elif sys.argv[1] == "heading":
            output = get_heading((op1, op2),(op3, op4))
        else:
            output = help
        print(output)
    except:
        print(help)
