#!/usr/bin/env python3
####
# Simple Script to create imagelist.mb-2
#
# 09/22/18  GT   Created

import sys, getopt, os, re
import glob, csv
from datetime import datetime
from collections import namedtuple
from PIL import Image

shortOpts = "hbd:f:o:g:x"
longOpts = ["help", "check-bayer", "imdir=", "imformat=", "outfile=", "gpsfile=", "nogpsfile"]

def usage():
  pname, sname = os.path.split(sys.argv[0])
  sys.stderr.write("usage: % s %s < path > \n" % (sname, str(longOpts)))
  print( """
  -h --help                  print this message
  -b --check-bayer           Read camera settings from original bayer images rather than RGB images
  -d --imdir=path2images     Path to image files (default './')
  -f --imformat=image_format Image format [image_format] [default 'tif']
  -o --outfile=outfname      Output data to [ofname] [default 'imagelist']
  -g --gpsfile=gpsfname      GPS time data from [gpsfname] [default './../logfile_gpstime_cam_sync.csv']
  -x --nogpsfile             Avoid using GPS tigger times (just image time matching)
  """ )
  sys.exit()

def utime_text(ts):
  return datetime.utcfromtimestamp(ts).strftime('%Y-%m-%d %H:%M:%S')

def StereoMatching(tleft, tright, gleft, gright, eleft, eright):
  j,k = 0,0
  matchStereo  = []
  nomatch_left = []
  nomatch_right = []
  allImages = []
  stereoItem = namedtuple('stereoItem', ['tleft','tright','tgps','gleft','gright','eleft','eright'])
  MAX_DIFFERENCE_IMG_TIME = 5e5

  while j < len(tleft) and k < len(tright):
    # Left image without right
    if tleft[j] <  tright[k] - MAX_DIFFERENCE_IMG_TIME:
      nomatch_left.append(tleft[j])
      stereo = stereoItem(tleft = tleft[j], tright = 0, tgps = tleft[j], gleft = gleft[j], gright = 0, eleft = eleft[j], eright = 0)
      allImages.append(stereo)
      j += 1
    # Right image without left
    elif tright[k] <  tleft[j] - MAX_DIFFERENCE_IMG_TIME:
      nomatch_right.append(tright[k])
      stereo = stereoItem(tleft = 0, tright = tright[k], tgps = tright[k], gleft = 0, gright = gright[k], eleft = 0, eright = eright[k])
      allImages.append(stereo)
      k += 1
    # STEREO MATCH
    else:
      stereo = stereoItem(tleft = tleft[j], tright = tright[k], tgps = tleft[j], gleft = gleft[j], gright = gright[k], eleft = eleft[j], eright = eright[k])
      matchStereo.append(stereo)
      allImages.append(stereo)
      j += 1; k += 1

  if j < len(tleft):
    nomatch_left.extend(tleft[j:])

  if k < len(tright):
    nomatch_right.extend(tright[k:])

  return matchStereo, allImages, nomatch_left, nomatch_right


def StereoMatchingGPS(tgps, tleft, tright, gleft, gright, eleft, eright):
  i,j,k = 0,0,0
  matchStereo  = []
  nomatch_left = []
  nomatch_right = []
  allImages = []
  stereoItem = namedtuple('stereoItem', ['tleft','tright','tgps','gleft','gright','eleft','eright'])
  MAX_DIFFERENCE_IMG_TIME = 5e5

  while i < len(tgps) and j < len(tleft) and k < len(tright):
    # Trigger without images
    if tgps[i] < min(tleft[j],tright[k])-MAX_DIFFERENCE_IMG_TIME:
      i += 1 # Dropping triger
    # Left image without trigger
    elif tleft[j] <  tgps[i] - MAX_DIFFERENCE_IMG_TIME:
      nomatch_left.append(tleft[j])
      j += 1
    # Right image without trigger
    elif tright[k] <  tgps[i] - MAX_DIFFERENCE_IMG_TIME:
      nomatch_right.append(tright[k])
      k += 1
    # STEREO MATCH
    elif (abs(tleft[j] - tgps[i])<MAX_DIFFERENCE_IMG_TIME) and (abs(tright[k] - tgps[i])<MAX_DIFFERENCE_IMG_TIME):
      stereo = stereoItem(tleft = tleft[j], tright = tright[k], tgps = tgps[i], gleft = gleft[j], gright = gright[k], eleft = eleft[j], eright = eright[k])
      matchStereo.append(stereo)
      allImages.append(stereo)
      i += 1; j += 1; k += 1
    # Only left image match with trigger
    elif abs(tleft[j] - tgps[i])<MAX_DIFFERENCE_IMG_TIME:
      nomatch_left.append(tleft[j])
      stereo = stereoItem(tleft = tleft[j], tright = 0, tgps = tgps[i], gleft = gleft[j], gright = 0, eleft = eleft[j], eright = 0)
      allImages.append(stereo)
      i += 1; j += 1
    # Only right image match with trigger
    elif abs(tright[k] - tgps[i])<MAX_DIFFERENCE_IMG_TIME:
      nomatch_right.append(tright[k])
      stereo = stereoItem(tleft = 0, tright = tright[k], tgps = tgps[i], gleft = 0, gright = gright[k], eleft = 0, eright = eright[k])
      allImages.append(stereo)
      i += 1; k += 1
    else:
      print("StereoMatchingGPS: Condition not expected")

  if j < len(tleft):
    nomatch_left.extend(tleft[j:])

  if k < len(tright):
    nomatch_right.extend(tright[k:])

  return matchStereo, allImages, nomatch_left, nomatch_right


def saveFiles(outdir, outFname, imformat, matchStereo, allImages, nomatch_left, nomatch_right, tleft, tright):
  # 0. All images matching a trigger (or all images if no GPS trigger to match)
  imfile = open(outdir+'/'+outFname+".mb-2", "w")
  imfile.write("#STEREOCAMERA\n#\n# Date range of files:\n")
  imfile.write("# "+utime_text(allImages[0].tgps/1e6)+"  to  "+utime_text(allImages[-1].tgps/1e6)+"\n#\n")
  for st in allImages:
    if st.tleft > 0:
      imfile.write('PROSILICA_L/'+str(st.tleft)+'.'+imformat+'\t\t')
    else:
      imfile.write('NULL'+'\t\t')
    if st.tright > 0:
      imfile.write('PROSILICA_R/'+str(st.tright)+'.'+imformat+'\t\t')
    else:
      imfile.write('NULL'+'\t\t')
    imfile.write("%.6f"%(st.tgps/1e6)+'\t'+'0 \n');
  imfile.close()
  # 1. Stereo Matches
  imfile = open(outdir+'/'+outFname+"-stereo.mb-2", "w")
  imfile.write("#STEREOCAMERA\n#\n# Date range of files:\n")
  imfile.write("# "+utime_text(matchStereo[0].tgps/1e6)+"  to  "+utime_text(matchStereo[-1].tgps/1e6)+"\n#\n")
  for st in matchStereo:
   imfile.write('PROSILICA_L/'+str(st.tleft)+'.'+imformat+'\t\t')
   imfile.write('PROSILICA_R/'+str(st.tright)+'.'+imformat+'\t\t')
   imfile.write("%.6f"%(st.tgps/1e6)+'\t'+'0 \n');
  imfile.close()
  # 2. Left imagelist
  imfile = open(outdir+'/'+outFname+"-left.mb-2", "w")
  imfile.write("#LEFTCAMERA\n#\n# Date range of files:\n")
  imfile.write("# "+utime_text(matchStereo[0].tgps/1e6)+"  to  "+utime_text(matchStereo[-1].tgps/1e6)+"\n#\n")
  for lt in tleft:
    imfile.write('PROSILICA_L/'+str(lt)+'.'+imformat+'\t\t')
    imfile.write("%.6f"%(lt/1e6)+'\n');
  imfile.close()
  # 3. Right imagelist
  imfile = open(outdir+'/'+outFname+"-right.mb-2", "w")
  imfile.write("#RIGHTCAMERA\n#\n# Date range of files:\n")
  imfile.write("# "+utime_text(matchStereo[0].tgps/1e6)+"  to  "+utime_text(matchStereo[-1].tgps/1e6)+"\n#\n")
  for rt in tright:
    imfile.write('PROSILICA_R/'+str(rt)+'.'+imformat+'\t\t')
    imfile.write("%.6f"%(rt/1e6)+'\n');
  imfile.close()
  # 4. NO MATCHES
  imfile = open(outdir+'/'+"no_match.txt", "w")
  imfile.write("#\n# Date range of files:\n")
  imfile.write("# "+utime_text(matchStereo[0].tgps/1e6)+"  to  "+utime_text(matchStereo[-1].tgps/1e6)+"\n#\n")
  imfile.write("# No matched LEFT images:\n")
  for lt in nomatch_left:
    imfile.write('PROSILICA_L/'+str(lt)+'.'+imformat+'\n')
  imfile.write("# No matched RIGHT images:\n")
  for rt in nomatch_right:
    imfile.write('PROSILICA_R/'+str(rt)+'.'+imformat+'\n')
  imfile.close()

def main (args, opts={}):
  imdir    = "./"
  outdir   = "./"
  imformat = "tif"
  outFname = "imagelist"
  gpsFname = "/../logfile_gpstime_cam_sync.csv"
  fGPStimes = True
  checkBayer = False
  for o, a in opts:
    if o in ("-h", "--help"):
      usage()
    elif o in ("-b", "--check-bayer"):
      checkBayer = True
    elif o in ("-d", "--imdir"):
      imdir = a
    elif o in ("-f", "--imformat"):
      imdir = a
    elif o in ("-o", "--outfile"):
      outFname = a
    elif o in ("-g", "--gpsfile"):
      gpsFname = a
    elif o in ("-x", "--nogpsfile"):
      fGPStimes = False
    else:
      assert False, "unhandled option: "+o+" "+a

  if os.path.dirname(outFname) == '':
    outdir   = imdir
    outFname = os.path.basename(outFname)
  else:
    outdir   = os.path.dirname(outFname)
    outFname = os.path.basename(outFname)

  # Getting image list
  image_list_left  = sorted(glob.glob(imdir+"/PROSILICA_L/*."+imformat))
  image_list_right = sorted(glob.glob(imdir+"/PROSILICA_R/*."+imformat))

  # Get time lists from image filenames
  time_list_left   = [int(os.path.splitext(os.path.basename(f))[0]) for f in image_list_left]
  time_list_right  = [int(os.path.splitext(os.path.basename(f))[0]) for f in image_list_right]

  # Get camera gain and exposure values from image files
  gain_list_left  = []
  exposure_list_left  = []
  gain_list_right  = []
  exposure_list_right  = []
  for f in image_list_left :
    if checkBayer:
      f = f.replace("color", "bayer", 1)
    img = Image.open(f)
    tagdata = img.tag.tagdata
    img.close()
    gain_exposure_str = tagdata[270].decode('UTF-8','ignore')
    (gain_str, exposure_str) = re.findall(r"[-+]?\d*\.?\d+|[-+]?\d+", gain_exposure_str);
    gain_list_left.append(float(gain_str))
    exposure_list_left.append(float(exposure_str))
  for f in image_list_right :
    if checkBayer:
      f = f.replace("color", "bayer", 1)
    img = Image.open(f)
    tagdata = img.tag.tagdata
    img.close()
    gain_exposure_str = tagdata[270].decode('UTF-8','ignore')
    (gain_str, exposure_str) = re.findall(r"[-+]?\d*\.?\d+|[-+]?\d+", gain_exposure_str);
    gain_list_right.append(float(gain_str))
    exposure_list_right.append(float(exposure_str))

  #  Use GPS Triggers?
  if fGPStimes:
    # Getting GPS times
    with open(imdir+gpsFname, "r") as f:
      reader = csv.reader(f)
      time_list_gps_raw = list(reader)
    time_list_gps = [int(row[0]) for row in time_list_gps_raw]

    # Matching Stereo Images with GPS tiggers
    matchStereo, allImages, nomatch_left, nomatch_right  = StereoMatchingGPS(time_list_gps, time_list_left, time_list_right, gain_list_left, gain_list_right, exposure_list_left, exposure_list_right)
    print(str(len(matchStereo))+"\t matched stereo pairs with GPS trigger times")
    print(str(len(allImages))+"\t complete and partial stereo image pairs")
    print(str(len(nomatch_left))+"\t unmatched left images")
    print(str(len(nomatch_right))+"\t unmatched right images")

  else:
    # Matching Stereo Images between them (no GPS)
    matchStereo, allImages, nomatch_left, nomatch_right  = StereoMatching(time_list_left, time_list_right, gain_list_left, gain_list_right, exposure_list_left, exposure_list_right)
    print(str(len(matchStereo))+"\t matched stereo pairs")
    print(str(len(allImages))+"\t complete and partial stereo image pairs")
    print(str(len(nomatch_left))+"\t unmatched left images")
    print(str(len(nomatch_right))+"\t unmatched right images")

  # Save Outputs
  #saveFiles(outdir, outFname, imformat, matchStereo, allImages, nomatch_left, nomatch_right, time_list_left, time_list_right)
  imfile = open(outdir+'/'+outFname+".mb-2", "w")
  imfile.write("#STEREOCAMERA\n#\n# Date range of files:\n")
  imfile.write("# "+utime_text(allImages[0].tgps/1e6)+"  to  "+utime_text(allImages[-1].tgps/1e6)+"\n#\n")
  i=0
  for st in allImages:
    if i < 5:
      imfile.write('## ')
    if st.tleft > 0:
      imfile.write('PROSILICA_L/'+str(st.tleft)+'.'+imformat+'\t\t')
    else:
      imfile.write('NULL'+'\t\t')
    if st.tright > 0:
      imfile.write('PROSILICA_R/'+str(st.tright)+'.'+imformat+'\t\t')
    else:
      imfile.write('NULL'+'\t\t')
    imfile.write("%.6f"%(st.tgps/1e6)+'\t'+"%.6f"%(st.tgps/1e6)+'\t'+"%.1f"%(st.gleft)+'\t'+"%.1f"%(st.gright)+'\t'+"%.1f"%(st.eleft)+'\t'+"%.1f"%(st.eright)+'\n')
    # if st.tleft > 0 and st.tright > 0 and (st.gleft != st.gright or st.eleft != st.eright):
    #   print("Image settings differ: PROSILICA_L/"+str(st.tleft)+"."+imformat+"\tPROSILICA_R/"+str(st.tright)+"."+imformat+"\t"+str(st.gleft)+"\t"+str(st.gright)+"\t"+str(st.eleft)+"\t"+str(st.eright))
    i += 1
  imfile.close()


if __name__ == "__main__":
  # Parse command line arguments
  try:
      opts, args = getopt.gnu_getopt(sys.argv[1:], shortOpts, longOpts)
  except getopt.GetoptError as err:
      # print help information and exit:
      print (str(err)) # will print something like "option -a not recognized"
      usage()

  # Run main script
  main (args,opts)
