import gurobipy as gp
from gurobipy import GRB
import networkx as nx
import matplotlib.pyplot as plt

def create_grid_graph(n_rows, n_cols):
    G = nx.grid_2d_graph(n_rows, n_cols)
    # Convert to directed graph if needed
    G = G.to_directed()
    # Add positions for plotting
    pos = {(i,j):(i,j) for i,j in G.nodes()}
    nx.set_node_attributes(G, pos, 'pos')
    return G

def compute_shortest_paths(G, required_nodes):
    # Compute shortest path lengths and paths between required nodes
    import itertools
    distances = {}
    paths = {}
    for i, j in itertools.permutations(required_nodes, 2):
        length, path = nx.single_source_dijkstra(G, i, j)
        distances[i,j] = length
        paths[i,j] = path
    return distances, paths

def create_complete_graph(required_nodes, distances):
    G_complete = nx.DiGraph()
    for i in required_nodes:
        G_complete.add_node(i)
    for (i,j), length in distances.items():
        G_complete.add_edge(i, j, weight=length)
    return G_complete

def solve_tsp_complete_graph(G_complete):
    model = gp.Model("TSP")
    nodes = list(G_complete.nodes)
    edges = list(G_complete.edges)
    num_nodes = len(nodes)
    
    # Create variables
    x = {}
    for i, j in edges:
        x[i, j] = model.addVar(vtype=GRB.BINARY, name=f"x_{i}{j}")
    
    # Set objective
    model.setObjective(gp.quicksum(G_complete[i][j]['weight'] * x[i,j] for i,j in edges), GRB.MINIMIZE)
    
    # Constraints
    for i in nodes:
        model.addConstr(gp.quicksum(x[i,j] for j in nodes if (i,j) in x) == 1, name=f"out{i}")
        model.addConstr(gp.quicksum(x[j,i] for j in nodes if (j,i) in x) == 1, name=f"in_{i}")
    
    # Subtour elimination constraints using Miller-Tucker-Zemlin formulation
    u = {}
    for i in nodes:
        u[i] = model.addVar(vtype=GRB.CONTINUOUS, lb=0, ub=num_nodes, name=f"u_{i}")
    
    model.addConstr(u[nodes[0]] == 0, name='subtour_elimination_start')
    
    for i, j in edges:
        if i != nodes[0] and j != nodes[0]:
            model.addConstr(u[i] - u[j] + num_nodes * x[i,j] <= num_nodes - 1, name=f"subtour_elimination_{i}_{j}")
    
    # Optimize
    model.optimize()
    
    solution = []
    for i, j in edges:
        if x[i,j].X > 0.5:
            solution.append((i,j))
    return solution

def reconstruct_full_path(tour, paths):
    full_path = []
    for i, j in tour:
        path = paths[i,j][:-1]  # Exclude last node to prevent duplication
        full_path.extend(path)
    # Add the last node of the last path
    full_path.append(tour[-1][1])
    return full_path

def visualize_solution(G, full_path, required_nodes):
    pos = nx.get_node_attributes(G, 'pos')
    plt.figure(figsize=(8,8))
    nx.draw(G, pos, with_labels=False, node_size=50, node_color='lightgrey', arrows=False)
    # Highlight required nodes
    nx.draw_networkx_nodes(G, pos, nodelist=required_nodes, node_color='red')
    # Draw the path
    edges_in_path = list(zip(full_path, full_path[1:]))
    nx.draw_networkx_edges(G, pos, edgelist=edges_in_path, edge_color='blue', width=2)
    plt.show()

# Example usage
n_rows = 3
n_cols = 3
G = create_grid_graph(n_rows, n_cols)

# Define required nodes to visit
required_nodes = [(0,0), (2,2), (1,2)]

# Compute shortest paths and distances between required nodes
distances, paths = compute_shortest_paths(G, required_nodes)

# Create a complete graph of required nodes with distances as weights
G_complete = create_complete_graph(required_nodes, distances)

# Solve TSP on the complete graph of required nodes
solution_edges = solve_tsp_complete_graph(G_complete)

# Build the tour from the solution edges
tour = []
current_node = required_nodes[0]
visited = set()
while len(visited) < len(required_nodes):
    for i,j in solution_edges:
        if i == current_node and j not in visited:
            tour.append((i,j))
            visited.add(i)
            current_node = j
            break
    else:
        break  # Break if no unvisited node is found

# Reconstruct the full path in the grid graph
full_path = reconstruct_full_path(tour, paths)

print("Optimal path visiting required nodes:", full_path)

# Visualize the solution
visualize_solution(G, full_path, required_nodes)

i have this code but the agent doesnt retrun home