#!c:\\python\\python.exe

import time
import sys
import serial
import math
import win32com.client


########################################################################################################################
# convert from knots to miles.
def knots_to_miles (knots):
    return float(knots) * 1.15077945


########################################################################################################################
# convert from degress/minutes to decimal degrees.
def deg_to_dec (degrees, heading):
    deg  = math.floor(degrees / 100.0)
    frac = (degrees / 100.0 - deg) / 0.6
    ret  = deg + frac

    if heading == "S" or heading == "W":
        ret = -ret

    return ret


########################################################################################################################
# sleep for the specified time while polling the gps device.
def gps_sleep (com, seconds):
    start = time.time()
    
    while 1:
        com.readline()
        if time.time() - start > seconds:
            break


########################################################################################################################
# poll the com port until a GPRMC sentence is found, then return lat/lng, speed in MPH and heading angle.
def get_heading (com):
    while 1:
        line = com.readline()
        if line.startswith("$GPRMC"):
            break
            
    tokens    = line.split(',')
    latitude  = deg_to_dec(float(tokens[3]),tokens[4])
    longitude = deg_to_dec(float(tokens[5]),tokens[6])
    speed     = knots_to_miles(float(tokens[7]))
    heading   = float(tokens[8])

    return (latitude, longitude, speed, heading)


########################################################################################################################
# poll the com port until a GPGLL sentence is found, then convert to lat/lng and return.
def get_position (com):
    while 1:
        line = com.readline()
        if line.startswith("$GPGLL"):
            break

    tokens    = line.split(',')
    latitude  = deg_to_dec(float(tokens[1]),tokens[2])
    longitude = deg_to_dec(float(tokens[3]),tokens[4])

    return (latitude, longitude)


########################################################################################################################
def kml_placemark (name="", vis=1, desc="", long=0.0, lat=0.0, range=6000.0, tilt=45.0, heading=0.0, icon="", \
                   extrude=0, altitudemode="relativeToGround", point_long=0.0, point_lat=0.0, point_alt=50.0):

    kml_dict = \
    {
        'name'         : name,
        'vis'          : vis,
        'desc'         : desc,
        'long'         : long,
        'lat'          : lat,
        'range'        : range,
        'tilt'         : tilt,
        'heading'      : heading,
        'icon'         : icon,
        'extrude'      : extrude,
        'altitudemode' : altitudemode,
        'point_long'   : point_long,
        'point_lat'    : point_lat,
        'point_alt'    : point_alt,
    }

    return """<?xml version="1.0" encoding="UTF-8"?>
<kml xmlns="http://earth.google.com/kml/2.0">
<Folder>
    <name>%(name)s</name>
    <description>%(desc)s</description>
    <Placemark>
        <name>%(name)s</name>
        <visibility>%(vis)d</visibility>
        <description>
            <![CDATA[
                %(desc)s
            ]]>
        </description>
        <LookAt>
            <longitude>%(long)f</longitude>
            <latitude>%(lat)f</latitude>
            <range>%(range)f</range>
            <tilt>%(tilt)f</tilt>
            <heading>%(heading)f</heading>
        </LookAt>
        <Style>
            <IconStyle>
                <Icon>
                    <href>%(icon)s</href>
                </Icon>
            </IconStyle>
        </Style>
        <Point>
            <extrude>%(extrude)d</extrude>
            <altitudeMode>%(altitudemode)s</altitudeMode>
            <coordinates>%(point_long)f,%(point_lat)f,%(point_alt)d</coordinates>
        </Point>
    </Placemark>
</Folder>
</kml>""" % kml_dict


########################################################################################################################
if __name__ == "__main__":
    # counting starts at 0, this is actually com3
    com = serial.Serial(2, 115200)

    if not com.isOpen():
        print "unable to open com port"
        sys.exit(1)

    earth = win32com.client.Dispatch("GoogleEarth.ApplicationGE")

    if not earth.IsInitialized():
        print "Google Earth not running."
        sys.exit(1)

    while 1:
        lat, lng, speed, heading = get_heading(com)

        kml = kml_placemark ( \
            name       = "where we be",
            desc       = "%.2f MPH" % speed
            long       = lng,
            lat        = lat,
            heading    = heading,
            icon       = "http://pedram.openrce.org/dropbox/jeep.png",
            point_long = lng,
            point_lat  = lat,
            extrude    = 1)

        places = earth.GetTemporaryPlaces()
        places.Visibility = 0

        earth.LoadKmlData(kml)
        earth.SetCameraParams(lat, lng, 100.0, 2, 25000.0, 25.0, 0.0, 1.0)
        print ">>> updating coords %f, %f\r" % (lat, lng),

        gps_sleep(com, 30)