from ortools.constraint_solver import routing_enums_pb2
from ortools.constraint_solver import pywrapcp
from bfs import distance, bfs
from house import create_houses


def create_data_model(multi_trash, truck):
    data = {}
    graphrow = []
    graph = []
    lista = multi_trash
    for i in range(8):
        list1=multi_trash
        if (i == 0):
            pos = truck.pos
        else:
            pos = lista[i-1].pos
        for j in range(8):
            if(j==0):
                endpos = truck.pos
            else:
                endpos = list1[j-1].pos
            if(i==j):
                graphrow.append(0)
            else:
                dist=distance(pos, endpos)
                graphrow.append(dist)

        graph.append(graphrow.copy())
        graphrow.clear()
    data['distance_matrix'] = graph.copy()
    data['num_vehicles'] = 1
    data['depot'] = 0
    print(data['distance_matrix'])
    return data


def print_solution(manager, routing, solution):
    index = routing.Start(0)
    plan_output = []
    route_distance = 0
    while not routing.IsEnd(index):
        plan_output.append(manager.IndexToNode(index))
        previous_index = index
        index = solution.Value(routing.NextVar(index))
        route_distance += routing.GetArcCostForVehicle(previous_index, index, 0)
    plan_output.append(manager.IndexToNode(index))
    print(plan_output)
    return plan_output


def tsp(x, y):
    data = create_data_model(x, y)

    manager = pywrapcp.RoutingIndexManager(len(data['distance_matrix']),
                                           data['num_vehicles'], data['depot'])

    routing = pywrapcp.RoutingModel(manager)


    def distance_callback(from_index, to_index):
        from_node = manager.IndexToNode(from_index)
        to_node = manager.IndexToNode(to_index)
        return data['distance_matrix'][from_node][to_node]

    transit_callback_index = routing.RegisterTransitCallback(distance_callback)

    routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index)

    #search_parameters = pywrapcp.DefaultRoutingSearchParameters()
   # search_parameters.first_solution_strategy = (
       # routing_enums_pb2.FirstSolutionStrategy.PATH_CHEAPEST_ARC)
    search_parameters = pywrapcp.DefaultRoutingSearchParameters()
    search_parameters.local_search_metaheuristic = (
        routing_enums_pb2.LocalSearchMetaheuristic.GUIDED_LOCAL_SEARCH)
    search_parameters.time_limit.seconds = 30
    search_parameters.log_search = True

    solution = routing.SolveWithParameters(search_parameters)

    if solution:
        sol = print_solution(manager, routing, solution)

    return sol

def tspmove(order, truck, Ltrash):
    houses = create_houses(40)
    path = []
    endpos=truck.pos
    for i in range(1, 8):
        startpos = endpos
        endpos = Ltrash[order[i]-1].pos
        #print(i,startpos,endpos)
        x = bfs(startpos, truck.dir_control, endpos, houses)
        #print("$$$$",x)
        for i in x:
            if(i==97):
                truck.rotate(-1)
            elif(i==100):
                truck.rotate(1)
        path.append(x)
    x = bfs(endpos, truck.dir_control, truck.pos, houses)
    for i in x:
        if (i == 97):
            truck.rotate(-1)
        elif (i == 100):
            truck.rotate(1)
    path.append(x)
    #print("###############################\n", path)
    truck.dir_control=0
    truck.direction = [1, 0]
    return path