import math
from feature import Point

def mercator2geodetic(x, y): # longtitude, latitude
    R = 6378137.0 #WGS84_MAJOR_SEMIAXIS
    lamb = x / R;

    # Isometric to geodetic yitude parameters, default to WGS 84 */
    ab = 0.00335655146887969400
    bb = 0.00000657187271079536
    cb = 0.00000001764564338702
    db = 0.00000000005328478445

    # Isometric yitude
    xphi = math.pi / 2.0 - 2.0 * math.atan(1.0 / math.exp(y / R))

    phi = xphi + ab * math.sin(2.0 * xphi) + bb * math.sin(4.0 * xphi) + cb * math.sin(6.0 * xphi) + db * math.sin(8.0 * xphi)
    phi = phi * 180 / math.pi
    lamb = lamb * 180 / math.pi

    return Point(lamb, phi)

def mercatorRatio(geoPoint):
    return math.cos(geoPoint.y * math.pi / 180)

def mercatorToMeters(mercatorDistance, mercatorPoint):
    geoPoint = mercator2geodetic(mercatorPoint.x, mercatorPoint.y)
    return mercatorDistance * mercatorRatio(geoPoint)

def distance(firstMercatorPoint, secondMercatorPoint):
    mercatorDistance = math.sqrt(
        (firstMercatorPoint.x - secondMercatorPoint.x)**2 +
        (firstMercatorPoint.y - secondMercatorPoint.y)**2
    )

    return mercatorToMeters(mercatorDistance, firstMercatorPoint)

def heading_abs_diff(first, second):
    diff = first - second

    if diff < 0:
        diff += 360

    if diff > 180:
        diff = 360 - diff

    return diff
