#!/usr/bin/python
# hacked together by Randall Munroe
# 2008-05-17

# This is a script for reading aloud cardinal directions
# toward a location using a USB/bluetooth GPS.

# to use it on Debian/Ubuntu, install the following packages:
# gpsd (should be installed)
# gpsd-clients
# figlet
# flite

# by typing
#    sudo apt-get install gpsd gpsd-clients figlet flite

# start gpsd with
#    sudo gpsd /dev/ttyUSB0

# replacing ttyUSB0 with whatever your GPS is plugged into
# (you can find out via dmesg).  ttyUSB0 is a good guess.

# Then just run it with
#    gpspipe -w | ./cyborg.py <lat> <long>

# it takes either 31.415 -92.653 format, or
# decimal degrees like 31 24.9 -92 39.18

# Here are a few settings:

# Say the o'clock direction every N seconds, when available
direvery=10

# Say the distance to target and ETA every N seconds, when available
distevery=15

# speak every time direction to target changes?
speakdirchange=True


# number of seconds between successive readings used
# to determine speed.  5 is good for walking, 3 or 4 if driving.
lagseconds=5

# number of seconds ago to use forlonger-term average speed reading
oldpasttime=150

import math


#contains [y,x]
def angle(vector):
    return (math.degrees(math.atan2(vector[0],vector[1])))%360

#kilometers
def ll_vector(c1, c2):
    yd=c2[0]-c1[0]
    xd=c2[1]-c1[1]

    yd *= 111.0;
    xd *= 111.0;

    xd *= abs(math.cos((c1[0]/360)*2*math.pi))

    return [yd,xd]

def distance(c1,c2):
    vector=ll_vector(c1,c2)
    return math.hypot(vector[0],vector[1])

def direction(c1,c2):
    vector=ll_vector(c1,c2)
    return angle(vector)

def d2words(angle):
    angle+=22.5
    angle%=360
    angle=int(angle/45)
    angle%=8
    words=["East","North-East","North","North-West","West","South-West","South","South-East"]
    return words[angle]

def stringnum(n):
    fl=int(n*100)
    return str(int(n))+"."+str(fl%100)

def directme(me, targ):
    print "Go", stringnum(distance(me, targ)), "kilometers", d2words(direction(me, targ))

def mps2mph(n):
    return 2.23693629*n

def getspeed(c1,c2,seconds):
    if seconds==0:
        return 0
    speed=distance(c1,c2)*1000/seconds
    return speed
    
def speedmph(c1,c2,seconds):
    speed=distance(c1,c2)*1000/seconds
    speed=mps2mph(speed)
    return speed

# format here is [time y x]
# return format is [time, [y, x], speed]
def speedarray(readings):
    speeds=[]
    for i in range(len(readings)-1):
        sp=getspeed(readings[i][1:3], readings[i+1][1:3], readings[i+1][0]-readings[i][0])
        vec=ll_vector(readings[i][1:3], readings[i+1][1:3])
        speeds.append([readings[i][0], vec, sp])
    return speeds

# format here is [time y x]
def lastnseconds(coords, numseconds):
    maxtime=0
    for i in coords:
        if i[0]>maxtime:
            maxtime=i[0]

    maxtime-=numseconds

    validcoords=[]

    for i in coords:
        if i[0]>=maxtime:
            validcoords.append(i)
    return validcoords

# format here is [time y x]
def condense(coords):
    time=coords[0][0]
    condensed=[]
    thesecoords=[time, 0, 0]
    n=0;
    for i in xrange(len(coords)):
        if coords[i][0]==time:
            thesecoords[0]=time
            thesecoords[1]+=coords[i][1]
            thesecoords[2]+=coords[i][2]
            n+=1
            if i==len(coords)-1:
                thesecoords[1]/=n
                thesecoords[2]/=n
                condensed.append([time, thesecoords[1], thesecoords[2]])
                return condensed
            
        else:
            thesecoords[1]/=n
            thesecoords[2]/=n
            n=0
            condensed.append([time, thesecoords[1], thesecoords[2]])
            thesecoords=[0, 0,0]
            if i==(len(coords)-1):
                return condensed
            else:
                time=coords[i+1][0]
    

def dumpbad(speedarray):
    if len(speedarray)<5:
        return speedarray
    speeds=[]
    for i in speedarray:
        speeds.append(i[2])
    speeds.sort()
    #cut off start and end quartiles
    speeds=speeds[int(len(speeds)*0.25):int(len(speeds)*0.75)]
#    print speeds
    minspeed=speeds[0]
    maxspeed=speeds[-1]
#    print minspeed
#    print maxspeed
    goodspeeds=[]

    for i in speedarray:
        if i[2]>=minspeed and i[2]<=maxspeed:
            goodspeeds.append(i)
    return goodspeeds

# now, with all that work, we just take a cheap guess
def currentspeed(speedarray):
    return speedarray[-1][2]


def getcoords(path):

    coords=[]
    inf=open(path, 'r')
    while(inf):
        string=inf.readline()
        string=string.strip()
        if not string:
            break
        string=string.split(' ')
        linelist=map(float, string)
        coords.append(linelist)
    return coords

def speedsfromfile(path):
    valids=lastnseconds(condense(getcoords(path)), 10)
    return dumpbad(speedarray(valids))

def currentdir(speeds):
    return direction(speeds[0][1], speeds[-1][1])

# NEW STUFF ADDED AFTER LOOP CODE

#takes history in [time, [y, x]] format
def mostrecentold(history, seconds):
    if len(history)<2:
        print "too-small list passed to mostrecentold"
        return
#    if history[0][0]>current[0]-seconds:
    # if no valid history entry, returns earliest
    best=history[0]
    current=history[-1]
    for reading in history:
        if reading[0]>best[0] and reading[0]<=(current[0]-seconds):
            best=reading
    return best

#remove all entries farther back than current-time
def loganize(history, time):
    i=0
    while(i<len(history)):
        if history[i][0] < (history[-1][0]-time):
            history=history[:i]+history[i+1:]
        else:
            i+=1
    return history

def getoffset(heading, angle):
    return ((angle+180)-heading)%360-180;

def offset2clock(angle):
    angle=12-int(((angle+15)%360)/30)
    return angle


# Main program starts here

import sys
import os
import retry
import time
import re

targ=[0,0]

delimeters=re.compile("[, ]")

#this whole section just gets the lat/long args in various formats

if len(sys.argv)==2:
    argstr=sys.argv[1]
    splitted=argstr.split(',')
    for i in range(len(splitted)):
        splitted[i]=re.sub(delimeters, "", splitted[i])

    nums=map(float, splitted)
    if len(nums)==2:
        targ[0]=nums[0]
        targ[1]=nums[1]

if len(sys.argv)>2:
    
    targ[0]=sys.argv[1]
    targ[1]=sys.argv[2]

    for i in range(len(targ)):
        targ[i]=re.sub(delimeters, "", targ[i])
        
    targ=map(float, targ)

if len(sys.argv)>4:
    args=[sys.argv[1], sys.argv[2], sys.argv[3], sys.argv[4]]

    for i in range(len(args)):
        args[i]=re.sub(delimeters, "", args[i])
    args=map(float, args)

    targ=[args[0]+args[1]/60.0, args[2]]
    if targ[1]<0:
        targ[1]-=abs(args[3])/60.0
    else:
        targ[1]+=abs(args[3])/60.0


#that was much longer than necesary.

print "Run this with gpspipe -w | python cyborg.py 37.355451,-121.959123"
print "Target:", targ[0], targ[1]

history=[]
# history format: [time, [y, x]]



calibrated_lag=0
lastdist=0
lastspoke=0
lastdistancespoke=100000000
lastclockspoke=0


print "Waiting for GPS data ..."
while(1):
#    print "getting new line"
    line=sys.stdin.readline()
    if not line:
        exit()
#    print "got new line"

    line=line.strip()
    line=line.split(" ")

# This is what a GPSD line referring to a location looks like.
# It's possible your mileage may vary.
    if not line[0] == "GPSD,O=GGA":
        continue


    readtime=float(line[1])
    y=float(line[3])
    x=float(line[4])

    history.append([readtime, [y, x]])
    history=loganize(history,oldpasttime*4) # kill old data points

    if len(history)<3:
#calibrate lag
        calibrated_lag=time.time()-history[-1][0]
        print "Getting enough data for tracking ..."
        print "Calibrating lag to", calibrated_lag
        continue
    
#    speed estimate:
#    print history
    current=history[-1]
    past=mostrecentold(history, lagseconds)
    oldpast=mostrecentold(history, oldpasttime)



    olddist=distance(targ, oldpast[1])
    newdist=distance(targ, current[1])
    seconds=current[0]-oldpast[0]
    
    vec=ll_vector(past[1], current[1])

    speed=getspeed(past[1], current[1], current[0]-past[0])
    avespeed=getspeed(oldpast[1], current[1], current[0]-oldpast[0])

    heading=angle(vec)

    targvec=ll_vector(current[1], targ)
    targheading=angle(targvec)
    targdist=distance(current[1], targ)

    print avespeed
    print "skipping"
    if avespeed<1:
        etastring=""
        eta=-1
    else:
        crowfliesspeed=max(speed, avespeed)
        crowfliestime=newdist/crowfliesspeed
        print "distance =", newdist
        print "cfs =", crowfliesspeed
        olddist=distance(oldpast[1], targ)
        approachingspeed=(olddist-newdist)*1000/(current[0]-oldpast[0])
        print "approachingspeed", approachingspeed
        speedguess=(max(0,approachingspeed)+2*crowfliesspeed)/3
        print "speedguess", speedguess
        eta=newdist*1000.0/speedguess

        etaprepend="eetee eigh"
        if eta<60:
            etaappend=" seconds"
            if int(eta)==1:
                etaappend=" second"
            etastring=etaprepend+str(int(eta))+etaappend
        elif eta<3600:
            etaappend=" minutes"
            if int(eta/60)==1:
                etaappend=" minute"
            etastring=etaprepend+str(int(eta/60))+etaappend
        else:
            etainpend=" hours"
            if int(eta/3600)==1:
                etainpend=" hour"
            etaappend=" minutes"
            if int(eta/60)%60==1:
                etaappend=" minute"
            etastring=etaprepend+str(int(eta/3600))+etainpend+str(int(eta/60)%60)+etaappend
    

    # talking section

    os.system("clear")

#    print speed
#    print avespeed
    print
    print " Currently going", d2words(heading),"("+str(offset2clock(heading-90))+"o'clock)", "at", mps2mph(speed), "mph"

    offset=getoffset(heading, targheading)
    print " Target is", targdist, "km", d2words(targheading), "("+str(offset2clock(targheading))+" o'clock)"
    print " "+str(offset2clock(offset))+" O'Clock"
    dirwords=str(int(targdist*1000))+" m"
    dirwords2=str(offset2clock(offset))
    command="figlet -m0 "+dirwords+";figlet -m0 "+dirwords2+"  o\`clock"
    os.system(command)

    sayclock="echo \""+dirwords2+" oclock"+"\" | flite"

    if targdist>1:
        saydist="echo \""+str(int(targdist))+"."+str(int(targdist*10)%10)+" kilometers\" | flite"
    else:
        saydist="echo \""+str(int(targdist*1000))+" meters\" | flite"


#    print "Lag:", (time.time()-current[0])-calibrated_lag
#    say nothing if we've been busy speaking and are behind by 2 seconds
#    print "speed", speed
#    print "avespeed", avespeed
    if time.time() - current[0]>(calibrated_lag+2):
        print "Skipping speech, need to catch up..."
        continue




#    print "Deciding whether to say the distance ..."
    if (current[0]-distevery > lastdist):
        os.system(saydist)

        if speed>1 and avespeed>0.5:
            sayeta="echo \""+etastring+"\" | flite"
            os.system(sayeta)
            lastdist=current[0]
            lastdistancespoke=targdist*1000
            continue
        else:
            saydir="echo \"Target direction "+d2words(targheading)+"\" | flite"
            lastdist=current[0]
            os.system(saydir)
            continue

#    print "evaluating lastclockspoke"
     
    if ((current[0]-direvery > lastspoke or (lastclockspoke != offset2clock(offset))) and speed>1):
        lastclockspoke=offset2clock(offset)
        lastspoke=current[0]
        os.system(sayclock)