import numpy as np
import math

# Mean radius of the earth
EARTH_RADIUS = 6371.009

def haversine(lat1, lon1, lat2, lon2):
    """
    Calculate the great circle distance between two points
    on the earth (specified in decimal degrees)
    
    Return (central angle, distance between points in km)
    """
    # convert decimal degrees to radians
    lat1, lon1, lat2, lon2 = [math.radians(x) for x in [lat1, lon1, lat2, lon2]]

    # haversine formula
    dlon = lon2 - lon1
    dlat = lat2 - lat1
    
    a = math.sin(dlat/2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon/2)**2
    central_angle = 2 * math.asin(math.sqrt(a))

    # s = r * theta
    km = EARTH_RADIUS * central_angle
    return (central_angle, km)

"""
Explanation: Calculate the GPS Distance with the Haversine Formula

Dan Couture @MathYourLife, github
2015-03-05

Problem
I've got the start and end gps location from an excursion across town and need to determine the travel distance
start: 43.059535, -71.013171
end:   43.083620, -70.892085

You could always use google maps, but that would just be cheating.
Haversine Formula
The goal for this formula is to calculate the shortest great circle distance between two points on the globe designated by latitude and longitudes.  The added benefit of the Haversine equation is that it calculates the central angle as well where $s = r\theta$.

source: https://software.intel.com/sites/default/files/great%20circle.png
The Haversine formula is mainly based on calculation of the central angle, $\theta$, between two gps coordinates.  Using the formula for arc length on a sphere
$$
s = r \theta
$$
where $r$ is the Earth's radius, and $\theta$ is the central angle calculated as
$$
\theta = 2 \arcsin\left( \sqrt{\sin^2 \left(\frac{\phi_2-\phi_1}{2}\right) + \cos(\phi_1)\cos(\phi_2)\sin^2 \left( \frac{\lambda_2-\lambda_1}{2} \right) } \right)
$$
with:
$$
\begin{align}
\phi &= \text{latitude}\
\lambda &= \text{longitude}\
\end{align}
$$
End of explanation
"""


start = (43.059535, -71.013171)
end = (43.083620, -70.892085)

central_angle, km = haversine(*(start + end))

print("Central Angle of %g radians" % central_angle)
print("Arc length distance of %g km" % km)

"""
Explanation: Calculate Excursion Distance
End of explanation
"""


