source: tmcsimulator/trunk/src/cptms/prep_postmiles.py @ 284

import collections,math,sys
# Scan the postmile file to preprocess it to identify N/S pairs
# that are so close they will overlap in the display. 
# Compute the perpendicular vector that will be used to adjust their position.
# Input filename is a Output to stdout.
# This program adds one extra column to the output, a color, to allow the
# results to be easily converted to json and displayed for visual verification. 
# (See the convert csv to json bash/awk script)
# Before being used in the simulation, remove the last column:
#     cut -f1-6 -d"," output.txt

# jdalbey Feb 2019
postmileFile = "d12_vds_uniq_sorted.csv"

# Helper function to find the perpendicular unit vector to the line
# between two postmiles.
def findPerpX(ax,ay,bx,by):
    dx = float(bx) - float(ax);
    dy = float(by) - float(ay);
    
    dist = math.sqrt(dx * dx + dy * dy);
    try:
        normX = dy / dist;  # calc a unit vector
        normY = -dx / dist;
        return round(normX, 6)
    except ZeroDivisionError:
        print "Oops, (",ay,",",ax,") appeared twice,"
        print "causing findPerp() to divide by zero. Probable cause: duplicates in input"
        print "Please correct the input file."
        sys.exit(-1)
# And same for Y ... I know it's redundant code.
def findPerpY(ax,ay,bx,by):
    dx = float(bx) - float(ax);
    dy = float(by) - float(ay);
    
    dist = math.sqrt(dx * dx + dy * dy);
    
    normX = dy / dist;  # calc a unit vector
    normY = -dx / dist;
    return round(normY, 6)

orientationLookup = {'N':0,'S':1,'E':0,'W':1}

def loadHighways():

#    f = open("pm_coords_uniq_sorted.csv",'r')
    f = open(postmileFile,'r')
    lines = [line.split(',') for line in f.readlines()]
    
    # Create a set containing just the route numbers 
    routeNums = set()
    for item in lines:
        routeNums.add(int(item[0]))
    # put the route numbers in order
    sortedRoutes = sorted (routeNums)    
    # Create the empty postmile collections
    for route in sortedRoutes:
        #print route,
        highways[str(route)]=[collections.OrderedDict(),collections.OrderedDict()]
    #print
    # Process all the data, placing it in proper route and collection
    for item in lines:
        route = item[0]
        orientation = orientationLookup[item[1]]
        postmileItem = item[2]
        highways[route][orientation][postmileItem]=item


def dumpHighways():
    # Dump the highways data we've organized
    for item in highways:
        for cnt in [0,1]:
            list1 = highways[item][cnt]
            print "highway",item,list1
            # show fields for one entry
            for pm_entry in list1:
                print pm_entry,list1[pm_entry]

def calcPerpendicularVectors(theList,thePerps,dirSign):
    size = len(theList)
    idx = 1
    while (idx < size-1):
        # see which is closer, previous or next
        a = abs(float(theList[idx][2]) - float(theList[idx+1][2]))
        b = abs(float(theList[idx][2]) - float(theList[idx-1][2]))
        if ( a<b ):
            #print "closest to ",theList[idx][1]+theList[idx][2]," is ", theList[idx+1][2],
            ax=theList[idx][4] #long
            ay=theList[idx][3] #lat
            bx=theList[idx+1][4]
            by=theList[idx+1][3]
            px = findPerpX(ax,ay,bx,by) * dirSign
            py = findPerpY(ax,ay,bx,by) * dirSign
            # TODO: ADd check to see if this pm already assigned px,py
            thePerps[theList[idx][2]] = [px,py]
            thePerps[theList[idx+1][2]] = [px,py]
            #print px,py
        else:
            #print ">closest to ",theList[idx][2]," is ", theList[idx-1][2],
            ax=theList[idx][4]
            ay=theList[idx][3]
            bx=theList[idx-1][4]
            by=theList[idx-1][3]
            px = findPerpX(bx,by,ax,ay) * dirSign # reverse order so normal stays NB
            py = findPerpY(bx,by,ax,ay) * dirSign
            # TODO: ADd check to see if this pm already assigned px,py
            thePerps[theList[idx][2]] = [px,py]
            thePerps[theList[idx-1][2]] = [px,py]
            #print px,py
        idx += 1

    # Did first and last spots get filled?
    if theList[0][2] in thePerps:
        #print "good, the first item ",theList[0][2]," is present"
        pass
    else:
        #print "oops, first item ",theList[0][2]," missing"
        # I'm too lazy to calc this value, so providing zero meaning no perp vector
        # Which means if by chance this spot has a "mate" then it won't get adjusted
        # as it should.  TODO: fix this by using the neighbor as adjacent
        thePerps[theList[0][2]]=[0,0]
    if theList[idx][2] in thePerps:
        #print "good, the last item ",theList[idx][2]," is present"
        pass
    else:
        #print "oops, last item ",theList[idx][2]," is missing"
        thePerps[theList[idx][2]]=[0,0]

# ------------------------------------------------------------------------------------------

highways = collections.OrderedDict()    
loadHighways()


# Iterate over all the highway routes
for route in highways:
    #print "Starting route: ",route
    # ---------------------------------------------------------------------------------
    # ## First, compute the perpendicular vectors for each item 
    # Compute nearest adjacent for each item (in SAME direction)
    # We create separate north/south lists so it's easier to locate an adjacent spot
    northlist = list (highways[route][0].values())
    southlist = list (highways[route][1].values())
    northPerps = {}  # a dictionary addressed by postmile that yields perp vectors
    southPerps = {}
    #print "northsize is ",northSize, "southsize is ",southSize
    calcPerpendicularVectors(northlist,northPerps, +1)
    calcPerpendicularVectors(southlist,southPerps, -1)
    
    # -------------------------------------------------------------------------------
    #print "*****"
    #print "Perps computed for these up pm's:",sorted(northPerps)
    #print "Perps computed for these down pm's:",sorted (southPerps)
    
    # Try to find matching pairs
    # Create a match list and add matching postmiles to it.
    north = highways[route][0]
    south = highways[route][1]
    matches = []
    for item in north:
        # if south ALSO has item add i to matches list
        if item in south:
            #print "match for: " + item
            matches.append(item)
    
    #print "found ",len(matches)," matches."
    #outFile = open("pairedDots.json","w") # put results here
    #[0xFF0000,0xFF4000,0xFF8000,0xFFBF00,0xFFFF00,0x00FF00,0x00FFFF,0x0080FF,0x0000FF,0x8000FF,0xFF00FF]
    colorcode = ["red","salmon","deeppink","coral","orangered","yellow","khaki","purple",
    "slateblue","lime","lightgreen","cyan","blue","slategray"]
    colorindex=0
    for match in matches:
        # We want to output these as json with matching color
        id = south[match][0] + " " + south[match][1] + " " + south[match][2]
        print ("%s,%s,%s,%s,%s,%s,%s" % (id,south[match][3],south[match][4],south[match][5].rstrip(),southPerps[south[match][2]][0],southPerps[south[match][2]][1],colorcode[colorindex]))
        # lookup perpvector for postmile = south[match][2]
        #perpx = southPerps[south[match][2]][0]
        #perpy = southPerps[south[match][2]][1]
        #print perpx, perpy
        
        id = north[match][0] + " " + north[match][1] + " " + north[match][2]
        print ("%s,%s,%s,%s,%s,%s,%s" % (id,north[match][3],north[match][4],north[match][5].rstrip(),northPerps[north[match][2]][0],northPerps[north[match][2]][1],colorcode[colorindex]))
        colorindex = (colorindex+1) % 14 # advance to next color
        # remove them from future consideration
        south.pop(match)
        north.pop(match)
        

    # -----------------------------------------------------------------------
    leftover_count = len(north)+len(south)
    #print "Leftover count:",leftover_count
    # After we've handled all the "matching" pairs of N/S dots
    # There will be "leftover" single dots 
    if (leftover_count > 0):
        #print "Leftovers ... single dots"
        #print len(north),"North keys", north.keys()
        #print len(south),"South keys", south.keys()
        
         
        # Assert: there are no matching keys in the two dictionaries,
        #         they should have been removed by the previous step.
        # Merge the two sets of keys into one list,
        # each entry is the postmile and direction
        mergedKeys = []
        upLetter = ''
        downLetter = ''
        if len(north) > 0:
            upLetter = north.items()[0][1][1]
            for item in north.keys():
                mergedKeys.append(item + upLetter)
            #print "north keys after merging and letter assigned:",mergedKeys
        if len(south) > 0:
            downLetter = south.items()[0][1][1]
            for item in south.keys():
                mergedKeys.append(item + downLetter)
        
        
        #Sort the list of keys in ascending order by postmile
        leftovers = sorted(mergedKeys)
        # Create a dictionary of postmiles and assigned color, and init to white
        pm_colors=collections.OrderedDict()
        for pm in leftovers:
            pm_colors[pm] = "white"
        
        # Look for adjacent items close together in opposite directions
        # Give them same color dots and assign perpendicular vectors
        close_count = 0
        prev = leftovers.pop(0)
        prev_dir = prev[-1:]
        prev_pm = prev[:-1]
        for curr in leftovers:
            curr_dir = curr[-1:]
            curr_pm  = curr[:-1]
            # Only consider adjacent items in OPPOSITE directions
            if curr_dir != prev_dir:
                curr_color = "white"
                # See if they are close enough to be considered a pair
                if (abs(float(curr_pm) - float(prev_pm)) <= 0.111):
                    #print "FOUND CLOSE: ",prev, curr
                    close_count += 1
                    # tag the previous item with a colored dot
                    pm_colors[curr_pm+curr_dir]="lime"
                    pm_colors[prev_pm+prev_dir]="lime"
               
            prev = curr
            prev_dir = curr_dir
            prev_pm = curr_pm
                
        #print "Counted ",close_count," close pairs"
        # print all the tagged items as csv with their tagged color
        for spot in pm_colors:
            curr_dir = spot[-1:]
            curr_pm  = spot[:-1]
            if (curr_dir == upLetter):
                curr_info = north[curr_pm]
                curr_perpx = northPerps[north[curr_pm][2]][0]
                curr_perpy = northPerps[north[curr_pm][2]][1]
            else:
                curr_info = south[curr_pm]
                curr_perpx = southPerps[south[curr_pm][2]][0]
                curr_perpy = southPerps[south[curr_pm][2]][1]
            id = curr_info[0] + " " + curr_info[1] + " " + curr_info[2]
            # white dots have no mate so don't need to be adjusted
            if (pm_colors[spot]=="white"):
                px=0
                py=0
            else:
                px=curr_perpx
                py=curr_perpy
            # output this spot 
            print ("%s,%s,%s,%s,%s,%s,%s" % (id,curr_info[3],curr_info[4],curr_info[5].rstrip(),px,py,pm_colors[curr_pm+curr_info[1]]))