# http://discussion.forum.nokia.com/forum/showthread.php?s=c9700af706de763105666122a37e1175&threadid=54857

import socket
import traceback
import operator # for cksum
import math # for bearing calc
 
def fix_gps_datestamp(s):
    # 230503        UTC date (day, month, year)
    d,m,y = s[0:2],s[2:4],s[4:6]
    return "20%s%s%s" % (y,m,d)

class BTReader:

    def connect(self):
        self.sock=socket.socket(socket.AF_BT,socket.SOCK_STREAM)
        # address,services=socket.bt_discover()
        # print "Discovered: %s, %s"%(address,services)
        # target=(address,services.values()[0]) 
        # print "Connecting to "+str(target)
        # and stash target?
        # ('00:0e:6d:5e:cc:fb',1) is the socketcomm unit
        target = ('00:0e:6d:5e:cc:fb',1) #  is the socketcomm unit
        self.sock.connect(target)
        self.last = None
        self.lat = None
        self.lon = None
    
    def readposition(self):
        saw_gpgga = 0
        saw_gprmc = 0
        saw_gpgsa = 0
        start_time = time.time()
        self.samples = 0
        self.badcksum = 0
        self.nocksum = 0
        while 1:
            try:
                buffer=""
                ch=self.sock.recv(1)
                while(ch!='$'):
                    ch=self.sock.recv(1)
                while 1:
                    if (ch=='\r'):
                        break
                    buffer+=ch
                    ch=self.sock.recv(1)
                if "*" not in buffer[1:]:
                    self.nocksum += 1
                    continue
                if buffer.count("*") > 1:
                    self.extra_stars = buffer.count("*")
                    continue
                sum_over, cksum = buffer[1:].split("*")
                gen_cksum = "%02X" % reduce(operator.xor, map(ord,sum_over))
                if gen_cksum != cksum:
                    self.badcksum += 1
                    continue
                self.samples += 1
                sentence = sum_over.split(",")
                verb = sentence.pop(0)
                if verb == "GPGGA":
                    saw_gpgga = 1
                    utc_timestamp, lat, lat_pol, lon, lon_pol, fix_qual, num_sats, HDOP, altitude, altitude_units, geoid_height, geoid_units, time_since_dgps, dgps_id = sentence
                    self.lat = float(lat)/100.0
                    if lat_pol == "S": self.lat = -self.lat
                    # lon_pol: W/E
                    self.lon = float(lon)/100.0
                    if lon_pol == "W": self.lon = -self.lon
                    self.timestamp = utc_timestamp
                    self.num_sats = num_sats
                    self.compute_bearing()
		    self.altitude = altitude + altitude_units
                    # return self.lat, self.lon, self.when
                elif verb == "GPRMC":
                    saw_gprmc = 1
                    utc_timestamp, status, lat, lat_pol, lon, lon_pol, ground_speed_kts, track_angle_degs, datestamp, magnetic_variation, mag_var_pol, extra = sentence
                    # lat_pol: N/S
                    self.lat = float(lat)/100.0
                    if lat_pol == "S": self.lat = -self.lat
                    # lon_pol: W/E
                    self.lon = float(lon)/100.0
                    if lon_pol == "W": self.lon = -self.lon
                    self.datestamp = fix_gps_datestamp(datestamp)
                    self.timestamp = utc_timestamp
                    self.ground_speed_kts = ground_speed_kts
                    self.track_angle_degs = track_angle_degs
                    self.compute_bearing()
                elif verb == "GPGSA":
                    saw_gpgsa = 1
                    auto, fixmode = sentence[:2]
                    self.fixmode = fixmode+"/"+auto
                    # self.auto = auto

            except Exception, e:
                self.last_traceback = "".join(traceback.format_exception(sys.exc_type, sys.exc_value, sys.exc_traceback))
                print self.last_traceback
                try:
                    print "sentence:", sentence
                except UnboundLocalError:
                    pass

                # continue
            if saw_gpgga and saw_gprmc and saw_gpgsa and (time.time() - start_time >= 2):
                # scan for at least 2 seconds, to "catch up"
                return
    
    def fields(self):
        return self.lat, self.lon, "%sT%s" % (self.datestamp, self.timestamp), self.ground_speed_kts, self.track_angle_degs, self.altitude
    def compute_bearing(self):
        if not self.last: return
        # most of these fail with nan, not traceback; let the outer catcher deal
        oldlat, oldlon = self.last
        deltalat = self.lat - oldlat # Y
        deltalon = self.lon - oldlon # X
        self.dist = math.hypot(deltalat, deltalon)
        if deltalat == 0 and deltalon == 0:
            self.bearing = None
            return
        self.bearing = 360.0*math.atan2(deltalat, deltalon)/(2*math.pi)

    def close(self):
        self.sock.close()
        print "closed bt socket"

import time
import camera
imgbase = u"E:\\Images"

# bt=BTReader()
# bt.connect()

def take_pic_where():
    global ss
    img = camera.take_photo() # -> graphics.Image
    #lat, lon, when, speed, angle = bt.readposition()
    ss.bt.readposition()
    ss.bt.last = (ss.bt.lat, ss.bt.lon)
    imgpath = time.strftime("%Y%m%dt%H%M%S")
    imgfile = os.path.join(imgbase, imgpath + ".jpg")
    logfile = os.path.join(imgbase, imgpath + ".where")
    img.save(imgfile)
    print >> open(logfile, "w"), ss.bt.fields()
    return imgfile

def btplay():
    bt=BTReader()
    bt.connect()
    print bt.readposition()
    print bt.readposition()
    print bt.readposition()
    print bt.fields()
    print bt.auto, bt.fixmode
    bt.close()

# from System/Apps/Python/imgviewer.py
import key_codes
import e32
lock=e32.Ao_lock()

running=1
def quit():
    global running,lock
    running=0
    lock.signal()

import appuifw
appuifw.app.screen='full'
appuifw.app.exit_key_handler=quit

from appuifw import *
from graphics import *

c=Canvas()
app.body=c
draw_realscreen=Draw(c)
# draw_realscreen.clear(0)

# draw_realscreen.text((0,10),u"bluecamera",(0,0,255))
import thread

def btworker(bt):
    global lock
    while 1:
	e32.ao_sleep(5)
        # bt.readposition() # that really doesn't work...
        lock.signal()


class screenstuff:
    def __init__(self, draw):
        self.title = u"bluecamera"
        self.where = ""
        self.what = ""
        self.age = time.time()
        self.draw = draw
        self.baseline = 10
        self.bt = BTReader()
        self.bt.connect()
        thread.start_new_thread(btworker, (self.bt,))

    def close(self):
        self.bt.close()

    def render(self):
        line1 = (0, 1*self.baseline)
        line2 = (0, 2*self.baseline)
        line3 = (0, 3*self.baseline)
        line4 = (0, 4*self.baseline)
        blue = (0,0,255)
        green = (0,255,0)
        yellow = (255,255,0)
        red = (255,0,0)
	black = 0
	background_3d = (0,99,0)
	background_2d = (99,99,0)
	background_color = {
	    "2/A": background_2d,
	    "3/A": background_3d,
	    }.get(self.bt.fixmode, black)

        self.draw.clear(background_color)
        self.draw.text( line1, self.title, blue)
        self.draw.text( line2, unicode(time.time()-self.age), red)
        self.draw.text( line3, self.what, yellow)
        n = 4
        btkeys = self.bt.__dict__.keys()
        btkeys.sort()
        # consider pre-coding a set of these...
        for linekey in btkeys:
            if linekey == "sock": continue
            lineval = self.bt.__dict__[linekey]
            if not lineval: continue
            line = u"%s: %s" % (linekey, lineval)
            self.draw.text( (0, n*self.baseline), line, green)
            n += 1
        # next, draw a *line* representing the vector?
        # how about reading the list and drawing a line of those?
        
ss = screenstuff(draw_realscreen)

def update_where():
    global ss
    ss.bt.readposition()
    # ss.where = u"|".join(ss.bt.fields())

def makepic():
    global lock,ss
    f = take_pic_where()
    ss.what = f
    ss.age = time.time()
    lock.signal()

def refresh():
    global lock, ss
    lock.signal()
    

c.bind(key_codes.EKey1,makepic)
c.bind(key_codes.EKey2,makepic)
c.bind(key_codes.EKey3,makepic)

c.bind(key_codes.EKey9,refresh)

while running:
    # e32.ao_sleep(0.5)
    update_where()
    ss.render()
    lock.wait()

ss.close()

# next: figure out bearing and distance from last hit...
#
# distance in degrees is easy - hypot(lat-lat, lon-lon)
# angle is easy if you cheat and assume the world is flat:
# atan(lat-lat, lon-lon)