def LocalSearch_FlippingPath(route: Route, graph: cvrpGraph, candidate1,
                             candidate2):

    node1 = candidate1 % len(route.getCustomers())
    node2 = candidate2 % len(route.getCustomers())
    if (node2 != 0):
        firstCandidate = route.getCustomers()[node1]
        prevfc = route.getCustomers()[node1 - 1]
        nextfc = route.getCustomers()[node1 + 1]

        secondCandidate = route.getCustomers()[node2]
        prevsc = route.getCustomers()[node2 - 1]
        nextsc = route.getCustomers()[node2 + 1]

        cost1 = graph.getValue(prevfc, firstCandidate) + graph.getValue(
            firstCandidate, nextfc)
        cost2 = graph.getValue(prevsc, secondCandidate) + graph.getValue(
            secondCandidate, nextsc)

        route.setCost(route.getCost() - (cost1 + cost2))
        a = route.getCustomers().index(firstCandidate)
        b = route.getCustomers().index(secondCandidate)
        route.getCustomers()[a] = secondCandidate
        route.getCustomers()[b] = firstCandidate

        cost3 = graph.getValue(prevfc, secondCandidate) + graph.getValue(
            secondCandidate, nextfc)
        cost4 = graph.getValue(prevsc, firstCandidate) + graph.getValue(
            firstCandidate, nextsc)

        route.setCost(route.getCost() + (cost3 + cost4))

    return route
def ClusterFirst_RouteSecond(graph, saveFolder):

    finalRoutes = []
    capacity = graph.getCapacity()
    demand = graph.getDemand()
    dimension = graph.getDimension()
    dist = [np.inf for i in range(dimension)]
    nodeQueue = []
    auxGraph = [(i, Route(capacity)) for i in range(dimension)]

    #Source Node has 0 cost, in this case route Depot - 1- Depot
    dist[0] = 0

    #nodeQueue = [(c+1,dist[c]) for c in range(len(dist))]
    route = Route(capacity)
    route.addCustomer(0, demand[0], False)
    route.addCustomer(1, demand[1], False)
    route.addCustomer(0, demand[0], False)
    route.setCost(graph.getValue(0, 1) + graph.getValue(1, 0))

    nodeQueue = [(1, route.getCost(), route)]
    node = 0
    nodeQueue.sort(key=lambda x: x[1], reverse=True)

    while len(nodeQueue) > 0:
        #Give the node in the auxiliary graph that has low cost
        nodeToexpand = nodeQueue.pop()
        node = nodeToexpand[0]
        cost = nodeToexpand[1]
        #Give the corresponding route, that contain node
        #auxGraph.pop()
        #Create each child of the node in the auxiliary graph
        for j in range(1, dimension):
            #j child
            if (j >= node):
                newRoute = Route(capacity)
                newRoute.setCost(0)
                newRoute.addCustomer(0, demand[0], False)
                control = -3
                #for each child that not exceed the truck capacity
                for i in range(node, j + 1):
                    #The not is not in the route and it not exceed the truck capacity
                    #Add Node
                    if (newRoute.checkCustomer(i) == -1):
                        control = newRoute.addCustomer(i, demand[i], False)
                        #Update Cost
                        if (control > 0):
                            newRoute.setCost(
                                newRoute.getCost() + graph.getValue(
                                    newRoute.getCustomers()[node - i], i))
                        else:
                            break
                #Close the route
                if (control > 0):
                    newRoute.setCost(newRoute.getCost() + graph.getValue(j, 0))
                    newRoute.addCustomer(0, demand[0], False)
                    if (dist[j] > newRoute.getCost() + cost):
                        dist[j] = newRoute.getCost() + cost
                        nodeQueue.append((j + 1, dist[j], newRoute))
                        auxGraph[j] = (node - 1, newRoute)
                        nodeQueue.sort(key=lambda x: x[1], reverse=True)
                else:
                    break

    u = len(auxGraph) - 1
    while (u != 0):
        node = auxGraph[u]
        u = node[0]
        finalRoutes.append(node[1])

    return finalRoutes
def Sequential_CW(routes, savings, graph: cvrpGraph):
    capacity = graph.getCapacity()
    demand = graph.getDemand()

    toDo, checked, _ = SearchaAndCompleteSequence(routes, graph, True)
    #no routes have been created yet
    while (SearchaAndCompleteSequence(routes, graph) == True
           and len(savings) > 0):
        routeSelected = Route(capacity)
        printable = savings.copy()
        k = 0
        saveNow = savings[0]
        l = saveNow[1]
        m = saveNow[2]
        if ((l not in checked) and (m not in checked)
                and (len(routeSelected.getCustomers()) == 0)):
            #No one served i and j so this route will be teh first
            routeSelected.addCustomer(l, demand[l], True)
            routeSelected.addCustomer(m, demand[m], False)
            savings.remove(saveNow)

            while k < len(savings):
                save = savings[k]
                k = k + 1
                i = save[1]
                j = save[2]
                customerI = -1
                customerJ = -1
                toprint = save
                if (toDo == True):
                    #Check if someone alraedy served i and j
                    if ((i not in checked) and (j not in checked)):
                        if (-2 < customerI < 0
                                and len(routeSelected.getCustomers()) > 0):
                            customerI = routeSelected.checkCustomer(i)

                        if (-2 < customerJ < 0
                                and len(routeSelected.getCustomers()) > 0):
                            customerJ = routeSelected.checkCustomer(j)

                        # case: i and j have not been served and they are not route first entry
                        #customer i is served from this route but j is not.
                        if (customerI >= 0 and customerJ == -1):
                            if (customerI == 0):
                                control = routeSelected.addCustomer(
                                    j, demand[j], True)
                                if (control != -1):
                                    savings.remove(save)
                                    k = 0
                            else:
                                control = routeSelected.addCustomer(
                                    j, demand[j], False)
                                if (control != -1):
                                    savings.remove(save)
                                    k = 0
                        #customer j is served from this route but i is not.
                        if (customerI == -1 and customerJ >= 0):
                            if (customerJ == 0):
                                control = routeSelected.addCustomer(
                                    i, demand[i], True)
                                if (control != -1):
                                    savings.remove(save)
                                    k = 0
                            else:
                                control = routeSelected.addCustomer(
                                    i, demand[i], False)
                                if (control != -1):
                                    savings.remove(save)
                                    k = 0
                print("Savings number :" + str(printable.index(toprint)))
            routes.append(routeSelected)
            routeCost = 0
            for i in range(len(routeSelected.getCustomers()) - 1):
                routeCost += graph.getValue(
                    routeSelected.getCustomers()[i],
                    routeSelected.getCustomers()[i + 1])
            routeSelected.setCost(routeCost)

            toDo, checked, _ = SearchaAndCompleteSequence(routes, graph, True)
            savings.append((float(
                graph.getValue(i, 0) + graph.getValue(0, j) -
                routeSelected.getCost()), i, j))
            savings.sort(key=lambda x: x[0], reverse=True)

        else:
            savings.remove(saveNow)

    return routes