# arch-tag: 413a3fa1-5a89-41f2-a905-0ad78cdd0267
from math import *

#############################################################################
##
## Copyright (C) 2002-2008 Chris Veness -- http://www.movable-type.co.uk/scripts/latlong-vincenty.html
## Copyright (C) 2007-2008 Sascha Silbe -- http://sascha.silbe.org/
##
## This file may be distributed and/or modified under the terms of the
## GNU Lesser General Public License version 2 as published by the Free
## Software Foundation and appearing in the file COPYING.LESSER included in
## the packaging of this file.
##
#############################################################################

# source of algorithm:
#       Vincenty direct formula - T Vincenty, "Direct and Inverse Solutions of Geodesics on the
#       Ellipsoid with application of nested equations", Survey Review, vol XXII no 176, 1975
#       http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
#
# based on JavaScript implementation by Chris Veness, available at
# http://www.movable-type.co.uk/scripts/latlong-vincenty-direct.html
# Python implementation by Sascha Silbe
#
# License for original JavaScript implementation:
# JavaScript functions are shown below. You are welcome to re-use these scripts [without any warranty express or implied]
# provided you retain my copyright notice and when possible a link to my website (under a LGPL license). If you have any
# queries or find any problems, please contact me.
# (C) 2002-2008 Chris Veness


# WGS-84 ellipsiod
a = 6378137.
b = 6356752.3142
f = 1/298.257223563

def distance(p1, p2) :
  p1Lat = p1[0]*pi/180
  p1Lon = p1[1]*pi/180
  p2Lat = p2[0]*pi/180
  p2Lon = p2[1]*pi/180
  L = p2Lon - p1Lon
  U1 = atan((1-f) * tan(p1Lat))
  U2 = atan((1-f) * tan(p2Lat))
  sinU1 = sin(U1)
  cosU1 = cos(U1)
  sinU2 = sin(U2)
  cosU2 = cos(U2)
  Lambda = L
  LambdaP = 2*pi
  iterLimit = 20

  while ((fabs(Lambda-LambdaP) > 1e-12) and iterLimit) :
    iterLimit -= 1
    sinLambda = sin(Lambda)
    cosLambda = cos(Lambda)
    sinSigma = sqrt((cosU2*sinLambda) * (cosU2*sinLambda) +
      (cosU1*sinU2-sinU1*cosU2*cosLambda) * (cosU1*sinU2-sinU1*cosU2*cosLambda))

    if (sinSigma==0) :
      return 0, 0  # co-incident points

    cosSigma = sinU1*sinU2 + cosU1*cosU2*cosLambda
    sigma = atan2(sinSigma, cosSigma)
    sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma
    cosSqAlpha = 1 - sinAlpha*sinAlpha
    if (cosSqAlpha == 0) :
      cos2SigmaM = 0  # equatorial line: cosSqAlpha=0 (Rule 6)
    else :
      cos2SigmaM = cosSigma - 2*sinU1*sinU2/cosSqAlpha

    C = f/16*cosSqAlpha*(4+f*(4-3*cosSqAlpha))
    LambdaP = Lambda
    Lambda = L + (1-C) * f * sinAlpha * (sigma + C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)))

  if (iterLimit==0) :
    # formula failed to converge
    return None, None

  uSq = cosSqAlpha * (a*a - b*b) / (b*b)
  A = 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq)))
  B = uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq)))
  deltaSigma = B*sinSigma*(cos2SigmaM+B/4*(cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)-B/6*cos2SigmaM*(-3+4*sinSigma*sinSigma)*(-3+4*cos2SigmaM*cos2SigmaM)))
  s = b*A*(sigma-deltaSigma)
  # initial bearing
  bearing = atan2(cosU2*sinLambda, cosU1*sinU2 - sinU1*cosU2*cosLambda)*180/pi
  # final bearing
  # bearing = atan2(cosU1*sinLambda, -sinU1*cosU2 + cosU1*sinU2*cosLambda)*180/pi

  while (bearing < 0) :
    bearing += 360

  return s, bearing


def destination(origin, bearing, distance) :
  lat1, lon1 = origin
  s = float(distance)
  alpha1 = bearing*pi/180
  sinAlpha1 = sin(alpha1)
  cosAlpha1 = cos(alpha1)

  tanU1 = (1-f) * tan(lat1*pi/180)
  cosU1 = 1. / sqrt((1 + tanU1*tanU1))
  sinU1 = tanU1*cosU1
  sigma1 = atan2(tanU1, cosAlpha1)
  sinAlpha = cosU1 * sinAlpha1
  cosSqAlpha = 1 - sinAlpha*sinAlpha
  uSq = cosSqAlpha * (a*a - b*b) / (b*b)
  A = 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq)))
  B = uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq)))

  sigma = s / (b*A)
  sigmaP = 2*pi
  while (abs(sigma-sigmaP) > 1e-12) :
    cos2SigmaM = cos(2*sigma1 + sigma)
    sinSigma = sin(sigma)
    cosSigma = cos(sigma)
    deltaSigma = B*sinSigma*(cos2SigmaM+B/4*(cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)-B/6*cos2SigmaM*(-3+4*sinSigma*sinSigma)*(-3+4*cos2SigmaM*cos2SigmaM)))
    sigmaP = sigma
    sigma = s / (b*A) + deltaSigma

  tmp = sinU1*sinSigma - cosU1*cosSigma*cosAlpha1
  lat2 = atan2(sinU1*cosSigma + cosU1*sinSigma*cosAlpha1, (1-f)*sqrt(sinAlpha*sinAlpha + tmp*tmp))
  Lambda = atan2(sinSigma*sinAlpha1, cosU1*cosSigma - sinU1*sinSigma*cosAlpha1)
  C = f/16*cosSqAlpha*(4+f*(4-3*cosSqAlpha))
  L = Lambda - (1-C) * f * sinAlpha * (sigma + C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)))

#  revAz = atan2(sinAlpha, -tmp)  # final bearing
  lat2D = lat2*180/pi
  lon2D = lon1+L*180/pi

  while (lat2D > 90) :
    lat2D -= 180

  while (lat2D < -90) :
    lat2D += 180

  while (lon2D > 180) :
    lon2D -= 360

  while (lon2D < -180) :
    lon2D += 360

  return (lat2D, lon2D)