#!/usr/bin/env python
#
# This file is Copyright (c) 2010 by the GPSD project
# BSD terms apply: see the file COPYING in the distribution root for details.
#
# Collect and plot latency-profiling data from a running gpsd.
# Requires gnuplot.
#
import sys, os, time, getopt, socket, math, copy
import gps

class Baton:
    "Ship progress indication to stderr."
    def __init__(self, prompt, endmsg=None):
        self.stream = sys.stderr
        self.stream.write(prompt + "...")
        if os.isatty(self.stream.fileno()):
            self.stream.write(" \010")
        self.stream.flush()
        self.count = 0
        self.endmsg = endmsg
        self.time = time.time()
        return

    def twirl(self, ch=None):
        if self.stream is None:
            return
        if ch:
            self.stream.write(ch)
        elif os.isatty(self.stream.fileno()):
            self.stream.write("-/|\\"[self.count % 4])
            self.stream.write("\010")
        self.count = self.count + 1
        self.stream.flush()
        return

    def end(self, msg=None):
        if msg == None:
            msg = self.endmsg
        if self.stream:
            self.stream.write("...(%2.2f sec) %s.\n" % (time.time() - self.time, msg))
        return

class spaceplot:
    "Spatial scattergram of fixes."
    name = "space"
    def __init__(self):
        self.fixes = []
    def d(self, a, b):
        return math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
    def gather(self, session):
        # Include altitude, not used here, for 3D plot experiments.
        # Watch out for the NaN value from gps.py.
        self.fixes.append((session.fix.latitude, session.fix.longitude, session.fix.altitude))
        return True
    def header(self, session, device):
        res = "# Position uncertainty, %s, %s, %ds cycle\n" % \
                 (title, device["driver"], device["cycle"])
        return res
    def data(self, unused):
        res = ""
        for i in range(len(self.recentered)):
            (lat, lon) = self.recentered[i][:2]
            (raw1, raw2, alt) = self.fixes[i]
            res += "%f\t%f\t%f\t%f\t%f\n" % (lat, lon, raw1, raw2, alt)
        return res
    def plot(self, unused, session, device):
        if len(self.fixes) == 0:
            sys.stderr.write("No fixes collected, can't estimate accuracy.")
            sys.exit(1)
	# centroid is just arithmetic avg of lat,lon
        self.centroid = (sum(map(lambda x:x[0], self.fixes))/len(self.fixes), sum(map(lambda x:x[1], self.fixes))/len(self.fixes))
        # Sort fixes by distance from centroid
        self.fixes.sort(lambda x, y: cmp(self.d(self.centroid, x), self.d(self.centroid, y)))
        # Convert fixes to offsets from centroid in meters
        self.recentered = map(lambda fix: gps.MeterOffset(self.centroid, fix[:2]), self.fixes)
        # Compute CEP(50%)
        cep_meters = gps.EarthDistance(self.centroid[:2], self.fixes[int(len(self.fixes)*0.50)][:2])
        cep95_meters = gps.EarthDistance(self.centroid[:2], self.fixes[int(len(self.fixes)*0.95)][:2])
        cep99_meters = gps.EarthDistance(self.centroid[:2], self.fixes[int(len(self.fixes)*0.99)][:2])
        alt_sum = 0
        alt_num = 0
        alt_fixes = []
        lon_max = -9999
        for i in range(len(self.recentered)):
            (lat, lon) = self.recentered[i][:2]
            (raw1, raw2, alt) = self.fixes[i]
            if not gps.isnan(alt):
                    alt_sum += alt
                    alt_fixes.append( alt)
                    alt_num += 1
            if lon > lon_max :
                    lon_max = lon
        if alt_num == 0:
            alt_avg = gps.NaN
            alt_ep = gps.NaN
        else:
            alt_avg = alt_sum / alt_num
            # Sort fixes by distance from average altitude
            alt_fixes.sort(lambda x, y: cmp(abs(alt_avg - x), abs(alt_avg - y)))
            alt_ep = abs( alt_fixes[ len(alt_fixes)/2 ] - alt_avg)
        if self.centroid[0] < 0:
            latstring = "%fS" % -self.centroid[0]
        elif self.centroid[0] == 0:
            latstring = "0"
        else:
            latstring = "%fN" % self.centroid[0]
        if self.centroid[1] < 0:
            lonstring = "%fW" % -self.centroid[1]
        elif self.centroid[1] == 0:
            lonstring = "0"
        else:
            lonstring = "%fE" % self.centroid[1]
        fmt = "set autoscale\n"
        fmt += 'set key below\n'
        fmt += 'set key title "%s"\n' % time.asctime()
        fmt += 'set size ratio -1\n'
        fmt += 'set style line 2 pt 1\n'
        fmt += 'set style line 3 pt 2\n'
        fmt += 'set xlabel "Meters east from %s"\n' % lonstring
        fmt += 'set ylabel "Meters north from %s"\n' % latstring
        fmt += 'set border 15\n'
        if not gps.isnan(alt_avg):
            fmt += 'set y2label "Meters Altitude from %f"\n' % alt_avg
            fmt += 'set ytics nomirror\n'
            fmt += 'set y2tics\n'
        fmt += 'cep=%f\n' % self.d((0,0), self.recentered[len(self.fixes)/2])
        fmt += 'cep95=%f\n' % self.d((0,0), self.recentered[int(len(self.fixes)*0.95)])
        fmt += 'cep99=%f\n' % self.d((0,0), self.recentered[int(len(self.fixes)*0.99)])
        fmt += 'set parametric\n'
        fmt += 'set trange [0:2*pi]\n'
        fmt += 'cx(t, r) = sin(t)*r\n'
        fmt += 'cy(t, r) = cos(t)*r\n'
        fmt += 'chlen = cep/20\n'
        fmt += "set arrow from -chlen,0 to chlen,0 nohead\n"
        fmt += "set arrow from 0,-chlen to 0,chlen nohead\n"
        if len(self.fixes) > 1000: plot_style = 'dots'
        else: plot_style = 'points'
        fmt += 'plot "-" using 1:2 with ' + plot_style + ' ls 3 title "%d GPS fixes" ' % (len(self.fixes))
        if not gps.isnan(alt_avg):
            fmt += ', "-" using ( %f ):($5 < 100000 ? $5 - %f : 1/0) axes x1y2 with %s ls 2 title " %d Altitude fixes, Average = %f, EP (50%%) = %f"' % (lon_max +1, alt_avg, plot_style, alt_num, alt_avg, alt_ep)
        fmt += ', cx(t, cep),cy(t, cep) ls 1 title "CEP (50%%) = %f meters"' % (cep_meters)
        fmt += ', cx(t, cep95),cy(t, cep95) title "CEP (95%%) = %f meters"' % (cep95_meters)
        fmt += ', cx(t, cep99),cy(t, cep99) title "CEP (99%%) = %f meters"' % (cep99_meters)
        fmt += "\n"
        fmt += self.header(session, device)
        fmt += self.data(session)
        if not gps.isnan(alt_avg):
            fmt += "e\n" + self.data(session)
        return fmt

class uninstrumented:
    "Total times without instrumentation."
    name = "uninstrumented"
    def __init__(self):
        self.stats = []
    def gather(self, session):
        if session.fix.time:
            seconds = time.time() - gps.misc.isotime(session.data.time)
            self.stats.append(seconds)
            return True
        else:
            return False
    def header(self, session, device):
        return "# Uninstrumented total latency, %s, %s, %dN%d, cycle %ds\n" % \
                 (title,
                  device["driver"], device["bps"],
                  device["stopbits"], device["cycle"])
    def data(self, unused):
        res = ""
        for seconds in self.stats:
            res += "%2.6lf\n" % seconds
        return res
    def plot(self, title, session, device):
        fmt = '''
set autoscale
set key below
set key title "Uninstrumented total latency, %s, %s, %dN%d, cycle %ds"
plot "-" using 0:1 title "Total time" with impulses
'''
        res = fmt % (title,
                      device['driver'], device['bps'],
                      device['stopbits'], device['cycle'])
        res += self.header(session, device)
        return res + self.data(session)

formatters = (spaceplot, uninstrumented)

def plotframe(await, fname, threshold, title):
    "Return a string containing a GNUplot script "
    if fname:
        for formatter in formatters:
            if formatter.name == fname:
                plotter = formatter()
                break
        else:
            sys.stderr.write("gpsprof: no such formatter.\n")
            sys.exit(1)
    try:
        session = gps.gps(verbose=verbose)
    except socket.error:
        sys.stderr.write("gpsprof: gpsd unreachable.\n")
        sys.exit(1)
    # Initialize
    session.read()
    if session.version == None:
        print >>sys.stderr, "gpsprof: requires gpsd to speak new protocol."
        sys.exit(1)
    # Set parameters
    options = ""
    if formatter not in (spaceplot, uninstrumented):
        options = ',"timing":true'
    try:
        session.send('?WATCH={"enable":true,"json":true%s}' % options)
        baton = Baton("gpsprof: looking for fix", "done")
        countdown = await
        basetime = time.time()
        while countdown > 0:
            if session.read() == -1:
                sys.stderr.write("gpsprof: gpsd has vanished.\n")
                sys.exit(1)
            baton.twirl()
            if session.data["class"] == "DEVICES":
                if len(session.data["devices"]) != 1:
                    print >>sys.stderr, "exactly one device must be attached.\n"
                    sys.exit(1)
                device = copy.copy(session.data["devices"][0])
                sys.stderr.write("found %s device @%sbps..." % (device["driver"], device["bps"]))
            if session.data["class"] == "WATCH":
                if "timing" in options and not session.data.get("xmit_time"):
                    sys.stderr.write("gpsprof: timing is not enabled.\n")
                    sys.exit(1)
            # We can get some funky artifacts at start of session
            # apparently due to RS232 buffering effects. Ignore
            # them.
            if threshold and time.time()-basetime < session.cycle * threshold:
                continue
            if session.fix.mode <= gps.MODE_NO_FIX:
                continue
            if countdown == await:
                sys.stderr.write("first fix in %.2fsec, gathering %d samples..." % (time.time()-basetime,await))
            if plotter.gather(session):
                countdown -= 1
        baton.end()
    finally:
        session.send('?WATCH={"enable":false,"timing":false}')
    command = plotter.plot(title, session, device)
    del session
    return command

if __name__ == '__main__':
    try:
        (options, arguments) = getopt.getopt(sys.argv[1:], "f:hm:n:s:t:D:")

        formatter = "space"
        raw = False
        title = time.ctime()
        threshold = 0
        await = 100
        verbose = 0
        for (switch, val) in options:
            if (switch == '-f'):
                formatter = val
            elif (switch == '-m'):
                threshold = int(val)
            elif (switch == '-n'):
                await = int(val)
            elif (switch == '-t'):
                title = val
            elif (switch == '-D'):
                verbose = int(val)
            elif (switch == '-h'):
                sys.stderr.write(\
                    "usage: gpsprof [-h] [-D debuglevel] [-m threshold] [-n samplecount] \n"
                     + "\t[-f {" + "|".join(map(lambda x: x.name, formatters)) + "}] [-s speed] [-t title]\n")
                sys.exit(0)
        sys.stdout.write(plotframe(await,formatter,threshold,title))
    except KeyboardInterrupt:
        pass

# The following sets edit modes for GNU EMACS
# Local Variables:
# mode:python
# End: