######################################################################
#
# Helper functions to find mapped trees.  Currently adding
# trees requires manually finding them in an RF data set 
# and storing the week/time of that event in this file. If you
# want the location, you can use the truth data to find it.
#
# Copyright Trimble 2021
######################################################################

from mutils import *
from collections import namedtuple
import time

Trees = namedtuple('Trees','week secs')

# These are all from this data set
# http://fermion.eng.trimble.com:5000/RFPlaybackRegression/Samples/2021-02-17-Trees-van-test.xml
# Try to find trees that have relatively clear view of the sky before/after 
#
# I use video from here:
# http://quark.eng.trimble.com/DataDump/2021-02-17-vanIMU-V550-89-Test3-Tree/VideoOut-Replay-NovatelSeptentrio/
# [I have all the files on my PC, easier to flick through the videos]. I simply viewed some (not all) 
# video and found example trees that met my criteria - not perfect, but good enough to give us a metric
# we can test against. I'm starting to drop results here:
# http://quark.eng.trimble.com/DataDump/DashBoard/RFRegressionSummary/TreeOBS/
# The sctructure of the directory will likely change as the processing is automated.
trees = [Trees(2145,332057),
         Trees(2145,332075),
         Trees(2145,332127),
         Trees(2145,332654),
         Trees(2145,332693),
         Trees(2145,332722),
         Trees(2145,332735),
         Trees(2145,332939),
         Trees(2145,333069),
         Trees(2145,333348),
         Trees(2145,333360),
         Trees(2145,333372),
         Trees(2145,333817),
         Trees(2145,333839),
         Trees(2145,333839),
         Trees(2145,333860),
         Trees(2145,333961),
         Trees(2145,333997),
         Trees(2145,334243),
         Trees(2145,334376),
         Trees(2145,334575),
         Trees(2145,335273),
         Trees(2145,335287),
         Trees(2145,335364),
         Trees(2145,335438),
         Trees(2145,335918),
         Trees(2145,337575),
         Trees(2145,337592),
         Trees(2145,337617),
         Trees(2145,337837),
         Trees(2145,337862),
         Trees(2145,338063),
         Trees(2145,338838),
         Trees(2145,338879),
         Trees(2145,339009),
         Trees(2145,339155),
         Trees(2145,339253),
         Trees(2145,339312),
         Trees(2145,339326),
         Trees(2145,339434),
         Trees(2145,339524),
         Trees(2145,339770),
         Trees(2145,340174),
         Trees(2145,340400),
         Trees(2145,340589)
]

def find(x):
    return where(x)[0]

def find_tree( weekNum, # Assume all data is in the same week
               pospac,
               as_start_stop_time,
               minTime = -10.0,
               maxTime = 10.0):
    """
    pospac = truth position from POSPac
    minTime = how much time before an event in seconds
    maxTime = how much time after an event in seconds

    Returns: ([overpass_lat_deg,overpass_lon_deg],
              [overpass event 0, event 1...] )
    Each overpass event has one of two formats:
     1- if 'as_start_stop_time' is True, each event is:
        (start_time_sec, stop_time_secs)
     2- otherwise, each event is a full array of info with following fields:
         TIME [s]
         LAT [deg]
         LON [deg]
         HGT [m]
         RANGE [s] == time before/after a "tree" event
    """

    timeVector = pospac[:,0]
    llh = pospac[:,1:4]
    heading = pospac[:,12]

    ref_loc = []
    data = []

    for tree in trees:
        # Compute the ENU difference between the overpass and truth
        # data. Then compute the 2D range to the obstruction as
        # well as the cross-track error (helps to eliminate any
        # data from adjacent roads with the same heading).
        treeTime = array([tree.week, tree.secs])

        j = find (   (timeVector >= (tree.secs + minTime)) # Due to sign convention add min time
                   & (timeVector <= (tree.secs + maxTime)) )

        if len(j) <= 1:
            continue

        #curr_alongRange = alongTrack[j]
        #curr_Range = Range[j]
        curr_t = timeVector[j]
        curr_llh = llh[j,:]

        if as_start_stop_time:
          data.append( (curr_t[0], curr_t[-1]) )
        else:
          d = empty([len(curr_t),6])

          d[:,0] = curr_t
          d[:,1:4] = curr_llh
          d[:,4] = curr_t - tree.secs # We place this in the range field so that we can use the overpass functions
          d[:,5] = curr_t - tree.secs
          k=dotdict(['TIME','LAT','LON','HGT','RANGE','TREETIME'])
          data.append( VdCls(d,k,[]) )
        ref_loc.append( [tree.week, tree.secs] )
    return ref_loc, data