#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
# This script reads dump1090 input from port, converts it in FLARM specific NMEA sentences and writes these sentences to a local named pipe
# More information about the ADS-B message format can be found under : http://www.homepages.mcb.net/bones/SBS/Article/Barebones42_Socket_Data.htm
#

from math import *
import datetime
import re
import os
from socket import socket, gethostbyname, getaddrinfo, error, \
  AF_INET, SOCK_STREAM, SOCK_DGRAM
from sys import argv, stderr
from time import time

NaN = float('nan')
Hex = ''

TypeADSB       = 1
traffic_data = {}	# dictionary for all traffic

class aircraft:
    def __init__(self):
        self.id = ""                # ICAO Id
        self.latitude = self.longitude = 0.0
        self.altitude = NaN         # Feet

class adsb2udp():
  "SBS client interface to ADS-B."
  def __init__(self, src_host='192.168.178.21', src_port=30003, device='', \
    my_Id='TEST', dst_host='192.168.2.108', dst_port=10110, \
    base_lat=48.715247, base_long=11.533155, base_alt=400.0):
    print 'adsb2udp: src host =' , src_host , ', src_port =' , src_port , \
    ', dst_host =' , dst_host , ', dst_port =' , dst_port , ', base_lat =' , \
    base_lat , ', base_long =' , base_long , ', base_alt =' , base_alt
    self.verbose = 0
    self.linebuffer = ""
    self.response = ""
    self.sock = None        # in case we blow up in connect
    self.pos = aircraft()
    self.device = device
    self.src_host = gethostbyname(src_host)
    self.src_port = src_port
    self.dst_host = gethostbyname(dst_host)
    self.dst_port = dst_port
    self.base_lat = base_lat
    self.base_long = base_long
    self.base_alt = base_alt
    if src_host != None:
        self.connect(src_host, src_port)
    self.d = socket(AF_INET, SOCK_DGRAM)

  def connect(self, host, port):
	"""Connect to a host on a given port."""
	msg = "getaddrinfo returns an empty list"
	self.sock = None
	for res in getaddrinfo(host, port, 0, SOCK_STREAM):
		af, socktype, proto, canonname, sa = res
		try:
			self.sock = socket(af, socktype, proto)
			#if self.debuglevel > 0: print 'connect:', (host, port)
			self.sock.connect(sa)
		except error, msg:
			#if self.debuglevel > 0: print 'connect fail:', (host, port)
			self.close()
			continue
		break
	if not self.sock:
		raise error, msg
		
	FIFO_PATH = '/tmp/my_fifo'
	if os.path.exists(FIFO_PATH):
		os.unlink(FIFO_PATH)

	if not os.path.exists(FIFO_PATH):
		os.mkfifo(FIFO_PATH)
    	self.my_fifo = open(FIFO_PATH, 'w+')
    	print "my_fifo:",self.my_fifo	

  def close(self):
      if self.sock:
          self.sock.close()
      self.sock = None

  def __del__(self):
      self.close()

  def read(self):
      "Wait for and read data being streamed from the daemon."
      if self.verbose > 1:
          stderr.write("poll: reading from daemon...\n")
      eol = self.linebuffer.find('\n')
      if eol == -1:
          
	  frag = self.sock.recv(4096)
          self.linebuffer += frag
          if self.verbose > 1:
              stderr.write("poll: read complete.\n")
          if not self.linebuffer:
              if self.verbose > 1:
                  stderr.write("poll: returning -1.\n")
              # Read failed
              return -1
          eol = self.linebuffer.find('\n')
          if eol == -1:
              if self.verbose > 1:
                  stderr.write("poll: returning 0.\n")
              # Read succeeded, but only got a fragment
              return 0
      else:
          if self.verbose > 1:
              stderr.write("poll: fetching from buffer.\n")

      # We got a line
      eol += 1
      self.response = self.linebuffer[:eol]
      self.linebuffer = self.linebuffer[eol:]

      # Can happen if daemon terminates while we're reading.
      if not self.response:
          return -1
      if self.verbose:
          stderr.write("poll: data is %s\n" % repr(self.response))
      self.received = time()
      # We got a \n-terminated line
      return len(self.response)

  def unpack(self, buf):
      global Hex
      error = 0
      Hex = ''
      fields = buf.strip().split(",")
      # print fields
      # ============== MSG 3 =========================
      if fields[0] == "MSG" and fields[1] == "3":
        # print fields
	self.pos.id = fields[4]
        self.latitude = self.longitude = 0.0
        self.altitude = NaN         # Feet
        if fields[11] != '':
          self.pos.altitude = float(fields[11])
        if fields[14] != '':
          self.pos.latitude = float(fields[14])
        if fields[15] != '':
          self.pos.longitude = float(fields[15])
	if (traffic_data.has_key((fields[4],'CS'))):
	  try:
	     traffic_data[fields[4],'Lat'] = float(fields[14])
	  except ValueError:
	     print "Conversion Error Lat: " + fields[14]
	     traffic_data[fields[4],'Lat'] = float(0)
	     error += 1
	  
	  try:
	     traffic_data[fields[4],'Lon'] = float(fields[15])
	  except:
	     print "Conversion Error Lon: " + fields[15]
	     traffic_data[fields[4],'Lon'] = float(0)
	     error += 1
	     
	  try:
	     traffic_data[fields[4],'Alt'] = float(fields[11])
	  except ValueError:
	     print "Conversion Error Alt: " + fields[11]
	     traffic_data[fields[4],'Alt'] = float(0)
	     error += 1
	     
	  traffic_data[fields[4],'Tim'] = datetime.datetime(int(fields[6].split('/')[0]),int(fields[6].split('/')[1]),int(fields[6].split('/')[2]),int(fields[7].split(":")[0]),int(fields[7].split(":")[1]),int(fields[7].split(":")[2].split(".")[0]),int(fields[7].split(":")[2].split(".")[1])*1000)
	  
	  if (traffic_data.has_key((fields[4],'Trk')) and error == 0):
	     # Setting Hex triggers NMEA data output
	     Hex = fields[4]
	     # print traffic_data.keys()
	     # print datetime.datetime.now(),"now"
	     for i,j in traffic_data:
	        if j == "Tim":
		# if j == "Tim":
		   # Sync issue between CPU time and GPS time from ADSB input may generate neg. time values
		   # print traffic_data[i,j],(datetime.datetime.now() - traffic_data[i,j]).seconds
		   if ((datetime.datetime.now() - traffic_data[i,j]).seconds < 80000) and (((datetime.datetime.now() - traffic_data[i,j]).seconds) >= 60):
		      if (traffic_data.has_key((i,'Tim'))):
		         del traffic_data[i,'Tim']
		      if (traffic_data.has_key((i,'GS'))):
		         del traffic_data[i,'GS']
		      if (traffic_data.has_key((i,'Trk'))):
		         del traffic_data[i,'Trk']
		      if (traffic_data.has_key((i,'Lat'))):
		         del traffic_data[i,'Lat']
		      if (traffic_data.has_key((i,'Lon'))):
		         del traffic_data[i,'Lon']
		      if (traffic_data.has_key((i,'Alt'))):
		         del traffic_data[i,'Alt']
		      if (traffic_data.has_key((i,'CS'))):
		         del traffic_data[i,'CS']
		      if (traffic_data.has_key((i,'VR'))):
		         del traffic_data[i,'VR']
		      # print "KILLER"
		      break # breaks i or j loop or both???
	     # print "====="
	     # print CS
	     # print traffic_data
	     # print (traffic_data[Hex,'Tim'] - datetime.datetime.now()).seconds
	     # print traffic_data[Hex,'Tim']
	  

      # ============== MSG 1 =========================
      if fields[0] == "MSG" and fields[1] == "1":
        # Message 1: MT,TT,SID,AID,Hex,FID,DMG,TMG,DML,TML,CS
	# print "HexID: " + fields[4]
	# print "Callsign: " + fields[10]
	if not (traffic_data.has_key((fields[4],'CS'))):
	  # Callsign not known yet ---> store in dict
	  traffic_data[fields[4],'CS'] = fields[10]
	  traffic_data[fields[4],'Tim'] = datetime.datetime(int(fields[6].split('/')[0]),int(fields[6].split('/')[1]),int(fields[6].split('/')[2]),int(fields[7].split(":")[0]),int(fields[7].split(":")[1]),int(fields[7].split(":")[2].split(".")[0]),int(fields[7].split(":")[2].split(".")[1])*1000)

      # ============== MSG 4 =========================
      if fields[0] == "MSG" and fields[1] == "4":
        # Message 4: MT,TT,SID,AID,Hex,FID,DMG,TMG,DML,TML,,,GS,Trk,,,VR
	if (traffic_data.has_key((fields[4],'CS'))):
	  # Callsign known, we can store data to it
	  try:
	     # Sometimes GS returns non floats
	     traffic_data[fields[4],'GS'] = float(fields[12])*0.514444
	  except ValueError:
	     # TODO: Check if GS field is already defined and keep old value?
	     print "GS conversion failure: " + fields[12]
	     traffic_data[fields[4],'GS'] = float(999)
	     
	  try:
	     traffic_data[fields[4],'Trk'] = int(fields[13])
	  except ValueError:
	     print "Trk conversion failure: " + fields[13]
	     traffic_data[fields[4],'Trk'] = int(180)
	  
	  try:
	     traffic_data[fields[4],'VR'] = float(fields[16])/3.28084/60
	  except ValueError:
	     print "Vertical rate conversion failure" + fields[16]
	     traffic_data[fields[4],'VR'] = float(0)
	     
	  traffic_data[fields[4],'Tim'] = datetime.datetime(int(fields[6].split('/')[0]),int(fields[6].split('/')[1]),int(fields[6].split('/')[2]),int(fields[7].split(":")[0]),int(fields[7].split(":")[1]),int(fields[7].split(":")[2].split(".")[0]),int(fields[7].split(":")[2].split(".")[1])*1000)

	        
  def checksum(self, sentence):
  
      """ Remove leading $ """
      sentence = sentence.lstrip('$')
  
      nmeadata,cksum = re.split('\*', sentence)
      #print nmeadata
      calc_cksum = 0
      for s in nmeadata:
          calc_cksum ^= ord(s)
  
      return calc_cksum


  def process(self):
    "Process one incoming ADS-B message into one outgoing UDP packet."

    try:
      while True:
  
        self.read()

        # stderr.write("poll: data is %s\n" % repr(self.response))
        if self.response.startswith("MSG,1") :
	    # print self.response
	    self.unpack(self.response)
	    
        if self.response.startswith("MSG,4") :
	    self.unpack(self.response)
	    
        if self.response.startswith("MSG,2") :
	    # MSG 2 triggered by ground switch - we don't process it here
	    print 'Alt, GS, Trk, Lat, Lng, GND'
	    print self.response
	    
        if self.response.startswith("MSG,3") :
            self.unpack(self.response)
	    # Only treat MSG3, if CS is already known
	    if Hex != '':
	      #print '----------------------------------------'
              #print ' ADS-B reading'
              #print '----------------------------------------'
              #print 'id          ' , self.pos.id
              #print 'latitude    ' , self.pos.latitude
              #print 'longitude   ' , self.pos.longitude
              #print 'altitude    ' , self.pos.altitude

              buf = "%s %s %f %f %.1f %.1f %.1f %u" % ( 0, self.pos.id, \
                self.pos.latitude, self.pos.longitude, self.pos.altitude / 3.2808, \
                0, 0 , TypeADSB )
         
              lon1, lat1, lon2, lat2 = map(radians, [self.base_long, self.base_lat, self.pos.longitude, self.pos.latitude])
         
              dlon = lon2 - lon1
              dlat = lat2 - lat1
              a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
              c = 2 * atan2(sqrt(a), sqrt(1-a))
              Distance = 6371000 * c

              Bearing = atan2(sin(lon2-lon1)*cos(lat2), cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1))             
              relBearing = degrees(Bearing)
              absBearing = (relBearing + 360) % 360
              #stderr.write("Bearing %f ° " % Bearing)
              #stderr.write("Distance %f km \n" % Distance)
         
              relNorth = cos(radians(absBearing))*Distance
              relEast = sin(radians(absBearing))*Distance
              relVert = self.pos.altitude/ 3.2808 - self.base_alt
         
              #tip: it is necessary to supply both $PFLAU and $PFLAA Flarm(R) NMEA messages to XCSoar, otherwise it does not not show the traffic.
              #PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,<IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType>
              #PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>,<RelativeVertical>,<RelativeDistance>(,<ID>)
            
              str1 = "$PFLAU,,,,,0,%d,0,%d,%u,%s*" % \
              ( relBearing, int(relVert), int(Distance), traffic_data[Hex,'CS'].replace(" ","") )
              csum = self.checksum(str1)
              str1 += "%02x\r\n" % csum
              # if traffic_data[Hex,'CS'].replace(" ","") == "DLH6HF":
	      # print str1
	      # stderr.write(str1)
              # XCSoar ignores warning data from PFLAU
              # $PFLAA,0,1671,186,410,2,DDAA30,252,,21,12.4,1*62
	      # print traffic_data
	      # $PFLAA,0,24,-5,3,1,123456,0,,0,0.1,1*36
              # $PFLAA,0,16,2,2,1,123457,0,,0,0.1,1*6A
              # $PFLAA,0,155,-1069,418,1,ABCDEF,221,,37,2.8,1*0B
              # print '$PFLAA,0,24,-5,3,1,123456,0,,0,0.1,1*36'

	      
	      str2 = "$PFLAA,0,%d,%d,%d,1,%s,%i,,%.1f,%.1f,8*" % \
              ( int(relNorth), int(relEast), int(relVert), traffic_data[Hex,'CS'].replace(" ",""),int(traffic_data[Hex,'Trk']),traffic_data[Hex,'GS'],traffic_data[Hex,'VR'])
              csum = self.checksum(str2)
              str2 += "%02x\r\n" % csum
              
	      # if traffic_data[Hex,'CS'].replace(" ","") == "DLH6HF":
              # stderr.write(str2)
            
              if (self.base_lat < 0) :
                  Latstring = "S"
              else :
                  Latstring = "N"
              if (self.base_long < 0) :
                  Longstring = "W"
              else :
                  Longstring = "E"
              latdeg = int(abs(floor(self.base_lat)))
              latmin = abs(60*(self.base_lat-latdeg))
              longdeg = int(abs(floor(self.base_long)))
              longmin = abs(60*(self.base_long-longdeg))
              #--GLL Latitude,N/S,Longitude,E/W,UTC,Status (A=Valid, V=Invalid),Checksum
              #GPRMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68
              now = datetime.datetime.utcnow()
              str3 = "$GPRMC,%02i%02i%02i,A,%s%.2f,%s,%s%.2f,%s,,,%02i%02i%02i,001.0,W*" % \
              (now.hour, now.minute, now.second, latdeg, latmin, Latstring, longdeg, longmin, Longstring, now.day, now.month, now.year )
              csum = self.checksum(str3)
              str3 += "%02x\r\n" % csum
            
	      #--- GPGGA sentences - required for flarmradar.ch
	      # $GPGGA,173431.00,4934.90002,N,00924.13332,E,7,5,0.96,415.1,M,48.0,M,,*60
	      str4 = "$GPGGA,%02i%02i%02i.00,%02d%.5f,%s,%03d%.5f,%s,7,5,0.96,415.1,M,48.0,M,,*" % \
	      (now.hour, now.minute, now.second, latdeg, latmin, Latstring, longdeg, longmin, Longstring)
              csum = self.checksum(str4)
              str4 += "%02x\r\n" % csum
	      # print str4
	    
              buf = str1 + str2 + str3 + str4
              # stderr.write(buf)
              # Write NMEA sentences to pipe for IPC to adsbclient.pl (Flarmradar)
	      self.my_fifo.write(buf)
            	
           
              # self.d.sendto(buf, (self.dst_host, self.dst_port))
  
    except KeyboardInterrupt:
      # Avoid garble on ^C
      print ""

if __name__ == '__main__':

  opts = { }

  argc = len(argv)
  if argc > 1:
    opts["src_host"]    = argv[1]
  
  if argc > 2:
    opts["src_port"]    = int(argv[2])

  if argc > 3:
    opts["dst_host"]    = argv[3]
  
  if argc > 4:
    opts["dst_port"]    = int(argv[4])
  
  session = adsb2udp(**opts)
  session.process()

# driver.py ends here