######################################################################
#
# Helper functions to find freeway overpasses.  Currently adding
# overpasses requires manually finding them on a map and entering
# coordinates here.
#
# Copyright Trimble 2020
######################################################################

from mutils import *
from collections import namedtuple
import time

Overpass = namedtuple('Overpass','lat lon heading')
overpasses = [
# Enter lat/lon[deg] and heading[deg] of road under the overpass:
Overpass(37.5281344, -122.27217222, 137), # small walkway
Overpass(37.5262249, -122.2701490, 134), # highway overpass
Overpass(37.5139653, -122.2562366, 139),
Overpass(37.49633611,-122.23321944,289.260),
Overpass(37.49209722,-122.22067222,299),
Overpass(37.483335833, -122.1807065, 115),
Overpass(37.4790015, -122.17249722, 126),
Overpass(37.4688223, -122.1552842, 126),
Overpass(37.4603597, -122.1408827, 125),
Overpass(37.4489738, -122.1229727, 141),
Overpass(37.44693611, -122.12096944, 139.9),
Overpass(37.42835, -122.10144444, 141),
Overpass(37.421025, -122.09250833, 133),
Overpass(37.41676389, -122.0866334, 133),
Overpass(37.41471111, -122.0838611, 130.3),
Overpass(37.4118467, -122.0781202, 117),
Overpass(37.4089589, -122.0704025, 116),
Overpass(37.40763611, -122.066375, 105),
Overpass(37.4006736, -122.035751, 285),
Overpass(37.39895833, -122.02782222, 284.8),
Overpass(37.39568333, -122.01282778, 285),
Overpass(37.3912564, -121.9959449, 292),
Overpass(37.3856117, -121.9767883, 109),
Overpass(37.3818991, -121.9638316, 109),
Overpass(37.3779719, -121.9482200, 103),
Overpass(37.3766833, -121.9416326, 103),
Overpass(37.3766631, -121.8550906, 141),
Overpass(37.3737240, -121.9279315, 106),
Overpass(37.3735508, -121.9272265, 107),
Overpass(37.3717120, -121.8758715, 46 ),
Overpass(37.3679018, -121.8462525, 143),
Overpass(37.364225,-121.90185,108.6),
Overpass(37.3634555556,-121.8963333333,95.80),
Overpass(37.3636055556,-121.8981361111,95.60),
Overpass(37.3631599, -121.8432405, 155),
Overpass(37.3597037, -121.8411792, 155),
Overpass(37.3472435, -121.8416139, 225),
Overpass(37.3469589, -121.9044695, 311),
Overpass(37.3398235, -121.8519716, 233),
Overpass(37.3738087,-121.9280381,317.504),
Overpass(37.4568493,-122.1346678,306.495),
Overpass(37.30313333,-121.81673889,321.4555),
Overpass(37.31844167,-121.83153056,322.3),
Overpass(37.33591111,-121.84840556,322.2),
Overpass(37.33961389,-121.85196389,322),
Overpass(37.34639722,-121.85854722,323.3),
Overpass(37.34989444,-121.86169722,324.4),
Overpass(37.35420833,-121.86618056,317.97),
Overpass(37.35847222,-121.87134722,309.5),
Overpass(37.36116667,-121.87740278,289.14),
Overpass(37.36243333,-121.88590833,278.1425),
Overpass(37.5445694444,-122.2879472222,143.78),
Overpass(37.5532333333,-122.2959666667,142.75),
Overpass(37.5708083333,-122.31403056,318.84),
Overpass(37.5842444444,-122.3286194444,317.97),
Overpass(37.59030833,-122.36162222,308.75),
Overpass(37.60258333,-122.38033889,128.98),
Overpass(37.61378889,-122.3966638889,325.25),
Overpass(37.6156361111,-122.3980472222,158.00),
Overpass(37.6313277778,-122.4027027778,354.46),
Overpass(37.6333611111,-122.4029777778,352.14),
Overpass(37.6359944444,-122.4037250000,339.76),
Overpass(37.6627638889,-122.4006055556,230.70),
Overpass(37.66386667,-122.39894722,230.40),
Overpass(37.7159277778,-122.3985833333,157.075),
Overpass(37.7340472222,-122.4060722222,154.35),
Overpass(37.7352555556,-122.4068277778,154.51),
Overpass(37.7365583333,-122.4075777778,158.25),
Overpass(37.75449444,-122.4030555556,176.01),
Overpass(37.7602722222,-122.3923027778,355.24),
Overpass(37.7628222222,-122.39265833,355.52),
Overpass(37.2818166667,-121.8084444444,155.44),
Overpass(37.2572388889,-121.7962416667,314.89),
Overpass(37.2296138889,-121.7474694444,125.22),
Overpass(37.7568750000,-122.4034972222,155.06),
Overpass(37.7621638889,-122.4057916667,206.89),
Overpass(37.7523444444,-122.4029222222,184.28),
Overpass(37.7439416667,-122.4057333333,210.30),
Overpass(37.6702166667,-122.3923750000,214.77),
Overpass(37.5762805556,-122.3198916667,318.80),
Overpass(37.4754722222,-122.1664916667,305.94),
Overpass(37.3974250000,-122.0208055556,283.11),
Overpass(37.3949416667,-122.0094305556,285.00),
Overpass(37.7689365,-122.4069767,133.249),
Overpass(37.7326087,-122.4052699,154.769),
Overpass(37.3239111111,-121.8921361111,76.03),
Overpass(37.3223000000,-121.8983527778,56.81),
Overpass(37.3209500000,-121.9003361111,40.99),
Overpass(37.3147861111,-121.9109722222,80.00),
Overpass(37.3542055556,-121.9113888889,38.34),
Overpass(37.3488361111,-121.9182000000,47.06),
Overpass(37.3426500000,-121.9259694444,38.90),
Overpass(37.3392138889,-121.9304000000,44.20),
Overpass(37.3231944444,-121.9406083333,353.95),
Overpass(37.3186277778,-121.9402333333,356.55),
Overpass(37.3173416667,-121.9402805556,1.96),
Overpass(37.3051611111,-121.9405944444,0.38),
Overpass(37.2943305556,-121.9383416667,336.76),
Overpass(37.2927888889,-121.9375916667,336.43),
Overpass(37.2589250000,-121.9535027778,27.90),
Overpass(37.2428583333,-121.8039666667,280.77),
Overpass(37.2432944444,-121.8177083333,270.24),
Overpass(37.2473444444,-121.8299666667,300.99),
Overpass(37.2507888889,-121.8368333333,300.70),
Overpass(37.2554916667,-121.8572527778,269.38),
Overpass(37.2554916667,-121.8605305556,269.20),
Overpass(37.2539333333,-121.8809555556,245.69),
Overpass(37.2506944444,-121.8954527778,268.43),
Overpass(37.2506694444,-121.8990527778,268.96),
Overpass(37.2505055556,-121.9222472222,268.84),
Overpass(37.2509750000,-121.9313611111,282.17),
Overpass(37.2529888889,-121.9429138889,283.38),
Overpass(37.2545000000,-121.9512972222,285.00),
Overpass(37.3239111,-121.8921361,333.84),
Overpass(37.2712472222,-121.9937777778,120.75),
Overpass(37.2572500000,-121.8588527778,14.19),
Overpass(37.2659694444,-121.8591000000,351.36),
Overpass(37.3017305556,-121.8767833333,327.30),
Overpass(37.3176666667,-121.8895277778,340.93),
Overpass(37.3199000000,-121.8902194444,347.01),
Overpass(37.2553777778,-121.9546583333,109.63),
Overpass(37.2557166667,-121.9559305556,112.83),
Overpass(37.2585833333,-121.9642000000,110.69),
Overpass(37.2677444444,-121.9866583333,119.55),
Overpass(37.2826666667,-122.0151194444,133.54),
Overpass(37.2858833333,-122.0192166667,139.29),
Overpass(37.2887388889,-122.0215972222,146.25),
Overpass(37.2939777778,-122.0258972222,146.02),
Overpass(37.3009472222,-122.0322583333,136.82),
Overpass(37.3088277778,-122.0414638889,136.54),
Overpass(37.3155916667,-122.0467833333,158.25),
Overpass(37.3228666667,-122.0500194444,168.72),
Overpass(37.3375138889,-122.0596305556,165.40),
Overpass(37.3452111111,-122.0598805556,179.33),
Overpass(37.3697944444,-122.0656027778,153.90),
Overpass(37.3781750000,-122.0676611111,175.72),
Overpass(37.3857305556,-122.0646416667,255.44),
Overpass(37.1315472222,-121.6335666667,334.07),
Overpass(37.1410944444,-121.6409972222,321.82),
Overpass(37.1529611111,-121.6525027778,321.90),
Overpass(37.1624777778,-121.6631750000,317.52),
Overpass(37.2097472222,-121.7231416667,317.20),
Overpass(37.2420500000,-121.7692166667,305.28),
Overpass(37.3630277778,-121.8915166667,275.94),
Overpass(37.1192444444,-121.6263416667,154.23),
Overpass(37.1044222222,-121.6113388889,133.78),
Overpass(37.0888500000,-121.5994388889,155.55),
Overpass(37.0713722222,-121.5898666667,157.97),
Overpass(37.0593722222,-121.5845722222,160.23),
Overpass(37.0436805556,-121.5777722222,340.34),
Overpass(37.0098166667,-121.5598361111,156.44),
Overpass(37.0567083333,-120.9697694444,91.54),
Overpass(37.1006527778,-121.0112000000,146.06),
Overpass(37.1063388889,-121.0158000000,146.31),
Overpass(37.5470833333,-122.3229111111,252.96),
Overpass(37.5125638889,-122.3301083333,216.06),
Overpass(37.4190194444,-122.2194194444,101.58),
Overpass(37.3436750000,-122.0939500000,136.92),
Overpass(37.3322444444,-122.0575472222,88.22),
Overpass(37.3213972222,-122.0558388889,85.00),
Overpass(37.3880972222,-122.0687916667,356.11),
Overpass(37.3989194444,-122.0695500000,347.90),
Overpass(37.4047833333,-122.0187222222,73.43),
Overpass(37.4068111111,-122.0097722222,73.86),
Overpass(37.4092583333,-121.9987444444,74.02),
Overpass(37.4192361111,-121.9594611111,84.20),
Overpass(37.4202638889,-121.9399083333,73.42),
Overpass(37.4234194444,-121.9232888889,69.50),
Overpass(37.4015333333,-121.9090027778,168.45),
Overpass(37.4154861111,-121.9131083333,162.99),
Overpass(37.4256611111,-121.9165000000,344.23),
Overpass(37.4274583333,-121.9172555556,338.52),
Overpass(37.4543416667,-121.9229611111,350.60),
Overpass(37.4836972222,-121.9368277778,320.62),
Overpass(37.4927111111,-121.9485888889,313.22),
Overpass(37.5224611111,-121.9876555556,301.09),
Overpass(37.5298027778,-122.0022305556,303.49),
Overpass(37.5392972222,-122.0173388889,329.77),
Overpass(37.5428166667,-122.0199972222,328.66),
Overpass(37.5353555556,-121.9334583333,236.56),
Overpass(37.5334916667,-121.9370638889,236.07),
Overpass(37.5310388889,-121.9416500000,231.46),
Overpass(37.5146472222,-121.9397388889,154.79),
Overpass(37.4144055556,-121.8803833333,166.26),
Overpass(37.4096416667,-121.8789555556,165.94),
Overpass(37.4055111111,-121.8776666667,165.38),
Overpass(37.3844777778,-121.8629444444,138.86),
Overpass(37.4257361111,-121.8838138889,159.32),
Overpass(37.5073888889,-121.9674472222,314.43),

# Overpasses in Japan (includes short tunnels and toll booths)
# 2019-09-13-Tokyo-Drive03:
Overpass(35.810425, 139.842314, 44),
Overpass(35.8355714, 139.8586699, 40),
Overpass(35.8368138, 139.8600649, 45),
Overpass(35.8505020, 139.8755957, 27),
Overpass(35.8654454, 139.8828574, 24),
Overpass(35.8802693, 139.9037301, 52),

# The following are excluded in 2019-09-13-Tokyo-Drive03 for being too severe:
#Overpass(35.881248, 139.905169, 50),
#Overpass(35.8826048, 139.9070176, 45),
#Overpass(35.8829834, 139.9074997, 45),
#Overpass(35.8875505, 139.9121671, 36),
#Overpass(35.8894949, 139.9139464, 36),
#Overpass(35.8901385, 139.9145547, 38),
#Overpass(35.8914338, 139.9158140, 39),
#Overpass(35.8979516, 139.9228879, 44),
#Overpass(35.8990461, 139.9242968, 47),
#Overpass(35.9001809, 139.9256647, 44),
#Overpass(35.9013242, 139.9269880, 41),
#Overpass(35.9021090, 139.9277935, 39),
#Overpass(35.9033501, 139.9289788, 40),

Overpass(35.9049693, 139.9304783, 37),
Overpass(35.9066335, 139.9320980, 41),
Overpass(35.9088028, 139.9346560, 47),
Overpass(35.9099010, 139.9361924, 50),
Overpass(35.9107533, 139.9375340, 53),
Overpass(35.9118774, 139.9395460, 57),
Overpass(35.9131201, 139.9422413, 63),
Overpass(35.9140657, 139.9446052, 64),
Overpass(35.9149762, 139.9468662, 62),
Overpass(35.9161195, 139.9494010, 60),
Overpass(35.9420835, 139.9695359, 27),
Overpass(35.9441179, 139.9706759, 23),
Overpass(35.9466269, 139.9719123, 20),
Overpass(35.9492878, 139.9730826, 19),
Overpass(35.9517712, 139.9740898, 17),
Overpass(35.9525353, 139.9743855, 17),
Overpass(35.9550472, 139.9752982, 16),
Overpass(35.9570544, 139.9759646, 14),
Overpass(35.9585915, 139.9764272, 15),
Overpass(35.9609203, 139.9771173, 14),
Overpass(35.9642471, 139.9781720, 18),
Overpass(35.9663478, 139.9791032, 22),
Overpass(35.9680794, 139.9800492, 26),
Overpass(35.9947802, 140.0324140, 34),
Overpass(35.9971495, 140.0343280, 34),
Overpass(35.9997522, 140.0365446, 36),
Overpass(36.0008278, 140.0375224, 37),
Overpass(36.0022515, 140.0388693, 39),
Overpass(36.0048440, 140.0415188, 41),
Overpass(36.0181831, 140.0636209, 69),
Overpass(36.0220327, 140.0760454, 56),
Overpass(36.0228723, 140.0776172, 55),
Overpass(36.0226435, 140.0834123, 128),
Overpass(36.0578510, 140.0839397, 351),

# Munich

Overpass(48.063511111111, 11.710430555556, 55.0),
Overpass(48.075522222222, 11.730138888889, 40.5),
Overpass(48.099147222222, 11.750647222222, 20.4),
Overpass(48.102630555556, 11.752516666667, 20.1),
Overpass(48.106175000000, 11.754141666667, 18.4),
Overpass(48.142694444444, 11.757836111111, 348.5),
Overpass(48.152280555556, 11.753283333333, 336.0),
Overpass(48.156336111111, 11.750280555556, 332.0),
Overpass(48.160669444444, 11.746686111111, 329.5),
Overpass(48.168883333333, 11.739119444444, 327.0),
Overpass(48.173525000000, 11.734430555556, 325.0),
Overpass(48.190766666667, 11.703847222222, 290.2),
Overpass(48.192400000000, 11.696677777778, 287.0),
Overpass(48.192947222222, 11.693827777778, 286.5),
Overpass(48.197194444444, 11.676013888889, 293.7),
Overpass(48.226641666667, 11.632072222222, 15.5),
Overpass(48.233311111111, 11.635363888889, 18.6),
Overpass(48.243316666667, 11.640422222222, 18.6),
Overpass(48.255625000000, 11.646508333333, 13.4),
Overpass(48.264216666667, 11.646494444444, 347.0),
Overpass(48.278130555556, 11.641763888889, 347.0),
Overpass(48.295100000000, 11.633680555556, 341.0),
Overpass(48.300147222222, 11.630694444444, 338.7),
Overpass(48.316930555556, 11.620580555556, 339.0),
Overpass(48.341983333333, 11.606513888889, 339.3),
Overpass(48.350425000000, 11.601497222222, 338.3),
Overpass(48.386291666667, 11.597377777778, 0.7),
Overpass(48.395305555556, 11.593883333333, 331.7),
Overpass(48.406241666667, 11.589730555556, 359.2),
Overpass(48.413394444444, 11.589686111111, 359.7),
Overpass(48.423002777778, 11.589491666667, 358.9),
Overpass(48.429041666667, 11.589380555556, 359.9),
Overpass(48.435300000000, 11.589325000000, 359.5),
Overpass(48.473802777778, 11.585205555556, 355.7),
Overpass(48.508447222222, 11.586072222222, 329.3),
Overpass(48.537080555556, 11.595263888889, 359.2),
Overpass(48.564336111111, 11.590741666667, 18.0),
Overpass(48.609916666667, 11.543544444444, 324.0),
Overpass(48.622419444444, 11.526502777778, 336.4),
Overpass(48.658875000000, 11.512111111111, 350.3),
Overpass(48.670555555556, 11.507669444444, 340.3),
Overpass(48.683558333333, 11.499319444444, 334.7),
Overpass(48.701469444444, 11.488725000000, 344.2),
Overpass(48.710113888889, 11.485050000000, 342.3),
Overpass(48.716822222222, 11.481727777778, 341.8),
Overpass(48.736675000000, 11.467830555556, 333.4),
Overpass(48.743855555556, 11.462347222222, 333.4),
Overpass(48.750636111111, 11.457963888889, 345.3),
Overpass(48.788097222222, 11.460369444444, 8.1),
Overpass(48.800808333333, 11.464688888889, 13.0),
Overpass(48.807183333333, 11.465930555556, 1.2),
Overpass(48.807586111111, 11.465933333333, 1.2),
Overpass(48.815716666667, 11.467288888889, 17.3),
Overpass(48.917666666667, 11.469416666667, 349.5),
Overpass(49.061847222222, 11.321894444444, 318.4),
Overpass(49.074944444444, 11.296369444444, 314.0),
Overpass(49.113141666667, 11.278927777778, 349.5),
Overpass(49.162108333333, 11.263866666667, 352.0),
Overpass(49.167766666667, 11.262655555556, 352.2),
Overpass(49.173519444444, 11.261330555556, 347.2),
Overpass(49.279391666667, 11.209316666667, 342.0),
Overpass(49.309558333333, 11.207700000000, 358.2),
Overpass(49.332277777778, 11.204986111111, 354.9),
Overpass(49.339655555556, 11.204002777778, 352.9),
Overpass(49.349452777778, 11.203727777778, 0.0),
Overpass(49.365852777778, 11.201897222222, 355.4),
Overpass(49.373083333333, 11.200958333333, 356.0),
Overpass(49.395711111111, 11.199861111111, 342.4),
Overpass(48.966016666667, 11.413047222222, 113.6),
Overpass(48.226902777778, 11.630925000000, 193.5),
Overpass(48.057922222222, 11.698677777778, 230.9),
Overpass(48.054841666667, 11.693919444444, 222.0),
Overpass(48.053116666667, 11.691813888889, 217.2),
Overpass(48.046980555556, 11.686363888889, 28.0),
Overpass(48.041041666667, 11.599513888889, 152.6),
Overpass(48.027433333333, 11.635533333333, 99.4),
Overpass(48.026250000000, 11.652527777778, 110.5),
Overpass(48.022491666667, 11.659280555556, 149.0),
Overpass(47.977716666667, 11.684552777778, 168.7),
Overpass(47.961922222222, 11.689100000000, 168.8),
Overpass(47.938758333333, 11.703102777778, 155.6),
Overpass(47.903475000000, 11.719533333333, 165.5),
Overpass(47.894147222222, 11.726275000000, 137.6),
Overpass(47.864694444444, 11.805911111111, 85.0),
Overpass(47.835333333333, 11.878808333333, 128.9),
Overpass(47.827225000000, 11.946569444444, 85.9),
Overpass(47.823013888889, 11.966080555556, 118.0),
Overpass(47.821941666667, 11.985619444444, 88.8),
Overpass(47.820391666667, 12.014369444444, 94.8),
Overpass(47.816000000000, 12.053819444444, 98.2),
Overpass(47.814536111111, 12.065411111111, 99.0)

]

# Search Time/Range around center time(center_t) until we get to
# a distance of maxRange meters away.  But.. if there is a 1s time
# gap then abort because that should never happen.
def find_indexes_around_center_time( center_t, Time, maxRange, Range ):
    max_time_gap = 1.
    all_i = []
    center_i = argmin( abs(Time-center_t) )
    i = center_i
    while Range[i] <= maxRange:
        all_i.append(i)
        last_t = Time[i]
        i+=1
        if i >= len(Range):
            break
        if Time[i]-last_t > max_time_gap:
            break
    i = center_i-1
    while Range[i] <= maxRange:
        all_i.append(i)
        last_t = Time[i]
        i-=1
        if i < 0:
            break
        if last_t - Time[i] > max_time_gap:
            break
    return sorted(all_i)


def find_overpasses( pospac,
                     as_start_stop_time,
                     maxRange = 201.0,
                     minRange = 60.,
                     maxCrossTrack = 30.0 ):
    """
    pospac = truth position from POSPac
    maxRange = maximum 2D distance to overpass [m].  Default 201m.
               (ProcessResults.py is 200m, so go a little beyond)
    minRange = we must get at least this close to overpass[m], otherwise
      we're probably on a road near the overpass.  Default 60m.
    maxCrossTrack = maximum crosstrack to overpass[m]. Default 30m.

    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 [m] == rough along-track distance to overpass
         towards == 1 if approaching overpass, 0 otherwise

    Note: if overpasses are close together, overpass events can overlap.
    Example usage:
      pp=doload('/mnt/data_drive/DataDump/2016-11-08-vanIMU-SJ/POSPac/Testrun-11-08-2016.txt')
      overpass_pos, events = find_overpasses(pp,False)
      plot( events[0].TIME, events[0].RANGE ) # first overpass
      plot( events[1].TIME, events[1].lat ) # next overpass..
    """

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

    # Compute the partial body frame to ENU rotation matrix
    #
    #   sy   cy   0
    #   cy  -sy   0
    #    0    0  -1
    #
    yaw = heading * pi / 180.0
    cy = cos(yaw)
    sy = sin(yaw)

    ##################
    # Find all data within a threshold of each overpass
    ##################
    ref_loc = []
    data = []
    for overpass in overpasses:
        # 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).
        overpassCoord = array([overpass.lat, overpass.lon, 0.])

        # Narrow down range we search to avoid massive llh2enu()
        # calls.  0.01 deg should be way over 200m (unless you're near
        # the north or south poles, but then hopefully you won't be
        # near an overpass).
        i = where( (abs(llh[:,0]-overpass.lat)<.01)
                   & (abs(llh[:,1]-overpass.lon)<.01) )[0]
        if len(i) <= 1:
            continue

        Time = t[i]
        overpassLLH = llh[i,:]
        (dE_over, dN_over, dU_over) = llh2enu( overpassLLH, overpassCoord)

        alongTrack = sy[i]*dE_over + cy[i]*dN_over

        Range = abs(alongTrack)

        crossTrack = cy[i]*dE_over + -sy[i]*dN_over

        deltaHeading = heading[i] - overpass.heading

        # Constrain the angle we are interested in to be within 0 and 180 by taking
        # first the mod (will be within +/-180) and then the abs value. We want
        # a heading match within a tolerance or a match 180 degrees offset (two
        # directions on the road).
        deltaHeading = np.abs(fmod(deltaHeading,180))
        # For any angle greater than 90 subtract 180, this brings all the angles
        # that are a match to the road close to 0.
        k = find(deltaHeading > 90)
        if(len(k) > 0):
           deltaHeading[k] -= 180
        # Now we should be clustered +/- around 0. Take the mod
        # and the data we care about is within a positive tolerance
        # of 0.
        deltaHeading  = np.abs(deltaHeading)

        # Find data that appears to be on the road and within minRange
        # of the overpass, the cross track criteria, and the heading
        # matches the road orienation under the overpass
        j = where(   (Range <= minRange)
                     & ( abs(crossTrack) < maxCrossTrack)
                     & ( deltaHeading < 10 ))[0]

        if len(j) <= 1:
            continue

        # We may have more than 1 overpass.  Split by 10s gaps.
        overpass_center_t = []
        curr_Time = Time[j]
        curr_Range = Range[j]
        jumps = (where(diff(curr_Time) > 10.)[0]+1).tolist() + [len(j)]
        i_start = 0
        for i_jump in jumps:
            if i_jump -1 == i_start:
                # ignore glitch/empty range
                continue
            ij = argmin(abs(curr_Range[i_start:i_jump-1]))
            overpass_center_t.append( curr_Time[ij+i_start] )
            i_start = i_jump

        for center_t in overpass_center_t:
            j = find_indexes_around_center_time( center_t, Time, maxRange, Range )
            if len(find( ( abs(crossTrack[j]) < maxCrossTrack)
                     & ( deltaHeading[j] < 10 ))) < 10:
                print("False detection around %.1f"%center_t)
                continue
            if len(find(diff(alongTrack[j])<0)) > 0:
                print("Oscillating range around %.1f"%center_t)
                continue

            curr_Time = Time[j]
            curr_Range = Range[j]
            if as_start_stop_time:
                data.append( (curr_Time[0], curr_Time[-1]) )
            else:
                d = empty([len(j),6])
                d[:,0] = Time[j]
                d[:,1:4] = overpassLLH[j,:]
                d[:,4] = alongTrack[j]
                d[1:,5] = diff(d[:,4]) < 0
                d[0,5] = d[1,5]
                k=dotdict(['TIME','LAT','LON','HGT','RANGE','towards'])
                data.append( VdCls(d,k,[]) )
            ref_loc.append( [overpass.lat, overpass.lon] )

    return ref_loc, data

def get_rcvr_err( overpass_events, d ):
    """
    overpass_events = list_of_overpass_events (from find_overpasses())
    d = receiver rec29 or rec35:2 data (from vd2cls())

    For each overpass, returns:
      ( err, overpass_events_indexes, d_indexes )
      where 'err' has fields:
        TIME = GPS secs
        err2D = 2D error[m] of 'd' at TIME
        err3D = 3D error[m] of 'd' at TIME
        errHgt = Height error[m] of 'd' at TIME
    Example:
      pp = doload('POSPac_truth.txt')
      d = vd2cls('file.T04','-d35:2')
      overpass_pos, events = find_overpasses(pp, False)
      all_err = get_rcvr_err( events, d )
      err, i_o, i_d = all_err[0]
      figure()
      plot( err.TIME, err.err3D )
      plot( events[0][i_o].RANGE, err.err3D )
    """
    data = []
    for overpass in overpass_events:
        t,i_o,i_d = intersect(overpass.TIME, d.TIME)
        dE,dN,dU = llh2enu( d[i_d,d.k.LAT:d.k.LAT+3],
                            overpass[i_o,overpass.k.LAT:overpass.k.LAT+3] )
        curr = empty((len(t),4))
        curr[:,0] = t
        curr[:,1] = sqrt( dE**2 + dN**2 )
        curr[:,2] = sqrt( dE**2 + dN**2 + dU**2 )
        curr[:,3] = abs( dU )
        k=dotdict(['TIME','err2D','err3D', 'errHgt'])
        data.append( (VdCls(curr,k,[]), i_o, i_d) )
    return data


dBayArea = 0
dTokyo   = 1 # Not implemented in the map yet
dMunich  = 2


# Plots the location of the overpass with a Google Map backgroun.
#
# region - only dBayArea  & dMunich implemented - filters the data to this region
# key    - Google API key
# filename - png filename we'll use to save the data.
def map_overpasses(region,key,filename):

  try:
    import matplotlib.pyplot as plt
    #import matplotlib.colors as colors
    import matplotlib.cm as cmx
    from salem import GoogleVisibleMap, Map
  except:
    print('Missing libraries')
    return

  lat = []
  lon = []
  for overpass in overpasses:
    x = overpass.lon
    y = overpass.lat
    # Filter the data to get the region we're interested in
    if(region == dBayArea):
      if( (x <= -121) and (x >= -123) and (y <= 38) and (y >= 36) ):
        lon.append(x)
        lat.append(y)
    if(region == dMunich):
      if( (x <= 13) and (x >= 11) and (y <= 50) and (y >= 47) ):
        lon.append(x)
        lat.append(y)
    else:
      print("Region not supported")
      return

  minX = np.min(lon)
  maxX = np.max(lon)
  minY = np.min(lat)
  maxY = np.max(lat)

  #f, (ax1,ax2) = plt.subplots(1, 2)
  f, ax1 = plt.subplots(1, 1)
  g = GoogleVisibleMap(x=[minX, maxX], y=[minY,maxY],
                       scale=2,  # scale is for more details
                       #maptype='roadmap',
                       maptype='hybrid',
                       key=key)

  # the google static image is a standard rgb image
  ggl_img = g.get_vardata()

  ax1.imshow(ggl_img)
  if(region == dBayArea):
    ax1.set_title('Bay Area Overpasses')
  elif(region == dMunich):
    ax1.set_title('Munich Area Overpasses')

  sm = Map(g.grid, factor=1, countries=False)
  sm.visualize(ax=ax1)
  # transform the lat/long into the image coord frame
  x_,y_ = sm.grid.transform(lon,lat)
  # Plot a different color for each slip count value
  ax1.scatter(x_,y_,color='r')

  plt.tight_layout()
  savePlot(filename)
  #plt.show()


def savePlot(filename):
  import io
  from PIL import Image

  # Save the png to memory
  ram = io.BytesIO()
  plt.savefig(ram,format='png',dpi=600)
  ram.seek(0)
  # Compress the png data before saving to the file system, the
  # matplotlib png's are not well compressed
  im = Image.open(ram)
  im2 = im.convert('RGB').convert('P', palette=Image.ADAPTIVE)
  im2.save(filename,format='PNG')



def writeKMLMarker(fid,lat,lon,heading):
  fid.write('<Placemark>')
  fid.write('<visibility>1</visibility>')
  fid.write('<description>')
  fid.write('<![CDATA[')
  fid.write('<table cellpadding="1" cellspacing="0">')
  fid.write('<tr><td>Lat</td><td>%.7f°</td></tr>' % lat)
  fid.write('<tr><td>Lon</td><td>%.7f°</td></tr>' % lon)
  fid.write('<tr><td>Heading</td><td>%.3f°</td></tr>' % heading)
  fid.write('</table>')
  fid.write(']]>')
  fid.write('</description>')
  fid.write('<styleUrl>#marker</styleUrl>')
  fid.write('<Point>')
  fid.write('<coordinates>')
  fid.write('%.8f,%.8f,%.3f' % (lon,lat,0))
  fid.write('</coordinates>')
  fid.write('</Point>')
  fid.write('</Placemark>\n')

def writeKMLHeader(fid,markerColor):
  fid.write('<?xml version="1.0" encoding="UTF-8"?>')
  fid.write('<kml xmlns="http://earth.google.com/kml/2.0"> <Document>')
  fid.write('<Style id="marker"><IconStyle>')
  fid.write('<color>ff%s</color>' % markerColor)
  fid.write('<scale>1.5</scale>') # Make the marker oversized so it is easy to see along with other data
  fid.write('<Icon><href>http://maps.google.com/mapfiles/kml/shapes/placemark_square.png</href></Icon>')
  fid.write('</IconStyle>')
  # This hides the directions to/from
  # in the place marker boxes
  fid.write('<BalloonStyle><text><![CDATA[$[description]]]></text></BalloonStyle>')
  fid.write('</Style>\n')

# Provide a function to create a KML file that contains all the overpass locations for a given region
# ToDo - add support for Tokyo
def createObstructionKML(filename,region):
  with open(filename,'w') as fid:
    writeKMLHeader(fid,'FF0000') # BBGGRR
    for overpass in overpasses:
      lon     = overpass.lon
      lat     = overpass.lat
      heading = overpass.heading

      # Filter the data to get the region we're interested in
      if(region == dBayArea):
        if( (lon <= -121) and (lon >= -123) and (lat <= 38) and (lat >= 36) ):
          writeKMLMarker(fid,lat,lon,heading)
      elif(region == dMunich):
        if( (lon <= 13) and (lon >= 11) and (lat <= 50) and (lat >= 47) ):
          writeKMLMarker(fid,lat,lon,heading)
      # ToDo - Add Tokyo support

    # Close the KML file
    fid.write('</Document> </kml>')