#! /usr/bin/python3

import os
import sys
import getopt
import csv

from math import *
from gps import *
from time import strftime, sleep

import RPi.GPIO as GPIO

##############################################################################
# sound stuff
##############################################################################

import os
from sys import platform as _platform

sound_args = ""

def play_sound(file):
  if _platform == "win32" or _platform == "win64" or _platform == "cygwin":
    cmd = "wmplayer " + '"' + file + '"'
  else:
    cmd = "aplay -N " + sound_args + " " + file + " &"
  os.system(cmd)

##############################################################################
# buzzer stuff
##############################################################################

# https://www.instructables.com/Raspberry-Pi-Tutorial-How-to-Use-a-Buzzer/

buzzer=23
seconds=3

def buzzer_init():
  GPIO.setwarnings(0)
  GPIO.setmode(GPIO.BCM)
  GPIO.setup(buzzer, GPIO.OUT)

def buzzer_on():
  GPIO.output(buzzer, GPIO.HIGH)
  play_sound("alert.wav")

def buzzer_off():
  GPIO.output(buzzer, GPIO.LOW)

def buzzer_run():
  global seconds
  sec = seconds
  while (sec):
    buzzer_on()
    sleep(0.5)
    buzzer_off()
    sleep(0.5)
    sec = sec - 1

buzzer_init()

##############################################################################
# gps stuff
##############################################################################

# https://ozzmaker.com/using-python-with-a-gps-receiver-on-a-raspberry-pi/

gpsd = gps(mode=WATCH_ENABLE|WATCH_NEWSTYLE) 

#
# GPS receiver is ublox from V2V-MD at 10x seconds
#

# http://www.movable-type.co.uk/scripts/latlong.html

earth_flatening = 1.0/298.257223563
earth_radius = 6378137.0

def haversine(lat1, lon1, lat2, lon2):
  """
  Calculate the great circle distance between two points
  on the earth (specified in decimal degrees)
  """
  # convert decimal degrees to radians
  lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])

  # haversine formula
  dlon = lon2 - lon1
  dlat = lat2 - lat1
  a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
  c = 2 * asin(sqrt(a))

  # 6367 km is the radius of the Earth
  return earth_radius * c

def roydistance(lat1, lon1, lat2, lon2):
  """
  Calculate the great circle distance between two points
  on the earth (specified in decimal degrees)
  """
  # convert decimal degrees to radians
  lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
    
  # Roy's method
  f1 = (1.0 - earth_flatening)

  top = (pow((lon2-lon1), 2) * pow(cos(lat1), 2)) + pow(lat2-lat1, 2)
  bot = pow(sin(lat1), 2) + (pow(f1, 2) * pow(cos(lat1), 2))
    
  dist = f1 * earth_radius * sqrt(top/bot)
    
  return dist

##############################################################################
# virtual cones
##############################################################################

# node from csv (each line is lat, long, distance, next code# (optional)):
# 42.522040, -83.237594, 5, 1

def read_csv(csv_file):
  file_csv = open(csv_file)
  data_csv = csv.reader(file_csv)
  virtual_cones = list(data_csv)
  total_cones = len(virtual_cones)
  print(csv_file, total_cones)
#  print(virtual_cones)
  return virtual_cones

##############################################################################
# main
##############################################################################

def main(argv):
  virtual_list = [[],[],[],[],[],[],[],[],[]]
  virtual_count = 0
  current_cone = [0,0,0,0,0,0,0,0]
  total_cones = [0,0,0,0,0,0,0,0]

  try:
    os.mkdir("logs")
  except:
    pass

  log = open(strftime("logs/way-%Y%m%d-%H%M.log"), 'a')
  log.write("Time,Latitude,Longitude,Heading,Speed,Description\n")

  try:
      opts, args = getopt.getopt(argv,"",["",""])
  except getopt.GetoptError:
      print(sys.argv[0], '<csvfile>')
      sys.exit(2)
#  for opt, arg in opts:
#     nop

  # slurp in files for each list
  for arg in args:
    virtual_cones = read_csv(arg)
    current_cone[virtual_count] = 0
    total_cones[virtual_count] = 0
    virtual_list[virtual_count] = virtual_cones
    virtual_count = virtual_count + 1

#  print(virtual_list)
  buzz_no = 0
  max_buzz = 3

  try:
    while True:
      report = gpsd.next() #
      if report['class'] == 'TPV':
      
        time = getattr(report, 'time', 0.0)
        lat = getattr(report, 'lat', 0.0)
        lon = getattr(report, 'lon', 0.0)
        heading = getattr(report, 'track', 0.0)
        speed = getattr(report, 'speed', 0.0)
        
        log.write(time+","+str(lat)+","+str(lon)+","+str(heading)+","+str(speed))
#        print("Position: %f, %f, %d" % (lat, lon, speed))
      
        # walk through each list
        for i in range(0, virtual_count):
          # end of list?
          if current_cone[i] >= total_cones[i]:
            current_cone[i] = 0
          
          # check first node for reset
          if current_cone[i]:
            # node from csv (each line is lat, long, distance, next code#)
            row = virtual_list[i][0]
            cone_lat = float(row[0])
            cone_lon = float(row[1])
            cone_dist = float(row[2])
            dist = haversine(lat, lon, cone_lat, cone_lon) / 1000.0
            if dist < cone_dist:
              current_cone[i] = 0
          
          # node from csv (each line is lat, long, distance, next code#)
          row = virtual_list[i][current_cone[i]]
          cone_lat = float(row[0])
          cone_lon = float(row[1])
          cone_dist = float(row[2])
            
          # next cone
          cone_next = current_cone[i] + 1
          try:
            cone_next = int(row[3])
          except:
            cone_next = 0
          
          # distance between vehicle and cone
#          dist = haversine(lat, lon, cone_lat, cone_lon)
          dist = roydistance(lat, lon, cone_lat, cone_lon)
                    
          # check distance trigger
          if (dist < cone_dist and buzz_no < max_buzz):
            log.write(".Waypoint="+str(i)+":"+str(current_cone[i]))
            print("Waypoint %d:%d reached: %f, %f, %d" % (i, current_cone[i], lat, lon, speed))
            buzzer_run()
            current_cone[i] = cone_next
            buzz_no = buzz_no + 1
          else:
            buzz_no = 0
        log.write("\n")
    
#    time.sleep(0.2)
 
  except (KeyboardInterrupt, SystemExit): #when you press ctrl+c
    log.close()
    print("Done.\nExiting.")

main(sys.argv[1:])