import sys
from pyproj import Geod
from math import sqrt, atan2, cos, hypot, sin, tan, radians, acos


''' CALCULATING DISTANCE ON A FLAT (PLANAR) SURFACE'''



def planar_hypotenuse_distance(x1, y1, x2, y2):
	"""
	Simple Pythagoras' Theorem approach to finding the hypotenuse

		(x2,y2) +
			   /|
			a / | b
			 /  |
	(x1,y1) +---+
			  c

	In a right angled triangle, the length of the hypotenuse is equal to the 
	square root of the sum of the squares of the other two sides.
	(a-squared = b-squared + c-squared)

	This implementation uses no functions from the math library
	(Square root is raising to the power of 0.5)

	You can alternatively use the built-in sqrt() and pow() functions:
	`return sqrt(pow(x1-x2,2) + pow(y1-y2,2))`
	"""
	return ((x1-x2)**2 + (y1-y2)**2)**0.5


def planar_hypotenuse_distance_2(x1, y1, x2, y2):
	"""
	Simple Pythagoras' Theorem approach to finding the hypotenuse, using the 
		implementation built in to the Python math library
	"""
	return hypot(x1-x2, y1-y2)



''' CALCULATING DISTANCE ON A SPHERE'''



# Average radius of WGS84 spherical Earth model in m
R = 6371008.771415

def spherical_equirectangular_distance(x1, y1, x2, y2):
	"""
	Simple Pythagoras' Theorem approach to finding the hypotenuse with an equirectangular
		projection

	This is only an approximation, and should not be used under most circumstances
	"""
	# convert to radians
	y1 = radians(y1)
	x1 = radians(x1)
	y2 = radians(y2)
	x2 = radians(x2)

	# pythagoras
	x = (x2-x1) * cos((y1+y2) / 2)
	y = (y2-y1)
	return sqrt(x**2 + y**2) * R


def spherical_haversine_distance(x1, y1, x2, y2):
	"""
	Calculate the 'as the crow flies' distance between two locations along a sphere using
		the Haversine equation. (HAVERSINE = HAlf inVERse SINE)

	This is a sligltly more complex approach than spherical_cosine_distance() and is more 
		precise. This is the one most commonly used in normal circumstances.
	"""

	# convert to radians (as this is what the math functions expect)
	a1 = radians(y1)
	b1 = radians(x1)
	a2 = radians(y2)
	b2 = radians(x2)

	# half sine
	sa = sin((a2 - a1)*0.5)
	sb = sin((b2 - b1)*0.5)

	# the square of half the chord length between the points
	a = sa * sa + cos(a1) * cos(a2) * sb * sb

	# angular distance in degrees
	c = 2 * atan2(sqrt(a), sqrt(1 - a))
	return c * float(R)


def spherical_cosine_distance(x1, y1, x2, y2):
	"""
	Calculate the 'as the crow flies' distance between two locations along a sphere using
		the Spherical Law of Cosines.

	This is a simpler approach than spherical_haversine_distance() that can give good results 
		on modern computers. However, precision is not generally as good, and this should not 
		be used if sun-metre accuracy is required.
	"""

	# convert to radians
	a = radians(y1)
	b = radians(y2)
	C = radians(x2 - x1)

	# spherical law of cosines
	x = sin(a) * sin(b) + cos(a) * cos(b) * cos(C)

	# return as distance
	return acos(x) * R



''' CALCULATING DISTANCE ON AN ELLIPSOID'''



def ellipsoidal_inverse_vincenty_distance(x1, y1, x2, y2):
	"""
	Calculate the 'as the crow flies' distance between two locations along an ellipsoid using
		the Inverse Vincenty method.
		
	This version is really just to demonstrate the complexity of this method. I would recommend 
		the implementation in `ellipsoidal_inverse_vincenty_distance_2` (below) for use in your
		own code.
	
	This version is hard-coded to WGS84:
		WGS84 = (a=6378137.0, b=6356752.31425, f=298.257223563)
	"""

	# inverse flattening for ellipsoid
	f = 1 / float(298.257223563)

	# eccentricity calculations
	e2 = e2 = f * (2 - f) # 1st eccentricity squared
	e22 = e2 / (1 - e2) # 2nd eccentricity squared

	_iterations = 50

	try:
		EPS = sys.float_info.epsilon
	except AttributeError:
		EPS = 2.2204460492503131e-16

	c1, s1, _ = _r3(y1, f)
	c2, s2, _ = _r3(y2, f)

	c1c2, s1s2 = c1 * c2, s1 * s2
	c1s2, s1c2 = c1 * s2, s1 * c2

	ll = dl = radians(abs(x2 - x1))
	for _ in range(_iterations):
		cll, sll, ll_ = cos(ll), sin(ll), ll

		t2 = c2 * sll, c1s2 - s1c2 * cll
		ss = hypot(*t2)
		if ss < EPS:
			print ("whoops!")
			return -666
		cs = s1s2 + c1c2 * cll
		s = atan2(ss, cs)

		sa = c1c2 * sll / ss
		c2a = 1 - (sa * sa)
		if abs(c2a) < EPS:
			c2a = 0  # equatorial line
			ll = dl + f * sa * s
		else:
			c2sm = cs - 2 * s1s2 / c2a
			ll = dl + _dl(f, c2a, sa, s, cs, ss, c2sm)

		if abs(ll - ll_) < 1.0e-12:   # about 0.006 mm
			break
	else:
		print ("whoops!")
		return -666

	if c2a:  # e22 == (a / b) ^ 2 - 1
		A, B = _p2(c2a, e22)
		s = A * (s - _ds(B, cs, ss, c2sm))

	b = 6356752.31425 # semi minor axis in m
	d = b * s
	return d

def _p2(c2a, ab2):  # A, B polynomials
    u2 = c2a * ab2  # e'2 WGS84 = 0.00673949674227643
    A = u2 / 16384.0 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2))) + 1
    B = u2 /  1024.0 * ( 256 + u2 * (-128 + u2 * ( 74 -  47 * u2)))
    return A, B

def _r3(a, f):  # reduced cos, sin, tan
    t = (1 - f) * tan(radians(a))
    c = 1 / hypot(1, t)
    s = t * c
    return c, s, t

def _dl(f, c2a, sa, s, cs, ss, c2sm):
    C = f / 16.0 * c2a * (4 + f * (4 - 3 * c2a))
    return (1 - C) * f * sa * (s + C * ss * (c2sm + C * cs * (c2sm * c2sm * 2 - 1)))

def _ds(B, cs, ss, c2sm):
    c2sm2 = 2 * c2sm * c2sm - 1
    ss2 = (ss * ss * 4 - 3) * (c2sm2 * 2 - 1)
    return B * ss * (c2sm + B / 4.0 * (c2sm2 * cs - B / 6.0 * c2sm * ss2))


def ellipsoidal_inverse_vincenty_distance_2(x1, y1, x2, y2):
	"""
	Calculate the 'as the crow flies' distance between two locations along an ellipsoid using
		the Inverse Vincenty method, using the implementation built in to the PyProj library.
	"""
	return Geod(ellps='WGS84').inv(x1, y1, x2, y2)[2]



if __name__ == '__main__':
	"""
	* HERE IS A DEMO TO EXAMINE THE IMPACT OF THER DIFFERENT METHODS ON THE BORDER OF USA AND MEXICO
	"""
	# imports
	from pyproj.crs import CRS
	from pyproj.transformer import Transformer
	from geopandas import read_file, GeoSeries

	# get border between USA and Mexico
	world = read_file('./data/natural-earth/ne_50m_admin_0_countries.shp')
	border = world[(world.ISO_A3 == 'USA')]['geometry'].iloc[0].intersection(world[(world.ISO_A3 == 'MEX')]['geometry'].iloc[0])

	# set up transformers
	wgs84 = CRS('+proj=longlat +datum=WGS84 +no_defs')
	global_transformer = Transformer.from_crs(wgs84, CRS('+proj=merc +a=6378137 +b=6378137 +lat_ts=0.0 +lon_0=0.0 +x_0=0.0 +y_0=0 +k=1.0 +units=m +nadgrids=@null +wktext  +no_defs'), always_xy=True)
	local_transformer = Transformer.from_crs(wgs84, CRS('+proj=lcc +lat_1=20 +lat_2=60 +lat_0=40 +lon_0=-96 +x_0=0 +y_0=0 +datum=NAD83 +units=m +no_defs'), always_xy=True)

	# ellipsoidal
	local_proj = local_proj2 = global_proj = global_proj2 = equirectangular = cosine = haversine = ellipsoidal = ellipsoidal2 = 0
	for line_seg in border.geoms:

		# projected
		xs, ys = global_transformer.transform([line_seg.coords[0][0],line_seg.coords[1][0]], 
			[line_seg.coords[0][1], line_seg.coords[1][1]])
		global_proj += planar_hypotenuse_distance(xs[0], ys[0], xs[1], ys[1])
		global_proj2 += planar_hypotenuse_distance_2(xs[0], ys[0], xs[1], ys[1])

		xs, ys = local_transformer.transform([line_seg.coords[0][0],line_seg.coords[1][0]], 
			[line_seg.coords[0][1], line_seg.coords[1][1]])
		local_proj += planar_hypotenuse_distance(xs[0], ys[0], xs[1], ys[1])
		local_proj2 += planar_hypotenuse_distance_2(xs[0], ys[0], xs[1], ys[1])

		# spherical
		equirectangular += spherical_equirectangular_distance(line_seg.coords[0][0],
			line_seg.coords[0][1], line_seg.coords[1][0], line_seg.coords[1][1])
		cosine += spherical_cosine_distance(line_seg.coords[0][0],
			line_seg.coords[0][1], line_seg.coords[1][0], line_seg.coords[1][1])
		haversine += spherical_haversine_distance(line_seg.coords[0][0],
			line_seg.coords[0][1], line_seg.coords[1][0], line_seg.coords[1][1])
		
		# ellipsoidal
		ellipsoidal += ellipsoidal_inverse_vincenty_distance(line_seg.coords[0][0],
			line_seg.coords[0][1], line_seg.coords[1][0], line_seg.coords[1][1])
		ellipsoidal2 += ellipsoidal_inverse_vincenty_distance_2(line_seg.coords[0][0],
			line_seg.coords[0][1], line_seg.coords[1][0], line_seg.coords[1][1])


	# convert to km
	local_proj /=1000
	global_proj /=1000
	local_proj2 /=1000
	global_proj2 /=1000
	equirectangular /=1000
	cosine /=1000
	haversine /=1000
	ellipsoidal /=1000
	ellipsoidal2 /=1000

	# print output
	print ("")
	print ("Planar:")
	print (f"\tGlobal Projection: \t\t{global_proj:,.2f}km ({global_proj - ellipsoidal2:.2f}km)")
	print (f"\tGlobal Projection (hypot): \t{global_proj2:,.2f}km ({global_proj2 - ellipsoidal2:.2f}km)")
	print (f"\tLocal Projection : \t\t{local_proj:,.2f}km ({local_proj - ellipsoidal2:.2f}km)")
	print (f"\tLocal Projection (hypot): \t{local_proj2:,.2f}km ({local_proj2 - ellipsoidal2:.2f}km)")
	print ("Spherical:")
	print (f"\tEquirectangular: \t\t{equirectangular:,.2f}km ({equirectangular - ellipsoidal2:.2f}km)")
	print (f"\tHaversine: \t\t\t{haversine:,.2f}km ({haversine - ellipsoidal2:.2f}km)")
	print (f"\tSpherical Cosine : \t\t{cosine:,.2f}km ({cosine - ellipsoidal2:.2f}km)")
	print ("Ellipsoidal:")
	print (f"\tInverse Vincenty: \t\t{ellipsoidal:,.2f}km ({ellipsoidal - ellipsoidal2:.2f}km)")
	print (f"\tInverse Vincenty (pyproj): \t{ellipsoidal2:,.2f}km (reference distance)")
	print ("")