import os
import string
import math
import datetime
from PIL import Image

def GetImageDiagonalSize (alt, zoom):
	a = 0.0023
	b = -0.1230
	c = 1.5300
	z = float (zoom)
	zoomScale = a * z * z + b * z + c
	altf = float (alt)
	return (altf * zoomScale)

class WorldPos:
	def __init__(self, newlon, newlat):
		self.lon = float(newlon)
		self.lat = float(newlat)
	def OffsetByFeet(self, hd, offset):
		feetPerLat = 60.0 * 6086.988
		feetPerLon = feetPerLat * math.cos(self.lat * 3.1415926 / 180.0)
		headingRad = hd * 3.1415926 / 180.0
		changeLon = math.sin(headingRad) * offset / feetPerLon
		changeLat = math.cos(headingRad) * offset / feetPerLat;
		return (WorldPos (self.lon + changeLon, self.lat + changeLat))

outf = open ('test.kml', 'w')
outf.write ("<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n")
outf.write ("<kml xmlns=\"http://earth.google.com/kml/2.2\">\n")
outf.write ( "<Document>\n")

fileList = os.listdir('.')
fileind = 0

last_longitude = 0.0
last_latitude = 0.0
last_altitude = 0

inf = open ('ShootLog.csv', 'r')
linestr = file.readline (inf) # read the header and toss it
linestr = file.readline (inf) # read the first real line
while (linestr != ''):
	fieldarr = string.split (linestr, ',')
	linestr = file.readline (inf) # read the next line
	year = int(fieldarr[0])
	month = int(fieldarr[1])
	day = int(fieldarr[2])
	hour = int(fieldarr[3])
	minute = int(fieldarr[4])
	second = int(fieldarr[5])
	longitude = float(fieldarr[6])
	latitude = float(fieldarr[7])
	altitude = int(fieldarr[8])
	speed = int(fieldarr[9])
	heading = float(fieldarr[10])
	timeSinceRelease = int(fieldarr[11])
	zoom = int(fieldarr[12])

	if (fileind >= len (fileList)):
		break
	filename = fileList[fileind]
	fileind = fileind+1
	while (filename != ''):
		fnarr = string.split (filename, '.')
		if (fnarr[0].find("1024")==-1 and (fnarr[1] == 'JPG' or fnarr[1] == 'jpg')):
			break;
		filename = fileList[fileind]
		fileind = fileind+1

	im = Image.open (filename)
	nameforgoogle = "1024"+filename
	im.resize ((1024,768),1).save(nameforgoogle)
	
	print filename, longitude, latitude

	fov = GetImageDiagonalSize (altitude, zoom)
	fovheight = 0.6 * fov
	fovwidth = 0.8 * fov
	center = WorldPos (longitude, latitude)

# try to get a more precise heading
	if (last_longitude != 0.0):
		lon_diff = longitude - last_longitude
		lat_diff = latitude - last_latitude
		if (lon_diff != 0.0 and lat_diff != 0.0):
			lon_size_correct = math.cos(latitude * 3.1415926 / 180.0)
			lon_diff = lon_diff * lon_size_correct
			new_heading = 180.0 * math.atan2 (lon_diff, lat_diff) / 3.1415926
			if (new_heading < 0):
				new_heading = new_heading + 360.0
			if (abs (new_heading - heading) < 5.0):
				heading = new_heading

	last_longitude = longitude
	last_latitude = latitude
	
# correct for speed and time since release
	knotsToFPS = 1.6878098571012
	timeSinceRelease += 0 # consistent lag from picture to recorded on computer
	feetSinceRelease = speed * knotsToFPS * timeSinceRelease / 1000.0
	center = center.OffsetByFeet (heading + 180.0, feetSinceRelease)

# correct for ascending or descending
# need to use MSL to get this right, not AGL
#	dtthis = datetime.datetime (year, month, day, hour, minute, second)
#	if (last_altitude != 0):
#		difftime = dtthis - dtlast
#		ascend_height = altitude - last_altitude
#		ascend_fps = ascend_height / difftime.seconds
#		forward_fps = knotsToFPS * speed
#		if (forward_fps > 0 and ascend_fps != 0):
#			climb_ratio = ascend_fps / forward_fps
#			move_fov = altitude * climb_ratio
#			center = center.OffsetByFeet (heading, move_fov)

#	dtlast = dtthis
#	last_altitude = altitude	
			
# correct for camera aim, constants determined by matching with google earth
	center = center.OffsetByFeet (heading, altitude*0.052508) 
	center = center.OffsetByFeet (heading+90, altitude*0.1211173855)

	northfacingtop = center.OffsetByFeet (0.0, (fovheight / 2.0))
	northfacingtopleft = northfacingtop.OffsetByFeet (-90.0, (fovwidth / 2.0))
	northfacingtopright = northfacingtopleft.OffsetByFeet (90.0, fovwidth)
	northfacingbottomleft = northfacingtopleft.OffsetByFeet (180.0, fovheight)

	boundeast = northfacingtopright.lon;
	boundwest = northfacingtopleft.lon;
	boundnorth = northfacingtopleft.lat;
	boundsouth = northfacingbottomleft.lat;

	bottom = center.OffsetByFeet (180.0, (fovheight / 2.0))
	left = center.OffsetByFeet (-90.0, (fovwidth / 2.0))
	right = center.OffsetByFeet (90.0, (fovwidth / 2.0))

	outf.write ( "<GroundOverlay>\n")
	outf.write ( "\t<name>" + nameforgoogle + "</name>\n")
	outf.write ( "\t<Icon>\n")
	outf.write ( "\t\t<href>" + "http://www.mwilt.org/lookimages/Flying2010-05-10/" + nameforgoogle + "</href>\n")
	outf.write ( "\t\t<viewBoundScale>0.75</viewBoundScale>\n")
	outf.write ( "\t</Icon>\n")
	outf.write ( "\t<LatLonBox>\n")
	outf.write ( "\t\t<north>" + str(boundnorth) + "</north>\n")
	outf.write ( "\t\t<south>" + str(boundsouth) + "</south>\n")
	outf.write ( "\t\t<east>" + str(boundeast) + "</east>\n")
	outf.write ( "\t\t<west>" + str(boundwest) + "</west>\n")
	outf.write ( "\t\t<rotation>" + str(-float(heading)) + "</rotation>\n")
	outf.write ( "\t</LatLonBox>\n")
	outf.write ( "</GroundOverlay>\n")
	
	outf.write ("<Placemark>\n");
	outf.write ("\t<name>" + nameforgoogle + "</name>\n");
	outf.write ("\t<description>Full resolution http://www.mwilt.org/lookimages/Flying2010-05-10/" + filename + "</description>\n");
	outf.write ("\t<Point>\n");
	outf.write ("\t\t<coordinates>" + str(boundwest) + "," + str(boundnorth) + ",0</coordinates>\n");
	outf.write ("\t</Point>\n");
	outf.write ("\t<LookAt>\n");
	outf.write ("\t\t<longitude>" + str(boundwest) + "</longitude>\n");
	outf.write ("\t\t<latitude>" + str(boundnorth) + "</latitude>\n");
	outf.write ("\t\t<range>100</range>\n");
	outf.write ("\t\t<tilt>10</tilt>\n");
	outf.write ("\t\t<heading>0</heading>\n");
	outf.write ("\t</LookAt>\n");
	outf.write ("</Placemark>\n");
	
	outf.flush()

outf.write ( "</Document>\n")
outf.write ( "</kml>\n")
outf.close ()