#!/usr/bin/env python

import math
import sys
import gdal
import gdalconst
import osr
import Numeric

radius_km = 6366.71
km_per_nm = 1.852

def degrees_to_radians(angle_degrees):
	return(math.pi/180.0 * angle_degrees)

def radians_to_degrees(angle_radians):
	return(180.0/math.pi * angle_radians)

def radians_to_nm(distance_radians):
	return(radius_km * distance_radians / km_per_nm)

def nm_to_radians(distance_nm):
	return(distance_nm * km_per_nm / radius_km)

start = (-86.9368989, 40.4123169) # LAF
#start = (-104.8492942, 39.5701283) # APA
#end = (-104.8492942, 39.5701283) # APA
end = (-120.5681889, 37.3804806) # MER

(lon1, lat1) = (degrees_to_radians(start[0]), degrees_to_radians(start[1]))
(lon2, lat2) = (degrees_to_radians(end[0]), degrees_to_radians(end[1]))
d = math.acos(math.sin(lat1)*math.sin(lat2)+math.cos(lat1)*math.cos(lat2)*math.cos(lon1-lon2))

prev = start


for fi in range(0, 11):
	f = fi / 10.0
	A=math.sin((1-f)*d)/math.sin(d)
	B=math.sin(f*d)/math.sin(d)
	x = A*math.cos(lat1)*math.cos(lon1) +  B*math.cos(lat2)*math.cos(lon2)
	y = A*math.cos(lat1)*math.sin(lon1) +  B*math.cos(lat2)*math.sin(lon2)
	z = A*math.sin(lat1)           +  B*math.sin(lat2)
	lat=math.atan2(z,math.sqrt(x*x+y*y))
	lon=math.atan2(y,x)
	print f, radians_to_degrees(lon), radians_to_degrees(lat)

d_nm = radians_to_nm(d)
print d_nm

for i in range(0, int(d_nm), 100) + [0.8 * d_nm, d_nm]:
	f = float(i)/d_nm
	A=math.sin((1-f)*d)/math.sin(d)
	B=math.sin(f*d)/math.sin(d)

	x = A*math.cos(lat1)*math.cos(lon1) +  B*math.cos(lat2)*math.cos(lon2)
	y = A*math.cos(lat1)*math.sin(lon1) +  B*math.cos(lat2)*math.sin(lon2)
	z = A*math.sin(lat1)           +  B*math.sin(lat2)
	lat=math.atan2(z,math.sqrt(x*x+y*y))
	lon=math.atan2(y,x)
	print i, f, radians_to_degrees(lon), radians_to_degrees(lat)

reference_image = gdal.Open('/var/www/aviationtoolbox/munge/chunked/scale_100/chunk_0_0.tif', gdalconst.GA_ReadOnly)
transform = reference_image.GetGeoTransform()
projection = reference_image.GetProjection()
srs = osr.SpatialReference()
srs.ImportFromWkt(projection)
ct_lonlat = srs.CloneGeogCS()
ct_to_lonlat = osr.CoordinateTransformation(srs, ct_lonlat)
ct_from_lonlat = osr.CoordinateTransformation(ct_lonlat, srs)

(X0, Y0, Z0) = ct_from_lonlat.TransformPoint(start[0], start[1])
(X1, Y1, Z1) = ct_from_lonlat.TransformPoint(end[0], end[1])
print X0, Y0
print X1, Y1

# Slope is safe to use for limited testing.
slope = (Y1 - Y0) / (X1 - X0)
print slope

(lon1, lat1) = (degrees_to_radians(start[0]), degrees_to_radians(start[1]))
(lon2, lat2) = (degrees_to_radians(end[0]), degrees_to_radians(end[1]))
d = math.acos(math.sin(lat1)*math.sin(lat2)+math.cos(lat1)*math.cos(lat2)*math.cos(lon1-lon2))
tc1 = math.acos((math.sin(lat2)-math.sin(lat1)*math.cos(d))/(math.sin(d)*math.cos(lat1)))


print '\n\n'
xtd_max = 0
for X in Numeric.arange(X0, X1, (X1 - X0) / 1000)[1:-1]:
	Y = Y0 + slope * (X - X0) 
	#print X, Y
	(lon_deg, lat_deg, elev) = ct_to_lonlat.TransformPoint(X, Y)
	lon = degrees_to_radians(lon_deg)
	lat = degrees_to_radians(lat_deg)
	#print lon, lat

	#dist_AD = acos(sin(0.592539)*sin(0.6021386)+
	#               cos(0.592539)*cos(0.6021386)*cos(2.066470-2.033309))
	#                  = 0.02905 radians (99.8665 nm)
	#crs_AD = acos((sin(0.6021386)-sin(0.592539)*cos(0.02905))/
	#                               (sin(0.02905)*cos(0.592539)))
	#		                  = 1.22473 radians  (70.17 degrees)
	# xtd = asin(sin(0.02905)*sin(1.22473-1.15003))
	#           = 0.00216747 radians
	#             = 0.00216747*180*60/pi =7.4512 nm right of course
	#XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB))

	#lat1 = 0.592539, lon1 = 2.066470
	#lat2 = 0.709186, lon2 = 1.287762
	#lat = 0.6021386, lon = 2.033309
	#tc1 = 1.150035
	dist_AD = math.acos(math.sin(lat1)*math.sin(lat)+ math.cos(lat1)*math.cos(lat)*math.cos(lon1-lon))
	# dist_AD = 0.02905
	crs_AD = math.acos((math.sin(lat)-math.sin(lat1)*math.cos(dist_AD))/(math.sin(dist_AD)*math.cos(lat1)))
	# crs_AD = 1.22473
	xtd = abs(math.asin(math.sin(dist_AD)*math.sin(crs_AD-tc1)))
	if xtd > xtd_max:
		xtd_max = xtd
		print lon_deg, lat_deg, radians_to_nm(xtd)

X_d = (X1 - X0) 
Y_d = (Y1 - Y0)
d_XY_m = math.sqrt(X_d * X_d + Y_d * Y_d)
d_XY_nm = d_XY_m / km_per_nm / 1000
print d_nm, d_XY_nm, d_XY_nm - d_nm