"""SimpleNMEA"""

import time            # strftime
import e32
import key_codes
import thread
import appuifw         # UI
import socket          # bluetooth
from math import *

def help():
   global going
   appuifw.note(u"Help", 'info')
   going = 0

trip_state = 0
log_state = 0

# Trip info
trip_dist=0

# Log info
log_points = 0
log_file = ""
log_fh = None

#
pos_north = 0
pos_east = 0
pos_altitude = ""
track_dir = 0
track_speed = ""

lastpos_north = 0
lastpos_east = 0

def make_filename():
    proto = "%Y-%m-%d_%H%M.nmea"
    return time.strftime(proto, time.localtime(time.time()))

def start_logging():
    global log_file, log_fh, log_state
    try:
      log_fh = open("c:\\data\\temp\\"+log_file,"a");
      log_state = 1
    except:
      log_state = 0
      appuifw.note(u"File open failed")
      log_fh = None

def stop_logging():
    global log_fh, log_state
    if log_fh:
        log_fh.close()
    log_state = 0

def make_info():
    global listbox, prog_state
    global trip_state, trip_dist, track_speed
    global pos_north, pos_east, pos_altitude
    global log_state, log_file, log_points

    content = []
    content.append( (unicode(prog_state), unicode(pretty_angle(pos_north,"NS"))+u','+unicode(pretty_angle(pos_east,"EW"))+u' '+unicode(pos_altitude)+u"m") )
    if trip_state == 1:
      line1 = u"Trip Started"
    else:
      line1 = u"Trip Stopped"

    # The %.4g modifier doesn't work so this is a workaround
    str = '%.4f' % trip_dist
    str = str[0:3]
    line2 = u"Dist: "+unicode(str)+u"km, Speed: "+unicode(track_speed)+u"km/h"
    content.append( (line1,line2) )

    if log_state == 1:
      line1 = u"Logging Started"
    else:
      line1 = u"Logging Stopped"

    line2 = unicode(log_points)+u" points, "+unicode(log_file);
    content.append( (line1,line2) )

    lb.set_list(content,lb.current())

def action():
    global trip_state, log_state
    index = lb.current()
    if index == 0:
      appuifw.note(u"Distancia Muy Grande.", 'error')
    elif index == 1:
      trip_state = 1-trip_state
    elif index == 2:
      if log_state == 0:
        start_logging()
      else:
        stop_logging()
    make_info()

def exit_key_handler():
    global going, lock
    lock.signal()
    going=0

# [0]=lat, [1]=lon
# Returns km
def haversineDist(p1,p2):
  R = 6371;   # earth's mean radius in km
  p1 = [ p1[0]*pi/180, p1[1]*pi/180 ]
  p2 = [ p2[0]*pi/180, p2[1]*pi/180 ]
  dLat  = p2[0] - p1[0];
  dLong = p2[1] - p1[1];
#
  a = sin(dLat/2) * sin(dLat/2) + cos(p1[0]) * cos(p2[0]) * sin(dLong/2) * sin(dLong/2);
  c = 2 * atan2(sqrt(a), sqrt(1-a));
  d = R * c;
#
  return d;

# Parse from the GPS HHMM.ssss into degress
# The number of H's can vary, which makes it slightly more complex
# dir = [NE] positive, [SW] negative
def parse_angle(ang,dir):
  pos = ang.find(".")
  deg = float(ang[0:pos-2])
  min = float(ang[pos-2:])
#
  ang = deg+(min/60)
  if dir == 'S' or dir == 'W':
    ang = -ang
  return ang

# Takes an angle in degrees and return HH.MM.SS [NSEW]
# dir is array, first arg is positive, second is negative
def pretty_angle(ang, dir):
   ang = ang + 1/7200   # For rounding
   if ang < 0:
     letter = dir[1]
   else:
     letter = dir[0]
   ang = abs(ang)
   deg = floor(ang)
   ang = 60*(ang - deg)
   min = floor(ang)
   ang = 60*(ang - min)
   sec = floor(ang)

   return "%d.%02d'%02d\"%s" % (deg, min, sec, letter)

socket_buffer = ""

def readline(sock):
  global socket_buffer

  pos = socket_buffer.find("\r\n")
  while pos == -1:
    socket_buffer = socket_buffer + sock.recv(1024)
    pos = socket_buffer.find("\r\n")

  res = socket_buffer[0:pos]
  socket_buffer = socket_buffer[pos+2:]
  return res

lb = appuifw.Listbox([(u"Starting",u"Please wait")], action)
appuifw.app.title = u"SimpleNMEA"
appuifw.app.body = lb
appuifw.app.menu = [(u"Help", help),(u"Exit", exit_key_handler)]
appuifw.app.exit_key_handler = exit_key_handler
lock = e32.Ao_lock()

def change_filename():
  global log_file
  newfile = make_filename()
  res = appuifw.query(u"New filename",'text', unicode(newfile))
  if res:
    if log_state:
      # If logging, rename file
      stop_logging()
      os.rename(log_file, newfile)
      log_file=newfile
      start_logging()
      appuifw.note(u"Logfile renamed")
    else:
      log_file = res

lb.bind(key_codes.EKeyStar, change_filename)

going = 0
connected = 0
prog_state = "Starting up"
gps_name = "BT GPS"
while not going:
        try:
             gps_name,services=socket.bt_discover()
             gps_target=(gps_name,services.values()[0])
             going = 1
        except socket.error, inst:
             appuifw.note(u"GPS not found, retrying...")

log_file = make_filename()

def log_data_real(data):
  global log_fh
  log_fh.write(data)

log_callgate = e32.ao_callgate(log_data_real)

def log_data(data):
   global log_callgate
   log_callgate(data)

def gps_read_thread(gps_target):
    global lock, prog_state, connected, going
    global log_state, log_fh, log_points, trip_state
    global trip_dist, track_speed, track_dir
    global pos_east, pos_north, pos_altitude

    redraw=1
    while going:
            if redraw: lock.signal()
            redraw = 0
            # Connect to the GPS, if we're not already connected
            if not connected:
                    redraw = 1
                    try:
                            # Connect to the bluetooth GPS using the serial service
                            sock = socket.socket(socket.AF_BT, socket.SOCK_STREAM)
                            sock.connect(gps_target)
                            connected = 1
                            prog_state = "Connected to GPS..."
#                            appuifw.note(u"Connected to the GPS")
                    except socket.error, inst:
                            connected = 0
                            prog_state = "Connect to GPS failed.  Retrying..."
                            #appuifw.note(u"Could not connected to the GPS. Retrying in 5 seconds...")
                            #time.sleep(5)
                            continue
            # If we are connected to the GPS, read a line from it
            if connected:
                    try:
                            rawdata = readline(sock)
                    except socket.error, inst:
                            # GPS has disconnected, bummer
                            connected = 0
#                            appuifw.note(u"Disconnected from the GPS. Retrying...")
                            prog_state = "Disconnected from GPS"
                            redraw = 1
                            continue

#                    if log_state:
#                            log_data(">["+rawdata+"]\n")
#                            log_points = log_points+1
                    if not rawdata[0] == '$' or not rawdata[-3] == '*':
                            continue
                    data = rawdata[1:-3].split(",")

                    # Ignore satellite stuff
                    if data[0] == 'GPGSV': continue
                    if data[0] == 'GPGSA': continue
                    redraw = 1
                    # Track speed and direction
                    if data[0] == 'GPVTG':
                        if log_state: log_data(rawdata+"\n")
                        track_speed = data[7]
                        track_dir = data[1]
                        continue
                    # Track position
                    if data[0] == 'GPRMC':
                        pos_north = parse_angle(data[3],data[4])
                        pos_east = parse_angle(data[5],data[6])
                        if log_state:
                           log_data(rawdata+"\n")
                           log_points = log_points+1
                        continue
                    if data[0] == 'GPGGA':
                        pos_north = parse_angle(data[2],data[3])
                        pos_east = parse_angle(data[4],data[5])
                        pos_altitude = data[9]
                        if log_state:
                           log_data(rawdata+"\n")
                           log_points = log_points+1
                        if trip_state:
                           dist = haversineDist( [lastpos_north,lastpos_east], [pos_north, pos_east] )
                           # Move in minimum steps of a metre to deal with drift
                           if dist > 0.001:
                               trip_dist = trip_dist + dist
                               lastpos_north, lastpos_east = (pos_north, pos_east)
                        else:
                           lastpos_north, lastpos_east = (pos_north, pos_east)
    # Signal lock before quitting
    lock.signal()

thread.start_new_thread( gps_read_thread, (gps_target,) )

#e32.ao_sleep(10)

#going = 0
while going:
   # Wait for thread to signal action
   lock.wait()
   # Update screen
   make_info()

if log_state:
  log_fh.close()