#!/usr/bin/python
#
# Copyright (c) 2019 Clementine Computing LLC.
#
# This file is part of PopuFare.
#
# PopuFare is free software: you can redistribute it and/or modify
# it under the terms of the GNU Affero General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# PopuFare is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU Affero General Public License for more details.
#
# You should have received a copy of the GNU Affero General Public License
# along with PopuFare.  If not, see <https://www.gnu.org/licenses/>.
#

# Script to test GPS positioning by simulating output of a GPS module.
#
# Outputs to stdout
#

import getopt
import sys
import time
import math

DT = 0.5
DS = 2
DTUPDATE = 2.0
ITER_COUNT = -1
LOCK_DELAY = 0
DSTEP = 16

argv = sys.argv

def show_help(ofp):
  ofp.write("\nusage:  gps-sim.py [-dt dt] [-ds ds] [-p lat0,lon0] [-p lat1,lon1]\n")
  ofp.write("\n")
  ofp.write("  -dt dt                 time between waypoints (default " +  str(DT) + "seconds)\n")
  ofp.write("  -ds ds                 time to stop at waypoint (default" + str(DS) + "seconds)\n")
  ofp.write("  -p lat,lon[,dt[,ds]]   latitudie, longitude of waypoint with optional dt, ds (can specify multiple times)\n")
  ofp.write("  -f fn                  file containing lat,lon positions\n")
  ofp.write("  -t t                   time between print updates (default " + str(DTUPDATE) + " seconds)\n")
  ofp.write("  -n n                   number of iterations (<0 for infinite, default " + str(ITER_COUNT) + ")\n")
  ofp.write("  --lock-delay d         time to delay before inital gps lock (default " + str(LOCK_DELAY) + ")\n")
  ofp.write("\n")

def parse_latlon(s):
  v = s.strip("\n").split(",")
  if len(v) < 2: return []
  if len(v) > 4: return []
  lat = float(v[0])
  lon = float(v[1])
  dt = DT
  ds = DS
  if len(v) > 2:
    dt = float(v[2])
    if len(v) > 3:
      ds = float(v[3])
  return [lat, lon, dt, ds]

def load_latlon_csv(fn):
  latlon = []
  with open(fn) as fp:
    for line in fp:
      v = parse_latlon(line)
      latlon.append(v)

  return latlon

latlon = []
noarg = True
ifn = ""


argvv = []
av = []
for p in range(len(argv[1:])):
  if (p%2) == 0:
    av = [argv[p+1]]
  else:
    av.append(argv[p+1])
    argvv.append(av)
    av = []


for argval in argvv:
  arg = argval[0]
  val = argval[1]
  if arg in ("-h", "--help"):
    show_help(sys.stdout)
    sys.exit(0)
    noarg = False
  elif arg in ("-dt"):
    DT = int(val)
    noarg = False
  elif arg in ("-ds"):
    DS = int(val)
    noarg = False
  elif arg in ("-p"):
    x = parse_latlon(val)
    latlon.append(x)
    noarg = False
  elif arg in ("-f"):
    ifn = val
    noarg = False
  elif arg in ("-n"):
    ITER_COUNT = int(val)
  elif arg in ("-t"):
    DTUPDATE = float(val)
  elif arg in ("--lock-delay"):
    LOCK_DELAY = float(val)

if len(ifn)>0:
  latlon = load_latlon_csv(ifn)

if len(latlon)==0:
  sys.stderr.write("Provide input file or lat/lon points\n")
  show_help(sys.stderr)
  sys.exit(-1)

if noarg:
  show_help(sys.stderr)
  sys.exit(1)

print "# dt,ds,t:", DT, DS, DTUPDATE
print "# latlon:", latlon
print "# iter_count:", ITER_COUNT
print "# ifn:", ifn
print "# lock_delay:", LOCK_DELAY

curiter = 0
if ITER_COUNT<0: curiter = ITER_COUNT-1

lastupdate_sec = 0.0
curidx = 0
nidx = len(latlon)
while curiter < ITER_COUNT:

  curpos = latlon[curidx]
  nxtpos = latlon[ (curidx+1)%nidx ]

  dlat = curpos[1] - curpos[0]
  dlon = curpos[1] - curpos[0]

  dpos = math.sqrt( (dlat*dlat) + (dlon*dlon) )
  for _step in range(DSTEP):
    _p = float(_step) / float(DSTEP)

    _pos = [ curpos[0] + _p*(nxtpos[0] - curpos[0]), curpos[1] + _p*(nxtpos[1] - curpos[1]) ]

    cursec = float(time.time())
    if (cursec - lastupdate_sec) > DTUPDATE:
      lastupdate_sec = cursec
      #print _p, _pos
      sys.stdout.write(str(_pos[0]) + " " + str(_pos[1]) + "\n")
      sys.stdout.flush()

    time.sleep(DS)

  curidx = (curidx + 1)%nidx

  time.sleep(DT)
  if ITER_COUNT>=0: curiter+=1