#!/usr/bin/env python

import matplotlib
# Allow running headless from the command line
matplotlib.use("agg")

from numpy import *
from pylab import *
import mutils as m
import os;
import sys;
import argparse

maxAxis = 0


#######################################################
# Start of script
#######################################################
if __name__ == "__main__":

  ######################################################################
  # Parse arguments
  parser = argparse.ArgumentParser(description='Parse positions from a T02/T04')
  parser.add_argument('name', help='String we\'ll use in the PNG files')
  parser.add_argument('-f','--receiver', help='T02/T04 filename from the receiver (assumes 35:2/27) unless --record_35_16 set')
  parser.add_argument('-n','--native', help='T02/T04 filename for Native Android file (position in 35:16)')
  parser.add_argument('-b','--blade', help='T02/T04 filename for Blade file')
  parser.add_argument('-u','--hud', help='Titan Player RTK Hud file')
  parser.add_argument('-w','--winastra', help='WinAstra POS file (with no header)')
  parser.add_argument('-p','--pospac', help='POSPac filename to use as truth position')
  parser.add_argument('-t','--truth_lla', help='Truth lat,lon,hgt, e.g. -t 37.3848990528,-122.0054260639,-6.260')
  parser.add_argument('-r','--record35_16', help='Analyze the record 35:16 instead of the 29 (T02 files) or 35:2 (T04)', action="store_true")
  parser.add_argument('-d','--dec', help='Decimate to 1Hz', action="store_true")
  args = parser.parse_args()
  ######################################################################

  SessionName = args.name
  
  engines = ['Native Android','Stinger','Blade','Titan','WinAstra']
  
  print("Dir: %s" % os.getcwd())

  #if(args.dec):
  #  decFlags = '--dec=' + str(args.dec)


  Last = 5 # Max potential position solutions
  for i in range(0,Last):
    if(i==0):   # Native Android file
      if(str(args.native) == 'None'):
        continue
      filename = args.native
      print("Native Android File %s" % filename)
    elif(i==1): # Receiver File
      if(str(args.receiver) == 'None'):
        continue
      filename_str = args.receiver
      tokens = filename_str.split(",")
      if(len(tokens) == 2):
        engines[1] = tokens[1]
      filename = tokens[0]
      print("Receiver File %s" % filename)
    elif(i==2): # Blade File
      if(str(args.blade) == 'None'):
        continue
      filename_str = args.blade
      tokens = filename_str.split(",")
      if(len(tokens) == 2):
        engines[2] = tokens[1]
      filename = tokens[0]
      print("Blade File %s" % filename)
    elif(i==3):       # Titan Player HUD file
      if(str(args.hud) == 'None'):
        continue
      filename = args.hud
      print("HUD File %s" % filename)
    else:       # WinAstra POS file
      if(str(args.winastra) == 'None'):
        continue
      filename = args.winastra
      print("WinAstra File %s" % filename)


    # ToDo: update viewdat so we can automatically convert record 29's to 35:2's, then
    # all we will need is something like the following
    #(ref,k)=m.vd2arr(filename,rec='-d35:2',opt=['--dec=1000 --translate_rec29_to_rec35_sub2'])
    name, file_extension = os.path.splitext(filename)
    if(file_extension == ".T02" or file_extension == ".t02"):
      os.system("t0x2t0x " + filename + " " + name + ".t04")
      filename = name + ".t04"
  
    if(i==3):
      print("Processing HUD file")
      header, data = m.load_hud(filename)
    
      Time = data[:,header['GPS_Sec']];
    
      Lat = data[:,header['Pos_lat']];
      Lon = data[:,header['Pos_lon']];
      Hgt = data[:,header['Pos_hgt']];
      LLH = array([Lat, Lon, Hgt]).T
      # Hud file
    elif(i==4):
      print("WinAstra file")
      data = loadtxt(filename)
    
      Time = data[:,0]
      Lat  = data[:,5]
      Lon  = data[:,6]
      Hgt  = data[:,7]
      LLH = array([Lat, Lon, Hgt]).T
    else:
      if(i==0 or ( (i == 1) and (args.record35_16))):
        # Native Android or T04 with a request to use 35:16
        print("Extracting 35:16")
        (ref,k)=m.vd2arr(filename,rec='-d35:16')
      else:
        print("Extracting 35:2")
        (ref,k)=m.vd2arr(filename,rec='-d35:2')

      Time    = ref[:,k.TIME]
      used    = ref[:,k.NUSED]
      tracked = ref[:,k.NTRK]
      GPSUsd   = ref[:,k.GPSUsd]
      SBASUsd  = ref[:,k.SBASUsd]
      GLOUsd   = ref[:,k.GLOUsd]
      GALUsd   = ref[:,k.GALUsd]
      QZSSUsd  = ref[:,k.QZSSUsd]
      BDSUsd   = ref[:,k.BDSUsd]
      IRNSSUsd = ref[:,k.IRNSSUsd]

      LLH = ref[:,k.LAT:k.LAT+3]

    if(args.dec):
      start = int(Time[0])
      last  = int(Time[-1])
      decTime = array(range(start,last))
      t, i_i, i_ref = m.intersect(decTime, Time)
      Time = Time[i_ref]
      LLH = LLH[i_ref,:]

    if(args.pospac):
      dPP_TIME=0
      dPP_LAT=1
      pospac = m.doload(args.pospac)
      t, i_pp, i_ref = m.intersect( pospac[:,dPP_TIME], Time )
      LLH_Ref = pospac[i_pp,dPP_LAT:dPP_LAT+3]
      LLH     = LLH[i_ref,:]
      TimeLLH = Time[i_ref]
      #LLH     = ref[i_ref,k.LAT:k.LAT+3]
      #TimeLLH = ref[i_ref,k.TIME]
    elif args.truth_lla:
      lla_str = args.truth_lla
      LLH_Ref = array([float(x) for x in lla_str.split(',')])
      TimeLLH = Time
      print(LLH_Ref)
    else:
      LLH_Ref = array([mean(LLH[:,0]), mean(LLH[:,1]), mean(LLH[:,2])])
      TimeLLH = Time
      print(LLH_Ref)

    print("Num Epochs %d" % len(TimeLLH))

    ######################################################################
    # Now plot a few things...
    ######################################################################

    colArray = ['b','r','g','m','c','k','#FF8C00','#FF99FF','#COCOCO','#CCFFCC','#FFCCCC'];

    (dE, dN, dU) = m.llh2enu( LLH, LLH_Ref, False, False)

    ######################################################################
    # Output a few statistics
    ######################################################################
  
    print("East   Sigma = %.3fm" % std(dE))
    print("North  Sigma = %.3fm" % std(dN))
    print("Height Sigma = %.3fm" % std(dU))
  
    err_2d = sqrt( dE**2 + dN**2 )
    cx_2d,cy_2d = m.docdf( err_2d )
    i_50_2d = find( cy_2d >= .5 )[0]
    i_68_2d = find( cy_2d >= .68 )[0]
    i_90_2d = find( cy_2d >= .9 )[0]
    i_95_2d = find( cy_2d >= .95 )[0]
    print('Error:       50%    68%    90%    95%   100%')
    print('2D      : %6.2f %6.2f %6.2f %6.2f %6.2f' % (cx_2d[i_50_2d],cx_2d[i_68_2d],cx_2d[i_90_2d],cx_2d[i_95_2d],cx_2d[-1]))

    if(cx_2d[i_95_2d] > maxAxis):
      maxAxis = cx_2d[i_95_2d]

    ######################################################################
    # End of statistics
    ######################################################################


    # Plot the 2D CDF
    figure(1)
    plot( cx_2d, cy_2d, label='%s%c68%%=%.3fm%c95%%=%.3fm'%(engines[i],10,cx_2d[i_68_2d],10,cx_2d[i_95_2d]),   color=colArray[i])
    figure(2)
    plot(TimeLLH,err_2d,label=engines[i],color=colArray[i])

  ########################################################
  # We've processed all the data - complete the plots
  ########################################################
  if(maxAxis < 0.9):
    Lim2D  = ceil(maxAxis*10)/10
  else:
    Lim2D  = ceil(maxAxis)

  figure(1)
  xlim([0,Lim2D])
  xlabel('Error [m]')
  ylabel('CDF')
  if args.truth_lla:
    title('2D Error - Relative to Static Truth');
  elif(args.pospac):
    title('2D Error - Relative to POSPac Truth');
  else:
    title('2D Error - Relative to Average');
  grid(True)

  legend(loc='best')
  tight_layout()
  show()
  # Save the data as a PNG file
  savefig("%s-2DCDF.png" % SessionName,dpi=150)
  close()

  # Plot the Horizontal Error
  figure(2)
  ylim([0,Lim2D])
  xlabel('Time [GPS Seconds]')
  ylabel('2D Error [m]')
  grid(True)

  legend(loc='best')
  tight_layout()
  show()
  # Save the data as a PNG file
  savefig("%s-2DErr.png" % SessionName,dpi=150)
  close()