from __future__ import print_function

import os
import pickle
import socket
from multiprocessing.pool import Pool

import bintrees
# import cv
import geojson
import math
import numpy as np
import scipy.misc
import scipy.ndimage
import sys
import zlib
# import cv2
from PIL import Image
from geojson import LineString, Feature, FeatureCollection
from rtree import index
from scipy import interpolate

from PathSimilarity import PathSimilarity


# This is not needed
# import os
# sys.path.append(os.path.dirname(sys.path[0])+"/MLInputGen/")
# import mapdriver as md


def GetClassifierResult(img, host="localhost", port=8001):
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.connect((host, port))

    data_string = pickle.dumps(img)

    data_string = zlib.compress(data_string, 6)

    print(len(data_string))
    print("Compress", len(zlib.compress(data_string, 6)))

    data_length = pickle.dumps(len(data_string))
    s.send(data_length)

    ack = s.recv(4096)

    s.send(data_string)

    result_string = s.recv(4096)
    result = pickle.loads(result_string)

    return result


def Coord2Pixels(lat, lon, min_lat, min_lon, max_lat, max_lon, sizex, sizey):
    print(max_lat, min_lat, sizex)
    ilat = sizex - int((lat - min_lat) / ((max_lat - min_lat) / sizex))
    ilat = int((lat - min_lat) / ((max_lat - min_lat) / sizex))
    ilon = int((lon - min_lon) / ((max_lon - min_lon) / sizey))

    return ilat, ilon


def TraceQueryBatch(data, host="localhost", port=8006):
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.connect((host, port))
    data_string = ""
    for i in range(len(data) / 5):
        data_string = data_string + str(data[i * 5 + 0]) + "," + str(data[i * 5 + 1]) + "," + str(data[i * 5 + 2]) + "," + str(
            data[i * 5 + 3]) + "," + str(data[i * 5 + 4]) + ","

    print("DataSentLen", len(data_string))

    s.send(data_string)
    result = s.recv(16384)
    items = result.split()
    result = [int(item) for item in items]
    print(result)

    return result


def TraceQueryBatch3P(data, host="localhost", port=8002):
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.connect((host, port))
    data_string = ""
    for i in range(len(data) / 9):
        data_string = data_string + str(data[i * 9 + 0]) + "," + str(data[i * 9 + 1]) + "," + str(data[i * 9 + 2]) + "," + str(
            data[i * 9 + 3]) + "," + str(data[i * 9 + 4]) + "," + str(data[i * 9 + 5]) + "," + str(data[i * 9 + 6]) + "," + str(
            data[i * 9 + 7]) + "," + str(data[i * 9 + 8]) + ",-1,-1,"

    print("DataSentLen", len(data_string))

    s.send(data_string)
    result = s.recv(16384)
    items = result.split()
    result = [int(item) for item in items]
    print(result)

    return result


def SpeedQueryBatch(data, host="localhost", port=8002):
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.connect((host, port))
    data_string = "S,"
    for i in range(len(data) / 4):
        data_string = data_string + str(data[i * 4 + 0]) + "," + str(data[i * 4 + 1]) + "," + str(data[i * 4 + 2]) + "," + str(
            data[i * 4 + 3]) + ","

    print("DataSentLen", len(data_string))

    s.send(data_string)
    result = s.recv(16384)
    items = result.split()
    result = [float(item) for item in items]
    print(result)

    return result


def distance(p1, p2):
    a = p1[0] - p2[0]
    b = (p1[1] - p2[1]) * math.cos(math.radians(p1[0]))
    return np.sqrt(a * a + b * b)


def pointDistance(p1, p2):
    a = p1[0] - p2[0]
    b = p1[1] - p2[1]
    return np.sqrt(a * a + b * b)


def latlonNorm(p1, lat=40):
    p11 = p1[1] * math.cos(math.radians(lat))

    l = np.sqrt(p11 * p11 + p1[0] * p1[0])

    return p1[0] / l, p11 / l


def simplfyWithShortestPathBatchStatic(self, sourceIDs, state=1, idx=0):
    print(idx, sourceIDs)
    results = []
    c = 0
    for sourceID in sourceIDs:
        print(state, idx, c, len(sourceIDs))
        results.append(simplfyWithShortestPathStatic(self, sourceID, state=state))
        c = c + 1

    return results


def simplfyWithShortestPathStatic(self, sourceID, state=1, deferred_apply=False):
    self.nodeDistance = {}
    self.nodeFrom = {}
    self.nodeSettle = {}

    result = {}

    result['edgePairs'] = {}

    result['locations'] = {}

    tree = bintrees.BinaryTree()

    for nid in self.nodes.keys():
        self.nodeDistance[nid] = 10000000000
        self.nodeFrom[nid] = -1
        self.nodeSettle[nid] = 0

    tree[sourceID] = 0
    self.nodeDistance[sourceID] = 0

    # for i in range(len(self.nodes.keys())):
    while True:
        dmin = 10000000
        dminid = -1

        # for j in self.nodes.keys():
        # 	if self.nodeDistance[j] < dmin and self.nodeSettle[j] == 0:
        # 		dmin = self.nodeDistance[j]
        # 		dminid = j

        # if dminid == -1 :
        # 	break
        if len(tree) == 0:
            break

        dminid = tree.min_key()
        dmin = tree[dminid]

        tree.remove_items([dminid])
        # print(dminid, dmin)
        # self.nodeSettle[dminid] = 1
        for nextNode in self.nodeLink[dminid]:
            lat1 = self.nodes[dminid][0]
            lon1 = self.nodes[dminid][1]

            lat2 = self.nodes[nextNode][0]
            lon2 = self.nodes[nextNode][1]

            a = lat1 - lat2
            b = (lon1 - lon2) / math.cos(math.radians(lat1))

            dist = np.sqrt(a * a + b * b)

            if self.nodeDistance[nextNode] > dmin + dist:
                self.nodeDistance[nextNode] = dmin + dist
                self.nodeFrom[nextNode] = dminid
                tree[nextNode] = dmin + dist

    print("Tree Size", len(tree))

    if state == 1:
        # for i in range(len(self.nodes.keys())):
        for idthis in self.nodeTerminate.keys():
            i = self.nodeHash[idthis]
            if self.nodeFrom[i] != -1:
                cur = i
                path = []
                while True:
                    path.append(cur)
                    p = self.nodeFrom[cur]

                    if (p, cur) not in result['edgePairs'].keys():
                        result['edgePairs'][(p, cur)] = 1
                    else:
                        result['edgePairs'][(p, cur)] += 1

                    if deferred_apply == False:
                        self.edgeScore[self.edgeHash[p * 10000000 + cur]] += 1

                        self.nodeScore[p] = max(self.nodeScore[p], self.edgeScore[self.edgeHash[p * 10000000 + cur]])
                        self.nodeScore[cur] = max(self.nodeScore[cur], self.edgeScore[self.edgeHash[p * 10000000 + cur]])

                    cur = p

                    if cur == sourceID:
                        break

                path.reverse()  # From root...

                # for j in range(2, len(path)-1, 20):
                # 	self.nodeLocations[path[j]] = 1

                for j in range(len(path)):
                    if path[j] % 20 == 0:
                        if deferred_apply == False:
                            self.nodeLocations[path[j]] = 1
                        result['locations'][path[j]] = 1

                if deferred_apply == False:
                    self.nodeLocations[self.nodeHash[idthis]] = 1
                result['locations'][self.nodeHash[idthis]] = 1

        if deferred_apply == False:
            self.nodeLocations[sourceID] = 1
        result['locations'][sourceID] = 1

    if state == 2:
        for idthis in self.nodeLocations.keys():
            i = idthis
            if self.nodeFrom[i] != -1:
                cur = i
                path = []
                while True:
                    path.append(cur)
                    p = self.nodeFrom[cur]

                    if (p, cur) not in result['edgePairs'].keys():
                        result['edgePairs'][(p, cur)] = 1
                    else:
                        result['edgePairs'][(p, cur)] += 1

                    if deferred_apply == False:
                        self.edgeScore[self.edgeHash[p * 10000000 + cur]] += 1

                        self.nodeScore[p] = max(self.nodeScore[p], self.edgeScore[self.edgeHash[p * 10000000 + cur]])
                        self.nodeScore[cur] = max(self.nodeScore[cur], self.edgeScore[self.edgeHash[p * 10000000 + cur]])

                    cur = p

                    if cur == sourceID:
                        break

    return result


class RoadGraph:
    def __init__(self, filename=None, skip_threshold=0.0):
        self.nodeHash = {}  # [tree_idx*10000000 + local_id] ->  id
        self.nodeHashReverse = {}
        self.nodes = {}  # id -> [lat,lon]
        self.edges = {}  # id -> [n1, n2]
        self.nodeLink = {}  # id -> list of next node
        self.nodeID = 0
        self.edgeID = 0
        self.edgeHash = {}  # [nid1 * 10000000 + nid2] -> edge id
        self.edgeScore = {}
        self.nodeTerminate = {}
        self.nodeScore = {}
        self.nodeLocations = {}
        self.edgeInt = {}
        self.deletedNodes = {}

        if filename is not None:

            dumpDat = pickle.load(open(filename, "rb"))

            forest = dumpDat[1]
            region = dumpDat[0]
            self.region = region

            # self.forest = forest

            self.NumOfTrees = len(forest)
            self.forest = [1 for _ in range(self.NumOfTrees)]

            tid = 0
            for t in forest:
                for n in t:
                    idthis = tid * 10000000 + n['id']

                    thislat = n['lat']
                    thislon = n['lon']

                    if region is not None:
                        if thislat < region[0] or thislon < region[1] or thislat > region[2] or thislon > region[3]:
                            continue

                    if n['edgeScore'] < skip_threshold:  # skip those low confidential edges

                        continue

                    if n['similarWith'][0] != -1:
                        idthis = n['similarWith'][0] * 10000000 + n['similarWith'][1]

                        thislat = forest[n['similarWith'][0]][n['similarWith'][1]]['lat']
                        thislon = forest[n['similarWith'][0]][n['similarWith'][1]]['lon']

                    if n['OutRegion'] == 1:
                        self.nodeTerminate[tid * 10000000 + n['parent']] = 1

                    if n['isExited'] == True:
                        self.nodeTerminate[tid * 10000000 + n['parent']] = 1

                    idparent = tid * 10000000 + n['parent']
                    parentlat = t[n['parent']]['lat']
                    parentlon = t[n['parent']]['lon']

                    if n['parent'] == 0:
                        print(tid, n['id'])

                    self.addEdge(idparent, parentlat, parentlon, idthis, thislat, thislon)

                tid += 1

        self.ReverseDirectionLink()

    def addEdge(self, nid1, lat1, lon1, nid2, lat2, lon2, reverse=False, nodeScore1=0, nodeScore2=0, edgeScore=0):  # n1d1->n1d2

        if nid1 not in self.nodeHash.keys():
            self.nodeHash[nid1] = self.nodeID
            self.nodeHashReverse[self.nodeID] = nid1
            self.nodes[self.nodeID] = [lat1, lon1]
            self.nodeLink[self.nodeID] = []
            # self.nodeLinkReverse[self.nodeID] = []
            self.nodeScore[self.nodeID] = nodeScore1
            self.nodeID += 1

        if nid2 not in self.nodeHash.keys():
            self.nodeHash[nid2] = self.nodeID
            self.nodeHashReverse[self.nodeID] = nid2
            self.nodes[self.nodeID] = [lat2, lon2]
            self.nodeLink[self.nodeID] = []
            # self.nodeLinkReverse[self.nodeID] = []
            self.nodeScore[self.nodeID] = nodeScore2
            self.nodeID += 1

        localid1 = self.nodeHash[nid1]
        localid2 = self.nodeHash[nid2]

        if localid1 * 10000000 + localid2 in self.edgeHash.keys():
            print("Duplicated Edge !!!", nid1, nid2)

            return self.edgeHash[localid1 * 10000000 + localid2]

        self.edges[self.edgeID] = [localid1, localid2]
        self.edgeHash[localid1 * 10000000 + localid2] = self.edgeID
        self.edgeScore[self.edgeID] = edgeScore
        self.edgeID += 1

        if localid2 not in self.nodeLink[localid1]:
            self.nodeLink[localid1].append(localid2)

        if reverse == True:
            if localid2 not in self.nodeLinkReverse.keys():
                self.nodeLinkReverse[localid2] = []

            if localid1 not in self.nodeLinkReverse[localid2]:
                self.nodeLinkReverse[localid2].append(localid1)

        return self.edgeID - 1

    def addEdgeToOneExistedNode(self, nid1, lat1, lon1, nid2, reverse=False, nodeScore1=0, edgeScore=0):  # n1d1->n1d2

        if nid1 not in self.nodeHash.keys():
            self.nodeHash[nid1] = self.nodeID
            self.nodeHashReverse[self.nodeID] = nid1
            self.nodes[self.nodeID] = [lat1, lon1]
            self.nodeLink[self.nodeID] = []
            self.nodeLinkReverse[self.nodeID] = []
            self.nodeScore[self.nodeID] = nodeScore1
            self.nodeID += 1

        localid1 = self.nodeHash[nid1]
        localid2 = nid2

        self.edges[self.edgeID] = [localid1, localid2]
        self.edgeHash[localid1 * 10000000 + localid2] = self.edgeID
        self.edgeScore[self.edgeID] = edgeScore
        self.edgeID += 1

        if localid2 not in self.nodeLink[localid1]:
            self.nodeLink[localid1].append(localid2)

        if localid1 not in self.nodeLinkReverse[localid2]:
            self.nodeLinkReverse[localid2].append(localid1)

    def connectTwoNodes(self, n1, n2):
        lat1 = self.nodes[n1][0]
        lon1 = self.nodes[n1][1]

        lat2 = self.nodes[n2][0]
        lon2 = self.nodes[n2][1]

        nn1 = self.nodeHashReverse[n1]
        nn2 = self.nodeHashReverse[n2]

        return self.addEdge(nn1, lat1, lon1, nn2, lat2, lon2)

    def applyShortestPathResult(self, edgePairs, location):
        for ep in edgePairs.keys():
            p = ep[0]
            cur = ep[1]

            self.edgeScore[self.edgeHash[p * 10000000 + cur]] += 1

            self.nodeScore[p] = max(self.nodeScore[p], self.edgeScore[self.edgeHash[p * 10000000 + cur]])
            self.nodeScore[cur] = max(self.nodeScore[cur], self.edgeScore[self.edgeHash[p * 10000000 + cur]])

        for loc in location.keys():
            self.nodeLocations[loc] = 1

    def simplfyWithShortestPathParallel(self, ProcessNum=12):

        pool = Pool(ProcessNum)

        sourceIDlist = [[] for i in range(ProcessNum)]

        c = 0
        print(len(self.forest))
        for i in range(len(self.forest)):
            # print(i,"/", len(roadMap.forest))
            if i * 10000000 in self.nodeHash.keys():
                # print(i)
                sourceIDlist[c].append(self.nodeHash[i * 10000000])

                c = c + 1
                if c >= ProcessNum:
                    c = 0

        args = []
        for i in range(ProcessNum):
            args.append((self, sourceIDlist[i], 1, i))

        print(args)

        result = []

        for i in range(ProcessNum):
            result.append(pool.apply_async(simplfyWithShortestPathBatchStatic, args=args[i]))

        for i in range(ProcessNum):
            r = result[i].get()

            for rr in r:
                self.applyShortestPathResult(rr['edgePairs'], rr['locations'])

        sourceIDlist = [[] for i in range(ProcessNum)]

        cc = 0
        for i in self.nodeLocations:
            sourceIDlist[cc].append(i)

            cc = cc + 1
            if cc >= ProcessNum:
                cc = 0

        args = []
        for i in range(ProcessNum):
            args.append((self, sourceIDlist[i], 2, i))

        result = []

        for i in range(ProcessNum):
            result.append(pool.apply_async(simplfyWithShortestPathBatchStatic, args=args[i]))

        for i in range(ProcessNum):
            r = result[i].get()

            for rr in r:
                self.applyShortestPathResult(rr['edgePairs'], rr['locations'])

    # @staticmethod
    def simplfyWithShortestPathBatch(self, sourceIDs, state=1, idx=0):

        results = []
        c = 0
        for sourceID in sourceIDs:
            print(state, idx, c, len(sourceIDs))
            results.append(self.simplfyWithShortestPath(sourceID, state=state))
            c = c + 1

        return results

    # @staticmethod
    def simplfyWithShortestPath(self, sourceID, state=1, deferred_apply=False):
        self.nodeDistance = {}
        self.nodeFrom = {}
        self.nodeSettle = {}

        result = {}

        result['edgePairs'] = {}

        result['locations'] = {}

        tree = bintrees.BinaryTree()

        for nid in self.nodes.keys():
            self.nodeDistance[nid] = 10000000000
            self.nodeFrom[nid] = -1
            self.nodeSettle[nid] = 0

        tree[sourceID] = 0
        self.nodeDistance[sourceID] = 0

        # for i in range(len(self.nodes.keys())):
        while True:
            dmin = 10000000
            dminid = -1

            # for j in self.nodes.keys():
            # 	if self.nodeDistance[j] < dmin and self.nodeSettle[j] == 0:
            # 		dmin = self.nodeDistance[j]
            # 		dminid = j

            # if dminid == -1 :
            # 	break
            if len(tree) == 0:
                break

            dminid = tree.min_key()
            dmin = tree[dminid]

            tree.remove_items([dminid])

            # print(dminid, dmin)

            # self.nodeSettle[dminid] = 1

            for nextNode in self.nodeLink[dminid]:
                lat1 = self.nodes[dminid][0]
                lon1 = self.nodes[dminid][1]

                lat2 = self.nodes[nextNode][0]
                lon2 = self.nodes[nextNode][1]

                a = lat1 - lat2
                b = (lon1 - lon2) / math.cos(math.radians(lat1))

                dist = np.sqrt(a * a + b * b)

                if self.nodeDistance[nextNode] > dmin + dist:
                    self.nodeDistance[nextNode] = dmin + dist
                    self.nodeFrom[nextNode] = dminid
                    tree[nextNode] = dmin + dist

        print("Tree Size", len(tree))

        if state == 1:
            # for i in range(len(self.nodes.keys())):
            for idthis in self.nodeTerminate.keys():
                i = self.nodeHash[idthis]
                if self.nodeFrom[i] != -1:
                    cur = i
                    path = []
                    while True:
                        path.append(cur)
                        p = self.nodeFrom[cur]

                        if (p, cur) not in result['edgePairs'].keys():
                            result['edgePairs'][(p, cur)] = 1
                        else:
                            result['edgePairs'][(p, cur)] += 1

                        if deferred_apply == False:
                            self.edgeScore[self.edgeHash[p * 10000000 + cur]] += 1

                            self.nodeScore[p] = max(self.nodeScore[p], self.edgeScore[self.edgeHash[p * 10000000 + cur]])
                            self.nodeScore[cur] = max(self.nodeScore[cur], self.edgeScore[self.edgeHash[p * 10000000 + cur]])

                        cur = p

                        if cur == sourceID:
                            break

                    path.reverse()  # From root...

                    # for j in range(2, len(path)-1, 20):
                    # 	self.nodeLocations[path[j]] = 1

                    for j in range(len(path)):
                        if path[j] % 20 == 0:
                            if deferred_apply == False:
                                self.nodeLocations[path[j]] = 1
                            result['locations'][path[j]] = 1

                    if deferred_apply == False:
                        self.nodeLocations[self.nodeHash[idthis]] = 1
                    result['locations'][self.nodeHash[idthis]] = 1

            if deferred_apply == False:
                self.nodeLocations[sourceID] = 1
            result['locations'][sourceID] = 1

        if state == 2:
            for idthis in self.nodeLocations.keys():
                i = idthis
                if self.nodeFrom[i] != -1:
                    cur = i
                    path = []
                    while True:
                        path.append(cur)
                        p = self.nodeFrom[cur]

                        if (p, cur) not in result['edgePairs'].keys():
                            result['edgePairs'][(p, cur)] = 1
                        else:
                            result['edgePairs'][(p, cur)] += 1

                        if deferred_apply == False:
                            self.edgeScore[self.edgeHash[p * 10000000 + cur]] += 1

                            self.nodeScore[p] = max(self.nodeScore[p], self.edgeScore[self.edgeHash[p * 10000000 + cur]])
                            self.nodeScore[cur] = max(self.nodeScore[cur], self.edgeScore[self.edgeHash[p * 10000000 + cur]])

                        cur = p

                        if cur == sourceID:
                            break

        return result

    def mergeTwoNodesBiDirection(self, id1, id2):  # remove id2
        if id1 == id2:
            return

        print("id1, id2", id1, id2)

        print(self.nodeLink[id2])
        for next_node in self.nodeLink[id2]:

            # remove links
            print(next_node, id2, self.nodeLink[next_node], self.nodeLink[id1])
            self.nodeLink[next_node].remove(id2)

            edgeId = self.edgeHash[next_node * 10000000 + id2]
            del self.edges[edgeId]

            edgeId = self.edgeHash[id2 * 10000000 + next_node]
            del self.edges[edgeId]

            # print("Deleted ", self.deletedNodes.keys())
            # Add to id1's nodeLink list
            if (next_node not in self.nodeLink[id1]) and (next_node not in self.deletedNodes.keys()) and (next_node != id1):
                self.nodeLink[id1].append(next_node)
                print(id1, "+", next_node)

                localid1 = id1
                localid2 = next_node

                self.edges[self.edgeID] = [localid1, localid2]
                self.edgeHash[localid1 * 10000000 + localid2] = self.edgeID
                self.edgeScore[self.edgeID] = 10
                self.edgeID += 1

            # Add id1 to next node's nodeLink
            if id1 not in self.nodeLink[next_node] and next_node != id1:
                self.nodeLink[next_node].append(id1)

                localid1 = next_node
                localid2 = id1

                self.edges[self.edgeID] = [localid1, localid2]
                self.edgeHash[localid1 * 10000000 + localid2] = self.edgeID
                self.edgeScore[self.edgeID] = 10
                self.edgeID += 1

        pass

    def mergeTwoNodes(self, id1, id2):  # remove id2
        if id1 == id2:
            return

        self.nodes[id1][0] = (self.nodes[id1][0] + self.nodes[id2][0]) / 2
        self.nodes[id1][1] = (self.nodes[id1][1] + self.nodes[id2][1]) / 2

        print("id1, id2", id1, id2)

        print(self.nodeLink[id2])

        for next_node in self.nodeLink[id2]:
            self.nodeLinkReverse[next_node].remove(id2)

            edgeId = self.edgeHash[id2 * 10000000 + next_node]
            del self.edges[edgeId]

            if (next_node not in self.nodeLink[id1]) and (next_node not in self.deletedNodes.keys()) and (next_node != id1):
                self.nodeLink[id1].append(next_node)

                localid1 = id1
                localid2 = next_node

                self.edges[self.edgeID] = [localid1, localid2]
                self.edgeHash[localid1 * 10000000 + localid2] = self.edgeID
                self.edgeScore[self.edgeID] = 10
                self.edgeID += 1

            if id1 not in self.nodeLinkReverse[next_node] and next_node != id1:
                self.nodeLinkReverse[next_node].append(id1)

        for next_node in self.nodeLinkReverse[id2]:
            self.nodeLink[next_node].remove(id2)

            edgeId = self.edgeHash[next_node * 10000000 + id2]
            del self.edges[edgeId]

            if (next_node not in self.nodeLinkReverse[id1]) and (next_node not in self.deletedNodes.keys()) and (next_node != id1):
                self.nodeLinkReverse[id1].append(next_node)

                localid1 = next_node
                localid2 = id1

                self.edges[self.edgeID] = [localid1, localid2]
                self.edgeHash[localid1 * 10000000 + localid2] = self.edgeID
                self.edgeScore[self.edgeID] = 10
                self.edgeID += 1

            if id1 not in self.nodeLink[next_node] and next_node != id1:
                self.nodeLink[next_node].append(id1)

        pass

    def mergeTwoNodesReserveEdge(self, id1, id2):  # remove id2
        if id1 == id2:
            return

        self.nodes[id1][0] = (self.nodes[id1][0] + self.nodes[id2][0]) / 2
        self.nodes[id1][1] = (self.nodes[id1][1] + self.nodes[id2][1]) / 2

        print("id1, id2", id1, id2)

        print(self.nodeLink[id2])

        for next_node in self.nodeLink[id2]:
            self.nodeLinkReverse[next_node].remove(id2)

            edgeId = self.edgeHash[id2 * 10000000 + next_node]
            self.edges[edgeId][0] = id1
            self.edgeHash[id1 * 10000000 + next_node] = edgeId
            del self.edgeHash[id2 * 10000000 + next_node]

            if (next_node not in self.nodeLink[id1]) and (next_node not in self.deletedNodes.keys()) and (next_node != id1):
                self.nodeLink[id1].append(next_node)

            if id1 not in self.nodeLinkReverse[next_node] and next_node != id1:
                self.nodeLinkReverse[next_node].append(id1)

        for next_node in self.nodeLinkReverse[id2]:
            self.nodeLink[next_node].remove(id2)

            edgeId = self.edgeHash[next_node * 10000000 + id2]
            self.edges[edgeId][1] = id1
            self.edgeHash[next_node * 10000000 + id1] = edgeId
            del self.edgeHash[next_node * 10000000 + id2]

            if (next_node not in self.nodeLinkReverse[id1]) and (next_node not in self.deletedNodes.keys()) and (next_node != id1):
                self.nodeLinkReverse[id1].append(next_node)

            if id1 not in self.nodeLink[next_node] and next_node != id1:
                self.nodeLink[next_node].append(id1)

        pass

    def removeDuplicateEdges(self):
        edges = {}
        c = 0
        for edgeID in self.edges.keys():
            if (self.edges[edgeID][0], self.edges[edgeID][1]) in edges.keys():
                del self.edges[edgeID]
                c += 1
            else:
                edges[(self.edges[edgeID][0], self.edges[edgeID][1])] = edgeID

        for edge, edgeid in edges.iteritems():
            self.edgeHash[edge[0] * 10000000 + edge[1]] = edgeid

        print("Remove", c, "Duplicated Edges")

    def getPath(self, id1, length=6, limit=40):

        paths = []
        path_tmp = list(range(length))

        def searchPath(idthis, depth, pid=-1):
            # global paths
            # global path_tmp
            # global length

            if len(paths) > limit:
                return

            path_tmp[length - depth] = idthis

            if depth == 1:
                p = map(lambda x: [self.nodes[x][0], self.nodes[x][1], x], path_tmp)
                paths.append(p)
                return

            for next_node in self.nodeLink[idthis]:
                ok = True

                if next_node == pid:
                    continue

                if next_node not in self.nodeScore.keys():
                    continue

                if self.nodeScore[next_node] < 1:
                    continue

                if self.edgeScore[self.edgeHash[idthis * 10000000 + next_node]] < 1:
                    continue

                searchPath(next_node, depth - 1, pid=idthis)

            pass

        searchPath(id1, length)

        return paths

    def BiDirection(self):
        edgeList = list(self.edges.values())

        for edge in edgeList:
            localid1 = edge[1]
            localid2 = edge[0]

            self.edges[self.edgeID] = [localid1, localid2]
            self.edgeHash[localid1 * 10000000 + localid2] = self.edgeID
            self.edgeScore[self.edgeID] = self.edgeScore[self.edgeHash[localid2 * 10000000 + localid1]]
            self.edgeID += 1

            if localid2 not in self.nodeLink[localid1]:
                self.nodeLink[localid1].append(localid2)

    def ReverseDirectionLink(self):
        edgeList = list(self.edges.values())

        self.nodeLinkReverse = {}

        for edge in edgeList:
            localid1 = edge[1]
            localid2 = edge[0]

            if localid1 not in self.nodeLinkReverse:
                self.nodeLinkReverse[localid1] = [localid2]
            else:
                if localid2 not in self.nodeLinkReverse[localid1]:
                    self.nodeLinkReverse[localid1].append(localid2)

        for nodeId in self.nodes.keys():
            if nodeId not in self.nodeLinkReverse.keys():
                self.nodeLinkReverse[nodeId] = []

    def write_map_to_file(self, map_filename):
        # output that we are starting the writing process
        print("Writing map to file... ")
        sys.stdout.flush()

        node_filename = map_filename + "vertices_RR.txt"
        edge_filename = map_filename + "edges_RR.txt"

        if os.path.exists(node_filename):
            os.remove(node_filename)

        if os.path.exists(edge_filename):
            os.remove(edge_filename)

        # open map file
        node_file = open(node_filename, 'w')

        # iterate through all map nodes
        for k, v in self.nodes.items():
            curr_node_info = str(k) + "," + str(v[1]) + "," + str(v[0]) + "," + str(0)
            node_file.write(curr_node_info + "\n")

        # close node file
        node_file.close()
        edge_file = open(edge_filename, 'w')

        for k, v in self.edges.items():
            edge_file.write(str(v[0]) + "," + str(v[1]) + "\n")

        # close map file
        edge_file.close()
        print("done.")

    def CombineSimilarSnippets(self, region=None, oneround=False, score_threshold=0):
        idx = index.Index()
        for idthis in self.nodes.keys():
            if idthis in self.nodeScore.keys():
                if self.nodeScore[idthis] > score_threshold:
                    if region == None:
                        idx.insert(idthis, (
                            self.nodes[idthis][0], self.nodes[idthis][1], self.nodes[idthis][0] + 0.000001,
                            self.nodes[idthis][1] + 0.000001))
                    elif self.nodes[idthis][0] > region[0] and self.nodes[idthis][1] > region[1] and self.nodes[idthis][0] < region[2] and \
                            self.nodes[idthis][1] < region[3]:
                        idx.insert(idthis, (
                            self.nodes[idthis][0], self.nodes[idthis][1], self.nodes[idthis][0] + 0.000001,
                            self.nodes[idthis][1] + 0.000001))

        # self.deletedNodes = {}

        while True:

            update_counter = 0

            for idthis in self.nodes.keys():
                if idthis in self.deletedNodes.keys():
                    continue

                if idthis in self.nodeScore.keys():
                    if self.nodeScore[idthis] > score_threshold:

                        lat = self.nodes[idthis][0]
                        lon = self.nodes[idthis][1]

                        if region is not None:
                            if lat < region[0] or lon < region[1] or lat > region[2] or lon > region[3]:
                                continue

                        r = 0.00015
                        possible_nodes = list(idx.intersection((lat - r, lon - r, lat + r, lon + r)))

                        paths_this = self.getPath(idthis)

                        best_p1 = []
                        best_p2 = []
                        best_d = 10000
                        best_err = []

                        threshold = 0.00048

                        print(idthis, self.nodeScore[idthis], " PossibleNodes ", len(possible_nodes))

                        def getDistance(item):
                            lat_ = self.nodes[item][0]
                            lon_ = self.nodes[item][1]

                            a = lat_ - lat
                            b = lon_ - lon

                            return np.sqrt(a * a + b * b)

                        possible_nodes = sorted(possible_nodes, key=getDistance)

                        c = 0
                        for possible_node in possible_nodes:
                            if possible_node == idthis:
                                continue

                            if possible_node in self.deletedNodes.keys():
                                continue

                            c = c + 1

                            if c > 3:
                                break

                            paths_target = self.getPath(possible_node)

                            print("Check " + str(len(paths_this) * len(paths_target)) + " path pairs")

                            cc = 0
                            for p1 in paths_this:

                                if cc > 200:
                                    break
                                for p2 in paths_target:
                                    flag = True

                                    cc = cc + 1

                                    d, err = PathSimilarity(p1, p2)

                                    if d < best_d and d < threshold:
                                        best_d = d
                                        best_err = err
                                        best_p1 = p1
                                        best_p2 = p2

                        if best_d < threshold:
                            print(idthis, best_d, best_err, update_counter)
                            print(best_p1)
                            print(best_p2)
                            print(" ")
                            update_counter = update_counter + 1
                            for i in range(len(p1)):

                                # if distance(self.nodes[best_p1[i]], self.nodes[best_p2[i]]) > 0.00020:
                                # 	continue

                                if best_p1[i][2] in self.deletedNodes.keys() and best_p2[i][2] in self.deletedNodes.keys():
                                    continue

                                if best_p1[i][2] in self.deletedNodes.keys() and best_p2[i][2] not in self.deletedNodes.keys():
                                    self.mergeTwoNodes(self.deletedNodes[best_p1[i][2]], best_p2[i][2])
                                    if self.deletedNodes[best_p1[i][2]] != best_p2[i][2]:
                                        self.deletedNodes[best_p2[i][2]] = self.deletedNodes[best_p1[i][2]]
                                else:
                                    self.mergeTwoNodes(best_p1[i][2], best_p2[i][2])
                                    if best_p2[i][2] != best_p1[i][2]:
                                        self.deletedNodes[best_p2[i][2]] = best_p1[i][2]

            print("Deleted ", self.deletedNodes)

            print(update_counter, " Update(s)")
            # break

            if update_counter == 0 or oneround == True:
                break

        return update_counter

    def Smoothen(self, n=3):

        self.edgeInt = {}

        for edgeId, edge in self.edges.iteritems():
            n1 = edge[0]
            n2 = edge[1]

            # self.edgeInt[edgeId] = None
            # continue

            if n1 not in self.nodeLinkReverse:
                self.edgeInt[edgeId] = None
                continue

            n0 = -1

            for nn in self.nodeLinkReverse[n1]:
                if nn not in self.deletedNodes.keys() and self.nodeScore[nn] > 0 and self.edgeScore[self.edgeHash[nn * 10000000 + n1]] > 0:
                    n0 = nn

            if n0 == -1:
                self.edgeInt[edgeId] = None
                continue

            paths = self.getPath(n0, length=4)

            if len(paths) == 0:
                self.edgeInt[edgeId] = None
                continue

            path = None

            for p in paths:
                if p[1][2] == n1 and p[2][2] == n2:
                    path = p

            if path is None:
                self.edgeInt[edgeId] = None
                continue

            X = map(lambda x: x[0], path)
            Y = map(lambda x: x[1], path)

            tck1, u = interpolate.splprep([X, Y], s=0, k=3)

            interval = (u[2] - u[1]) / (n + 1)
            TT = np.arange(u[1] + interval, u[2] - 0.000000001, interval)

            pathInt_ = interpolate.splev(TT, tck1)

            pathInt = map(lambda x: [pathInt_[0][x], pathInt_[1][x]], range(len(pathInt_[0])))

            self.edgeInt[edgeId] = pathInt

        pass

    def TOPOWalk(self, nodeid, step=0.00005, r=0.00300, direction=False):

        localNodeList = {}
        localNodeDistance = {}

        mables = []

        localEdges = {}

        # localNodeList[nodeid] = 1
        # localNodeDistance[nodeid] = 0

        def explore(node_cur, node_prev, dist):

            if node_cur in localNodeList.keys():
                if localNodeDistance[node_cur] <= dist:
                    return

            if dist > r:
                return

            lat1 = self.nodes[node_cur][0]
            lon1 = self.nodes[node_cur][1]

            localNodeList[node_cur] = 1
            localNodeDistance[node_cur] = dist

            # mables.append((lat1, lon1))

            if node_cur not in self.nodeLinkReverse.keys():
                self.nodeLinkReverse[node_cur] = []

            reverseList = []

            if direction == False:
                reverseList = self.nodeLinkReverse[node_cur]

            for next_node in self.nodeLink[node_cur] + reverseList:

                edgeS = 0

                if node_cur * 10000000 + next_node in self.edgeHash.keys():
                    edgeS = self.edgeScore[self.edgeHash[node_cur * 10000000 + next_node]]

                if next_node * 10000000 + node_cur in self.edgeHash.keys():
                    edgeS = max(edgeS, self.edgeScore[self.edgeHash[next_node * 10000000 + node_cur]])

                if self.nodeScore[next_node] > 0 and edgeS > 0:
                    pass
                else:
                    continue

                if next_node == node_prev:
                    continue

                lat0 = 0
                lon0 = 0

                lat1 = self.nodes[node_cur][0]
                lon1 = self.nodes[node_cur][1]

                lat2 = self.nodes[next_node][0]
                lon2 = self.nodes[next_node][1]

                # TODO check angle of next_node

                localEdgeId = node_cur * 10000000 + next_node

                # if localEdgeId not in localEdges.keys():
                # 	localEdges[localEdgeId] = 1

                l = distance((lat2, lon2), (lat1, lon1))
                num = int(math.ceil(l / step))

                bias = step * math.ceil(dist / step) - dist
                cur = bias

                while cur < l:
                    alpha = cur / l
                    # for a in range(1,num):
                    #	alpha = float(a)/num
                    if dist + l * alpha > r:
                        break

                    latI = lat2 * alpha + lat1 * (1 - alpha)
                    lonI = lon2 * alpha + lon1 * (1 - alpha)

                    if (latI, lonI) not in mables:
                        mables.append((latI, lonI))

                    cur += step

                l = distance((lat2, lon2), (lat1, lon1))

                explore(next_node, node_cur, dist + l)

        explore(nodeid, -1, 0)

        return mables

    def removeNode(self, nodeid):
        for next_node in self.nodeLink[nodeid]:
            edgeid = self.edgeHash[nodeid * 10000000 + next_node]

            del self.edges[edgeid]
            del self.edgeScore[edgeid]
            del self.edgeHash[nodeid * 10000000 + next_node]

            if nodeid in self.nodeLinkReverse[next_node]:
                self.nodeLinkReverse[next_node].remove(nodeid)

        for prev_node in self.nodeLinkReverse[nodeid]:
            edgeid = self.edgeHash[prev_node * 10000000 + nodeid]

            del self.edges[edgeid]
            del self.edgeScore[edgeid]
            del self.edgeHash[prev_node * 10000000 + nodeid]

            if nodeid in self.nodeLink[prev_node]:
                self.nodeLink[prev_node].remove(nodeid)

        del self.nodes[nodeid]
        del self.nodeScore[nodeid]
        del self.nodeLink[nodeid]
        del self.nodeLinkReverse[nodeid]

    def removeDeadEnds(self, oneround=False):
        deleted = 0
        for nodeid in self.nodes.keys():
            if self.nodeHashReverse[nodeid] in self.nodeTerminate.keys():
                continue

            if self.nodeHashReverse[nodeid] % 10000000 == 0:
                continue

            d = self.NumOfNeighbors(nodeid)
            if d == 1 or len(self.nodeLink[nodeid]) == 0 or len(self.nodeLinkReverse[nodeid]) == 0:
                self.removeNode(nodeid)
                deleted += 1

        return deleted

    def setScoreThreshold(self, threshold=1):
        self.scoreThreshold = threshold

    def NumOfNeighbors(self, nodeid):
        neighbor = {}

        for next_node in self.nodeLink[nodeid] + self.nodeLinkReverse[nodeid]:
            if next_node != nodeid:
                neighbor[next_node] = 1

        return len(neighbor.keys())

    def getNeighbors(self, nodeid):
        neighbor = {}

        for next_node in self.nodeLink[nodeid] + self.nodeLinkReverse[nodeid]:
            if next_node != nodeid:
                neighbor[next_node] = 1

        return neighbor.keys()

    def removeEdgesByThreshold(self, score_threshold=0.0):
        for edgeid in self.edges.keys():
            if edgeid in self.edgeScore.keys():
                if self.edgeScore[edgeid] > score_threshold:
                    continue

                node1 = self.edges[edgeid][0]
                node2 = self.edges[edgeid][1]

                if node1 not in self.deletedNodes.keys() and node2 not in self.deletedNodes.keys():
                    continue

                if self.nodeScore[node1] > score_threshold:
                    continue

                if self.nodeScore[node2] > score_threshold:
                    continue

                del self.edges[edgeid]
                del self.edgeHash[node1 * 10000000 + node2]

    def findNearbySegments(self, score_threshold=1.0, limitation=100000000, region=None, dist_threshold=0.00035):
        parallelSegs = []

        idx = index.Index()

        print(len(self.edges.keys()))
        for edgeid in self.edges.keys():
            if region is not None:
                if self.nodes[self.edges[edgeid][0]][0] < region[0] or self.nodes[self.edges[edgeid][0]][0] > region[2]:
                    continue

                if self.nodes[self.edges[edgeid][0]][1] < region[1] or self.nodes[self.edges[edgeid][0]][1] > region[3]:
                    continue

            if edgeid in self.edgeScore.keys():
                if self.edgeScore[edgeid] < score_threshold:
                    continue

                node1 = self.edges[edgeid][0]
                node2 = self.edges[edgeid][1]

                if node1 in self.deletedNodes.keys() or node2 in self.deletedNodes.keys():
                    continue

                if self.nodeScore[node1] < score_threshold:
                    continue

                if self.nodeScore[node2] < score_threshold:
                    continue

                print(edgeid, len(self.edges.keys()))

                lat1 = self.nodes[node1][0]
                lon1 = self.nodes[node1][1]

                lat2 = self.nodes[node2][0]
                lon2 = self.nodes[node2][1]

                idx.insert(edgeid, (min(lat1, lat2), min(lon1, lon2), max(lat1, lat2), max(lon1, lon2)))

        for edgeid in self.edges.keys():
            if region is not None:
                if self.nodes[self.edges[edgeid][0]][0] < region[0] or self.nodes[self.edges[edgeid][0]][0] > region[2]:
                    continue

                if self.nodes[self.edges[edgeid][0]][1] < region[1] or self.nodes[self.edges[edgeid][0]][1] > region[3]:
                    continue

            if edgeid in self.edgeScore.keys():
                if self.edgeScore[edgeid] < score_threshold:
                    continue

                node1 = self.edges[edgeid][0]
                node2 = self.edges[edgeid][1]

                if node1 in self.deletedNodes.keys() or node2 in self.deletedNodes.keys():
                    continue

                if self.nodeScore[node1] < score_threshold:
                    continue

                if self.nodeScore[node2] < score_threshold:
                    continue

                # find the closest edge pair

                lat = (self.nodes[node1][0] + self.nodes[node2][0]) / 2
                lon = (self.nodes[node1][1] + self.nodes[node2][1]) / 2
                r = dist_threshold

                possibleEdges = list(idx.intersection((lat - r, lon - r, lat + r, lon + r)))

                parallelRoads = []
                candidate_edgeid = -1
                min_dist = dist_threshold

                dir_lat, dir_lon = latlonNorm((self.nodes[node2][0] - self.nodes[node1][0], self.nodes[node2][1] - self.nodes[node1][1]),
                                              lat=self.nodes[node2][0])
                norm_lat = -dir_lon
                norm_lon = dir_lat

                center_lat = lat
                center_lon = lon

                meter_lat = 1.0 / (111111.0)
                meter_lon = 1.0 / (111111.0 * math.cos(lat / 360.0 * (math.pi * 2)))

                p1 = (center_lat - 12.5 * meter_lat * dir_lat, center_lon - 12.5 * meter_lon * dir_lon)
                p2 = (center_lat + 12.5 * meter_lat * dir_lat, center_lon + 12.5 * meter_lon * dir_lon)

                for id_ in possibleEdges:
                    if id_ == edgeid:
                        continue

                    can_node1 = self.edges[id_][0]
                    can_node2 = self.edges[id_][1]

                    can_dir_lat, can_dir_lon = latlonNorm(
                        (self.nodes[can_node2][0] - self.nodes[can_node1][0], self.nodes[can_node2][1] - self.nodes[can_node1][1]),
                        lat=self.nodes[node2][0])

                    angle_dist = pointDistance((can_dir_lat, can_dir_lon), (dir_lat, dir_lon))

                    if angle_dist < 1.9 and angle_dist > 0.1:
                        continue

                    ilat, ilon, extend_length, ok = edgeIntersection(center_lat, center_lon, norm_lat, norm_lon, self.nodes[can_node1][0],
                                                                     self.nodes[can_node1][1], self.nodes[can_node2][0],
                                                                     self.nodes[can_node2][1])
                    print(ilat, ilon, extend_length, ok)

                    if ok == 0:
                        ilat, ilon, extend_length, ok = edgeIntersection(center_lat, center_lon, -norm_lat, -norm_lon,
                                                                         self.nodes[can_node1][0], self.nodes[can_node1][1],
                                                                         self.nodes[can_node2][0], self.nodes[can_node2][1])
                    print(ilat, ilon, extend_length, ok)

                    if ok == 0:
                        continue

                    if extend_length < min_dist:
                        can_p1 = (ilat - 12.5 * meter_lat * can_dir_lat, ilon - 12.5 * meter_lon * can_dir_lon)
                        can_p2 = (ilat + 12.5 * meter_lat * can_dir_lat, ilon + 12.5 * meter_lon * can_dir_lon)

                        parallelRoads = (edgeid, id_, p1, p2, can_p1, can_p2)
                        min_dist = extend_length
                        candidate_edgeid = id_

                if candidate_edgeid != -1:
                    print("Add new pairs", len(parallelSegs), parallelRoads)
                    parallelSegs.append(parallelRoads)

                    if len(parallelSegs) > limitation:
                        return parallelSegs

        return parallelSegs

    def getConnectedComponent(self, nodeID):
        node_list = []

        queue = [nodeID]

        while len(queue) > 0:
            n0 = queue.pop(0)
            node_list.append(n0)
            print(n0)
            for n in self.nodeLink[n0] + self.nodeLinkReverse[n0]:
                if n in queue:
                    continue
                if n in node_list:
                    continue

                queue.append(n)

        return node_list

    def removeSmallConnectedComponent(self, threshold=20):
        node_list = []

        for n in self.nodes.keys():
            if n not in node_list:
                cclist = self.getConnectedComponent(n)

                if len(cclist) < threshold:
                    for nn in cclist:
                        self.removeNode(nn)

                node_list = node_list + cclist

    def extendDeadEndP2P(self, lowbound=0):
        idx = index.Index()

        DeadEnds = {}

        newNodeID = 0

        for nid in self.nodes.keys():
            if nid > lowbound:
                if self.NumOfNeighbors(nid) == 1:
                    DeadEnds[nid] = 1
                    lat = self.nodes[nid][0]
                    lon = self.nodes[nid][1]

                    r = 0.00001

                    idx.insert(nid, (lat - r, lon - r, lat + r, lon + r))

        print("Dead Ends", len(DeadEnds.keys()))
        for nid in DeadEnds.keys():
            lat1 = self.nodes[nid][0]
            lon1 = self.nodes[nid][1]

            rlat = 0.00050
            rlon = 0.00050 / math.cos(math.radians(lat1))

            possibleNodes = list(idx.intersection((lat1 - rlat, lon1 - rlon, lat1 + rlat, lon1 + rlon)))

            min_d = 10000
            target = -1
            for pid in possibleNodes:
                id0 = 0
                id3 = 0

                print(nid, self.nodeLink[nid] + self.nodeLinkReverse[nid])
                for id_ in self.nodeLink[nid] + self.nodeLinkReverse[nid]:
                    if id_ != nid:
                        id0 = id_

                print(pid, self.nodeLink[pid] + self.nodeLinkReverse[pid])
                for id_ in self.nodeLink[pid] + self.nodeLinkReverse[pid]:
                    if id_ != pid:
                        id3 = id_

                lat2 = self.nodes[pid][0]
                lon2 = self.nodes[pid][1]

                lat0 = self.nodes[id0][0]
                lon0 = self.nodes[id0][1]

                lat3 = self.nodes[id3][0]
                lon3 = self.nodes[id3][1]

                np1x, np1y = latlonNorm((lat1 - lat0, lon1 - lon0))
                np2x, np2y = latlonNorm((lat2 - lat1, lon2 - lon1))
                np3x, np3y = latlonNorm((lat3 - lat2, lon3 - lon2))

                if pointDistance((np1x, np1y), (np2x, np2y)) < 1.4 and pointDistance((np2x, np2y), (np3x, np3y)) < 1.4:
                    d = distance((lat1, lon1), (lat2, lon2))
                    if d < min_d:
                        min_d = d
                        target = pid

            if target != -1:
                lat2 = self.nodes[target][0]
                lon2 = self.nodes[target][1]
                self.addEdge("p2p" + str(newNodeID), lat1, lon1, "p2p" + str(newNodeID + 1), lat2, lon2)
                newNodeID += 2

        print("New Nodes", newNodeID)

    def findIntersections(self):
        idx = index.Index()
        for edgeId, edge in self.edges.iteritems():
            n1 = edge[0]
            n2 = edge[1]

            lat1 = self.nodes[n1][0]
            lon1 = self.nodes[n1][1]

            lat2 = self.nodes[n2][0]
            lon2 = self.nodes[n2][1]

            idx.insert(edgeId, (min(lat1, lat2), min(lon1, lon2), max(lat1, lat2), max(lon1, lon2)))

        edgePairs = []
        edgePairsMask = []

        def crossProduct(p1, p2):
            return p1[0] * p2[1] - p1[1] * p2[0]

        for edgeId, edge in self.edges.iteritems():
            n1 = edge[0]
            n2 = edge[1]

            lat1 = self.nodes[n1][0]
            lon1 = self.nodes[n1][1]

            lat2 = self.nodes[n2][0]
            lon2 = self.nodes[n2][1]

            possibleEdges = list(idx.intersection((min(lat1, lat2), min(lon1, lon2), max(lat1, lat2), max(lon1, lon2))))

            for p_edgeId in possibleEdges:
                if p_edgeId == edgeId:
                    continue

                n3 = self.edges[p_edgeId][0]
                n4 = self.edges[p_edgeId][1]

                lat3 = self.nodes[n3][0]
                lon3 = self.nodes[n3][1]

                lat4 = self.nodes[n4][0]
                lon4 = self.nodes[n4][1]

                d1 = crossProduct((lat3 - lat1, lon3 - lon1), (lat2 - lat3, lon2 - lon3))
                d2 = crossProduct((lat2 - lat3, lon2 - lon3), (lat4 - lat2, lon4 - lon2))
                d3 = crossProduct((lat4 - lat2, lon4 - lon2), (lat1 - lat4, lon1 - lon4))
                d4 = crossProduct((lat1 - lat4, lon1 - lon4), (lat3 - lat1, lon3 - lon1))

                if (d1 > 0 and d2 > 0 and d3 > 0 and d4 > 0) or (d1 < 0 and d2 < 0 and d3 < 0 and d4 < 0):
                    if (edgeId, p_edgeId) not in edgePairsMask and (p_edgeId, edgeId) not in edgePairsMask:
                        dlat1, dlon1 = latlonNorm((lat2 - lat1, lon2 - lon1))
                        dlat2, dlon2 = latlonNorm((lat4 - lat3, lon4 - lon3))

                        d = dlat1 * dlat2 + dlon1 * dlon2

                        angle = abs(d)

                        check_distance = 0.0005

                        check_lat1 = lat1 - dlat1 * check_distance
                        check_lon1 = lon1 - dlon1 * check_distance / math.cos(math.radians(lat1))

                        check_lat2 = lat2 + dlat1 * check_distance
                        check_lon2 = lon2 + dlon1 * check_distance / math.cos(math.radians(lat1))

                        check_lat3 = lat3 - dlat2 * check_distance
                        check_lon3 = lon3 - dlon2 * check_distance / math.cos(math.radians(lat1))

                        check_lat4 = lat4 + dlat2 * check_distance
                        check_lon4 = lon4 + dlon2 * check_distance / math.cos(math.radians(lat1))

                        center_lat1, center_lon1, _, _ = edgeIntersection(lat1, lon1, dlat1, dlon1 / math.cos(math.radians(lat1)), lat3,
                                                                          lon3, lat4, lon4)

                        # center_lat1 = (lat1 + lat2 + lat3 + lat4) / 4
                        # center_lon1 = (lon1 + lon2 + lon3 + lon4) / 4

                        r1 = 0.00005
                        rc = 0.00008

                        data = [check_lat1, check_lon1, center_lat1, center_lon1, check_lat3, check_lon3, r1, rc, r1]
                        data += [check_lat3, check_lon3, center_lat1, center_lon1, check_lat1, check_lon1, r1, rc, r1]
                        data += [check_lat3, check_lon3, center_lat1, center_lon1, check_lat2, check_lon2, r1, rc, r1]
                        data += [check_lat2, check_lon2, center_lat1, center_lon1, check_lat3, check_lon3, r1, rc, r1]
                        data += [check_lat4, check_lon4, center_lat1, center_lon1, check_lat2, check_lon2, r1, rc, r1]
                        data += [check_lat2, check_lon2, center_lat1, center_lon1, check_lat4, check_lon4, r1, rc, r1]
                        data += [check_lat4, check_lon4, center_lat1, center_lon1, check_lat1, check_lon1, r1, rc, r1]
                        data += [check_lat1, check_lon1, center_lat1, center_lon1, check_lat4, check_lon4, r1, rc, r1]

                        result = TraceQueryBatch3P(data)

                        print(edgeId, p_edgeId, angle, result, center_lat1, center_lon1)

                        edgePairs.append((edgeId, p_edgeId, angle, result))
                        edgePairsMask.append((edgeId, p_edgeId))

        # if len(edgePairs) > 100:
        #	return edgePairs

        return edgePairs

    def Dump2GeoJson(self, filename, region=[-200, -200, 200, 200]):

        print("Dump geojson to " + filename)
        res = ""

        myfeature = []

        for edgeId, edge in self.edges.iteritems():
            n1 = edge[0]
            n2 = edge[1]

            lat1 = self.nodes[n1][0]
            lon1 = self.nodes[n1][1]

            lat2 = self.nodes[n2][0]
            lon2 = self.nodes[n2][1]

            lat_min = min(lat1, lat2)
            lat_max = max(lat1, lat2)

            lon_min = min(lon1, lon2)
            lon_max = max(lon1, lon2)

            inside = True
            if lon_min > region[3] or lon_max < region[1] or lat_max < region[0] or lat_min > region[2]:
                inside = False

            if inside == True:
                myfeature.append(
                    Feature(properties={"id": edgeId, "type": "residential"}, geometry=LineString([(lon1, lat1), (lon2, lat2)])))

        feature_collection = FeatureCollection(myfeature)

        with open(filename, "w") as fout:
            geojson.dump(feature_collection, fout, indent=2)


def ConnectivityChecker2Points(lat1, lon1, lat2, lon2, radius=0.00005):
    dist = distance((lat1, lon1), (lat2, lon2))

    if dist < radius * 2:
        return True
    else:
        radius = min(max(dist * 0.1, 0.00005), 0.00012)

    data = [lat1, lon1, lat2, lon2, radius]

    result = TraceQueryBatch(data)

    gpsAt1 = result[0]
    gpsAt2 = result[1]

    validateTrips = result[3] + result[4]

    print(result)

    # validateTrips = 0

    if gpsAt1 < 5 or gpsAt2 < 5:
        return True

    if validateTrips < 3:
        return False

    return True


def edgeIntersection(baseX, baseY, dX, dY, n1X, n1Y, n2X, n2Y):
    t = dX * n1Y + dY * n2X - dX * n2Y - dY * n1X

    c = n2X * n1Y - n1X * n2Y + baseX * (n2Y - n1Y) + baseY * (n1X - n2X)

    if t == 0:
        return 0, 0, 0, 0

    alpha = c / t

    if alpha < 0:
        return 0, 0, 0, 0

    iX = baseX + alpha * dX
    iY = baseY + alpha * dY

    d = (iX - n1X) * (n2X - iX) + (iY - n1Y) * (n2Y - iY)

    if d < 0:
        return 0, 0, 0, 0

    extend_length = np.sqrt(alpha * dX * alpha * dX + alpha * dY * alpha * dY)

    return iX, iY, extend_length, 1


class RoadGraphCombiner:
    def __init__(self, RoadGraphA, RoadGraphB, ConnectivityChecker=None, CNNOutput=None, region=None):

        if CNNOutput is not None:
            cnn_dat = scipy.ndimage.imread(CNNOutput) * 255

        # Add to index
        idx = index.Index()
        for edgeId, edge in RoadGraphA.edges.iteritems():
            n1 = edge[0]
            n2 = edge[1]

            if n1 in RoadGraphA.deletedNodes.keys() or n2 in RoadGraphA.deletedNodes.keys():
                continue

            if RoadGraphA.nodeScore[n1] < 1 or RoadGraphA.nodeScore[n2] < 1:
                continue

            if n1 in RoadGraphA.nodeTerminate.keys() or n2 in RoadGraphA.nodeTerminate.keys():
                continue

            score = RoadGraphA.edgeScore[RoadGraphA.edgeHash[n1 * 10000000 + n2]]
            if score < 1:
                continue

            lat1 = RoadGraphA.nodes[n1][0]
            lon1 = RoadGraphA.nodes[n1][1]
            lat2 = RoadGraphA.nodes[n2][0]
            lon2 = RoadGraphA.nodes[n2][1]

            idx.insert(edgeId, (min(lat1, lat2), min(lon1, lon2), max(lat1, lat2), max(lon1, lon2)))

        # Find Candidate Edge
        DeadEnds = {}

        for edgeId, edge in RoadGraphB.edges.iteritems():
            n1 = edge[0]
            n2 = edge[1]

            nn1 = RoadGraphB.NumOfNeighbors(n1)
            nn2 = RoadGraphB.NumOfNeighbors(n2)

            if nn1 == 1 or nn2 == 1:
                DeadEnds[edgeId] = 1

        # Extend Edge

        newNodeId = 0

        print("DeadEnds ", len(DeadEnds.keys()))

        for edgeId in DeadEnds.keys():
            edge = RoadGraphB.edges[edgeId]
            n1 = edge[0]
            n2 = edge[1]

            nn1 = RoadGraphB.NumOfNeighbors(n1)
            nn2 = RoadGraphB.NumOfNeighbors(n2)

            lat1 = RoadGraphB.nodes[n1][0]
            lon1 = RoadGraphB.nodes[n1][1]

            lat2 = RoadGraphB.nodes[n2][0]
            lon2 = RoadGraphB.nodes[n2][1]

            # if lat1 < 42.3519 or lat1 > 42.3631 or lon1 < -71.1277 or lon1 > -71.1127:
            # 	continue

            avglat = (lat1 + lat2) / 2
            avglon = (lon1 + lon2) / 2

            if nn1 == 1:
                dlat = lat1 - lat2
                dlon = lon1 - lon2

                lat = lat1
                lon = lon1
                r = 0.00100  # 100 meters

                possibleEdges = list(idx.intersection((lat - r, lon - r, lat + r, lon + r)))

                if len(possibleEdges) > 0:
                    print(len(possibleEdges))

                min_dist = 0.00050
                newlat = 0
                newlon = 0
                old_n1 = 0
                old_n2 = 0

                for possibleEdge in possibleEdges:
                    pn1 = RoadGraphA.edges[possibleEdge][0]
                    pn2 = RoadGraphA.edges[possibleEdge][1]

                    pn1lat = RoadGraphA.nodes[pn1][0]
                    pn1lon = RoadGraphA.nodes[pn1][1]

                    pn2lat = RoadGraphA.nodes[pn2][0]
                    pn2lon = RoadGraphA.nodes[pn2][1]

                    pn3 = -1
                    pn0 = -1

                    for next_node in RoadGraphA.nodeLink[pn2]:
                        if next_node != pn1:
                            pn3 = next_node

                    for prev_node in RoadGraphA.nodeLinkReverse[pn1]:
                        if prev_node != pn2:
                            pn0 = prev_node

                    if pn0 == -1 or pn3 == -1:
                        continue

                    pn0lat = RoadGraphA.nodes[pn0][0]
                    pn0lon = RoadGraphA.nodes[pn0][1]

                    pn3lat = RoadGraphA.nodes[pn3][0]
                    pn3lon = RoadGraphA.nodes[pn3][1]

                    newlat_, newlon_, dist, ok = edgeIntersection(lat, lon, dlat, dlon, pn1lat, pn1lon, pn2lat, pn2lon)

                    if dist < min_dist and ok == 1:
                        min_dist = dist

                        newlat = newlat_
                        newlon = newlon_

                        old_n1 = pn1
                        old_n2 = pn2

                        if self.CNNConnectivityChecker(lat, lon, newlat, newlon, cnn_dat, region) == True:
                            if self.ConnectivityChecker(avglat, avglon, pn1lat, pn1lon, pn0lat, pn0lon) == True or self.ConnectivityChecker(
                                    avglat, avglon, pn2lat, pn2lon, pn3lat, pn3lon) == True:
                                RoadGraphA.addEdgeToOneExistedNode("Tmp" + str(newNodeId), newlat, newlon, old_n1, reverse=True,
                                                                   nodeScore1=100, edgeScore=100)
                                RoadGraphA.addEdgeToOneExistedNode("Tmp" + str(newNodeId), newlat, newlon, old_n2, reverse=True,
                                                                   nodeScore1=100, edgeScore=100)

                                RoadGraphA.addEdge("Tmp" + str(newNodeId), newlat, newlon, "CNN" + str(n1), lat, lon, reverse=True,
                                                   nodeScore1=100, nodeScore2=100, edgeScore=100)

                                newNodeId += 1
                                print(newNodeId)

            # TODO
            # print(min_dist)

            # if min_dist < 0.00050: # 50 meters
            # 	print(newlat, newlon)
            # 	RoadGraphA.addEdgeToOneExistedNode("Tmp"+str(newNodeId), newlat, newlon, old_n1, reverse=True, nodeScore1 = 100, edgeScore = 100)
            # 	RoadGraphA.addEdgeToOneExistedNode("Tmp"+str(newNodeId), newlat, newlon, old_n2, reverse=True, nodeScore1 = 100, edgeScore = 100)

            # 	RoadGraphA.addEdge("Tmp"+str(newNodeId), newlat, newlon, "CNN"+str(n1), lat, lon, reverse=True, nodeScore1 = 100, nodeScore2 = 100, edgeScore = 100)

            # 	newNodeId += 1

            if nn2 == 1:
                dlat = lat2 - lat1
                dlon = lon2 - lon1

                lat = lat2
                lon = lon2
                r = 0.00100  # 100 meters

                possibleEdges = list(idx.intersection((lat - r, lon - r, lat + r, lon + r)))

                if len(possibleEdges) > 0:
                    print(len(possibleEdges))

                min_dist = 0.00050
                newlat = 0
                newlon = 0
                old_n1 = 0
                old_n2 = 0

                for possibleEdge in possibleEdges:
                    pn1 = RoadGraphA.edges[possibleEdge][0]
                    pn2 = RoadGraphA.edges[possibleEdge][1]

                    pn1lat = RoadGraphA.nodes[pn1][0]
                    pn1lon = RoadGraphA.nodes[pn1][1]

                    pn2lat = RoadGraphA.nodes[pn2][0]
                    pn2lon = RoadGraphA.nodes[pn2][1]

                    pn3 = -1
                    pn0 = -1

                    for next_node in RoadGraphA.nodeLink[pn2]:
                        if next_node != pn1:
                            pn3 = next_node

                    for prev_node in RoadGraphA.nodeLinkReverse[pn1]:
                        if prev_node != pn2:
                            pn0 = prev_node

                    if pn0 == -1 or pn3 == -1:
                        continue

                    pn0lat = RoadGraphA.nodes[pn0][0]
                    pn0lon = RoadGraphA.nodes[pn0][1]

                    pn3lat = RoadGraphA.nodes[pn3][0]
                    pn3lon = RoadGraphA.nodes[pn3][1]

                    newlat_, newlon_, dist, ok = edgeIntersection(lat, lon, dlat, dlon, pn1lat, pn1lon, pn2lat, pn2lon)

                    if dist < min_dist and ok == 1:
                        min_dist = dist

                        newlat = newlat_
                        newlon = newlon_

                        old_n1 = pn1
                        old_n2 = pn2

                        if self.CNNConnectivityChecker(lat, lon, newlat, newlon, cnn_dat, region) == True:
                            if self.ConnectivityChecker(avglat, avglon, pn1lat, pn1lon, pn0lat, pn0lon) == True or self.ConnectivityChecker(
                                    avglat, avglon, pn2lat, pn2lon, pn3lat, pn3lon) == True:
                                RoadGraphA.addEdgeToOneExistedNode("Tmp" + str(newNodeId), newlat, newlon, old_n1, reverse=True,
                                                                   nodeScore1=100, edgeScore=100)
                                RoadGraphA.addEdgeToOneExistedNode("Tmp" + str(newNodeId), newlat, newlon, old_n2, reverse=True,
                                                                   nodeScore1=100, edgeScore=100)

                                RoadGraphA.addEdge("Tmp" + str(newNodeId), newlat, newlon, "CNN" + str(n2), lat, lon, reverse=True,
                                                   nodeScore1=100, nodeScore2=100, edgeScore=100)

                                newNodeId += 1
                                print("NodeID", newNodeId)

            # TODO

        # print(min_dist)

        # if min_dist < 0.00050: # 50 meters
        # 	print(newlat, newlon)
        # 	RoadGraphA.addEdgeToOneExistedNode("Tmp"+str(newNodeId), newlat, newlon, old_n1, reverse=True, nodeScore1 = 100, edgeScore = 100)
        # 	RoadGraphA.addEdgeToOneExistedNode("Tmp"+str(newNodeId), newlat, newlon, old_n2, reverse=True, nodeScore1 = 100, edgeScore = 100)

        # 	RoadGraphA.addEdge("Tmp"+str(newNodeId), newlat, newlon, "CNN"+str(n2), lat, lon, reverse=True, nodeScore1 = 100, nodeScore2 = 100, edgeScore = 100)

        # 	newNodeId += 1

        # Add GraphB to GraphA
        for edgeId, edge in RoadGraphB.edges.iteritems():
            n1 = edge[0]
            n2 = edge[1]

            lat1 = RoadGraphB.nodes[n1][0]
            lon1 = RoadGraphB.nodes[n1][1]

            lat2 = RoadGraphB.nodes[n2][0]
            lon2 = RoadGraphB.nodes[n2][1]

            RoadGraphA.addEdge("CNN" + str(n1), lat1, lon1, "CNN" + str(n2), lat2, lon2, reverse=True, nodeScore1=100, nodeScore2=100,
                               edgeScore=100)

        pass

    def ConnectivityChecker(self, lat1, lon1, lat2, lon2, lat3, lon3, radius=0.00005):
        # TODO check speed?

        data = [lat1, lon1, 0.00030, 0.2, lat2, lon2, 0.00030, 0.2, lat3, lon3, 0.00030, 0.2]

        result = SpeedQueryBatch(data, port=8020)

        result = [(5 / x * 3.6 / 1.6) for x in result]

        if result[0] < 40 and result[1] < 40 and result[2] < 40:
            return True

        # else:
        # 	return False

        data = [lat1, lon1, lat2, lon2, radius]

        result = TraceQueryBatch(data)

        gpsAt1 = result[0]
        gpsAt2 = result[1]

        validateTrips = result[3] + result[4]

        print(result)

        # validateTrips = 0

        if gpsAt1 < 5:
            return True

        if validateTrips < 3:
            return False

        data = [lat1, lon1, lat2, lon2, lat3, lon3, radius, radius, radius]
        validateTrips = max(validateTrips, TraceQueryBatch3P(data))

        data = [lat3, lon3, lat2, lon2, lat1, lon1, radius, radius, radius]
        validateTrips = max(validateTrips, TraceQueryBatch3P(data))

        print("validateTrips ", validateTrips)

        if validateTrips > 2:
            return True

        return False

    def CNNConnectivityChecker(self, lat1, lon1, lat2, lon2, img, region, l=80, check_range=12):
        min_lat = region[0]
        min_lon = region[1]
        max_lat = region[2]
        max_lon = region[3]

        sizex = np.shape(img)[0]
        sizey = np.shape(img)[1]

        ilat, ilon = Coord2Pixels(lat1, lon1, min_lat, min_lon, max_lat, max_lon, sizex, sizey)
        ilatT, ilonT = Coord2Pixels(lat2, lon2, min_lat, min_lon, max_lat, max_lon, sizex, sizey)

        n = []

        n.append((ilat, ilon))
        n.append((ilat + 1, ilon))
        n.append((ilat - 1, ilon))
        n.append((ilat, ilon + 1))
        n.append((ilat, ilon - 1))
        n.append((ilat + 1, ilon + 1))
        n.append((ilat + 1, ilon - 1))
        n.append((ilat - 1, ilon + 1))
        n.append((ilat - 1, ilon - 1))

        visited = []
        mark = []

        flag = False

        while True:
            if len(n) == 0:
                break

            loc = n.pop(0)

            if loc in visited:
                continue

            if (loc[0] - ilat) * (loc[0] - ilat) + (loc[1] - ilon) * (loc[1] - ilon) > l * l:
                continue

            if img[loc[0], loc[1]] == 0:
                continue

            visited.append(loc)

            n.append((loc[0] + 1, loc[1]))
            n.append((loc[0] - 1, loc[1]))
            n.append((loc[0], loc[1] + 1))
            n.append((loc[0], loc[1] - 1))
            n.append((loc[0] + 1, loc[1] + 1))
            n.append((loc[0] - 1, loc[1] + 1))
            n.append((loc[0] + 1, loc[1] - 1))
            n.append((loc[0] - 1, loc[1] - 1))

            if (loc[0] - ilatT) * (loc[0] - ilatT) + (loc[1] - ilonT) * (loc[1] - ilonT) <= check_range * check_range:
                flag = True
                break

        return flag


def CheckParallelSegments(s1, s2, lat_res, lon_res, image_size=512, folder="", start_lat=0, start_lon=0, resolution=1024, padding=128,
                          zoom=18, scale=1, output_id=0):
    center_lat = (s1[0][0] + s1[1][0]) / 2.0
    center_lon = (s1[0][1] + s1[1][1]) / 2.0

    range_minlat = center_lat - lat_res * image_size / 2
    range_maxlat = center_lat + lat_res * image_size / 2

    range_minlon = center_lon - lon_res * image_size / 2
    range_maxlon = center_lon + lon_res * image_size / 2

    m, ok = md.GetMapInRect(range_minlat, range_minlon, range_maxlat, range_maxlon, folder=folder, start_lat=start_lat, start_lon=start_lon,
                            resolution=resolution, padding=padding, zoom=zoom, scale=scale)
    m1 = scipy.misc.imresize(m.astype(np.uint8), (image_size, image_size), mode="RGB")

    seg_img = np.zeros((image_size, image_size), np.uint8)
    seg_cv = cv.fromarray(seg_img)

    s1_lat_i = int((s1[0][0] - range_minlat) / lat_res)
    s1_lon_i = int((s1[0][1] - range_minlon) / lon_res)
    s2_lat_i = int((s1[1][0] - range_minlat) / lat_res)
    s2_lon_i = int((s1[1][1] - range_minlon) / lon_res)

    s3_lat_i = int((s2[0][0] - range_minlat) / lat_res)
    s3_lon_i = int((s2[0][1] - range_minlon) / lon_res)
    s4_lat_i = int((s2[1][0] - range_minlat) / lat_res)
    s4_lon_i = int((s2[1][1] - range_minlon) / lon_res)

    cv.Line(seg_cv, (s1_lon_i, s1_lat_i), (s2_lon_i, s2_lat_i), (255), 1, cv.CV_AA)
    cv.Line(seg_cv, (s3_lon_i, s3_lat_i), (s4_lon_i, s4_lat_i), (255), 1, cv.CV_AA)

    cv.Flip(seg_cv)

    seg_np = np.asarray(seg_cv, dtype='uint8')

    inputs = np.zeros((image_size, image_size, 4))

    inputs[:, :, 0:3] = m1
    inputs[:, :, 3] = seg_np

    Image.fromarray(m1).save("test111.png")
    Image.fromarray(inputs[:, :, 3].reshape((image_size, image_size)).astype(np.uint8)).save("test112.png")

    Image.fromarray(m1).save("test111.png")

    result = md.GetClassifierResult(inputs)
    # result = 0
    print(result)

    inputs[:, :, 0] = np.clip(inputs[:, :, 3] + inputs[:, :, 0], 0, 255)
    Image.fromarray(inputs[:, :, 0:3].reshape((image_size, image_size, 3)).astype(np.uint8)).save("test/test" + str(output_id) + ".png")

    return result


if __name__ == "__main__":
    data = [42.355538, -71.083505, 0.00030, float(sys.argv[1]), 42.352176, -71.081198, 0.00030, float(sys.argv[1]), 42.352176, -71.081198,
            0.00030, float(sys.argv[1])]

    s = SpeedQueryBatch(data, port=8020)
    print(s)
    speed = s[0] + 0.0000001
    print(5 / speed * 3.6 / 1.6, "miles/h")
    exit()
    dumpDat = pickle.load(open(sys.argv[1], "rb"))
