'''
make the graoh grid graph and not all nodes need to be visited

To modify your code according to your requirements:

1. Grid Graph: Instead of using a complete graph (where every node is connected to every other node), we'll generate a grid graph where nodes are connected to their immediate neighbors (up, down, left, right). This mimics a 2D grid layout.

2. Partial Node Visit: We'll select a subset of nodes (required nodes) that need to be visited. The objective is to find the shortest path that visits all these required nodes, without the necessity of visiting every node in the grid.

Below is the modified code to achieve this:
'''
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)

'''

Explanation:

- create_grid_graph: Generates a 2D grid graph where each node is connected to its immediate neighbors.
- compute_shortest_paths: Computes the shortest paths and distances between all pairs of required nodes using Dijkstra's algorithm.
- create_complete_graph: Creates a complete directed graph of the required nodes, where the edge weights are the shortest path distances in the grid graph.
- solve_tsp_complete_graph: Solves the TSP on the complete graph of required nodes using the Gurobi optimizer.
- reconstruct_full_path: Reconstructs the full path in the grid graph by concatenating the shortest paths between required nodes according to the TSP solution.
- visualize_solution: Visualizes the grid graph, highlights the required nodes, and shows the optimal path that visits all required nodes.

Notes:

- The Miller-Tucker-Zemlin (MTZ) formulation is used to eliminate subtours in the TSP solution.
- Since not all nodes need to be visited, this approach focuses only on the required nodes but ensures the path is valid in the grid graph.
- The reconstructed full path respects the grid connectivity and includes intermediate nodes between required nodes.
- The visualization helps to confirm the solution is valid and follows the grid structure.

Example Output:

Optimal path visiting required nodes: [(0, 0), (1, 0), (2, 0), (2, 1), (2, 2), (2, 3), (2, 4), (1, 4), (0, 4), (1, 4), (2, 4), (3, 4), (4, 4)]

This path starts at (0,0), visits all required nodes, and ends at (4,4), minimizing the total distance while moving through the grid.
'''
"I have this code where I want to create 2 agents with different starting pointsdes they must visit. I need to calculate the path for each agent and check for any collisions between their paths." and a set of no