"""Build forward and reverse adjacency lists from edge arrays."""

fwd = [[] for _ in range(nbnode)]

rev = [[] for _ in range(nbnode)]

for i in range(len(gfrom)):

fwd[gfrom[i]].append((gto[i], gw[i], i))

rev[gto[i]].append((gfrom[i], gw[i], i))

"""Standard Dijkstra returning distance and parent arrays."""

dist = np.full(nbnode, np.inf)

parent = np.full(nbnode, -1, dtype=np.intp)

dist[source] = 0.0

pq = [(0.0, source)]

while pq:

d, u = heapq.heappop(pq)

if d > dist[u]:

continue

for v, w, _ in adj[u]:

nd = d + w

if nd < dist[v]:

dist[v] = nd

parent[v] = u

heapq.heappush(pq, (nd, v))

"""Dijkstra with early stopping when target is settled."""

dist = np.full(nbnode, np.inf)

parent = np.full(nbnode, -1, dtype=np.intp)

dist[source] = 0.0

pq = [(0.0, source)]

while pq:

d, u = heapq.heappop(pq)

if d > dist[u]:

continue

if u == target:

break

for v, w, _ in adj[u]:

nd = d + w

if nd < dist[v]:

dist[v] = nd

parent[v] = u

heapq.heappush(pq, (nd, v))

"""Bidirectional Dijkstra returning shortest distance and meeting node."""

if source == target:

return 0.0, source, np.zeros(nbnode), np.zeros(nbnode), np.full(nbnode, -1, dtype=np.intp), np.full(nbnode, -1, dtype=np.intp)

dist_f = np.full(nbnode, np.inf)

dist_r = np.full(nbnode, np.inf)

parent_f = np.full(nbnode, -1, dtype=np.intp)

parent_r = np.full(nbnode, -1, dtype=np.intp)

dist_f[source] = 0.0

dist_r[target] = 0.0

pq_f = [(0.0, source)]

pq_r = [(0.0, target)]

settled_f = set()

settled_r = set()

mu = np.inf

meeting = -1

while pq_f or pq_r:

# Check termination

df_min = pq_f[0][0] if pq_f else np.inf

dr_min = pq_r[0][0] if pq_r else np.inf

if df_min + dr_min >= mu:

break

# Forward step

if pq_f and df_min <= dr_min:

d, u = heapq.heappop(pq_f)

if d > dist_f[u]:

continue

settled_f.add(u)

if u in settled_r and dist_f[u] + dist_r[u] < mu:

mu = dist_f[u] + dist_r[u]

meeting = u

for v, w, _ in fwd[u]:

nd = d + w

if nd < dist_f[v]:

dist_f[v] = nd

parent_f[v] = u

heapq.heappush(pq_f, (nd, v))

if v in settled_r and nd + dist_r[v] < mu:

mu = nd + dist_r[v]

meeting = v

else:

d, u = heapq.heappop(pq_r)

if d > dist_r[u]:

continue

settled_r.add(u)

if u in settled_f and dist_f[u] + dist_r[u] < mu:

mu = dist_f[u] + dist_r[u]

meeting = u

for v, w, _ in rev[u]:

nd = d + w

if nd < dist_r[v]:

dist_r[v] = nd

parent_r[v] = u

heapq.heappush(pq_r, (nd, v))

if v in settled_f and dist_f[v] + nd < mu:

mu = dist_f[v] + nd

meeting = v

"""A* search with Euclidean heuristic."""

dist = np.full(nbnode, np.inf)

parent = np.full(nbnode, -1, dtype=np.intp)

dist[source] = 0.0

tx, ty = coords_x[target], coords_y[target]

def h(n):

dx = coords_x[n] - tx

dy = coords_y[n] - ty

return math.sqrt(dx * dx + dy * dy) / constant

pq = [(h(source), 0.0, source)]

while pq:

_, d, u = heapq.heappop(pq)

if d > dist[u]:

continue

if u == target:

break

for v, w, _ in fwd[u]:

nd = d + w

if nd < dist[v]:

dist[v] = nd

parent[v] = u

heapq.heappush(pq, (nd + h(v), nd, v))

"""Reconstruct path from parent array, returning node reference IDs."""

if parent[target] == -1 and source != target:

return []

path = []

cur = target

while cur != -1:

path.append(node_refs[cur])

if cur == source:

break

cur = parent[cur]

path.reverse()

"""Reconstruct path from bidirectional search."""

if meeting == -1:

return []

# Forward path: source -> meeting

path_f = []

cur = meeting

while cur != -1:

path_f.append(cur)

if cur == source:

break

cur = parent_f[cur]

path_f.reverse()

# Reverse path: meeting -> target

path_r = []

cur = parent_r[meeting]

while cur != -1:

path_r.append(cur)

if cur == target:

break

cur = parent_r[cur]

full = path_f + path_r

"""Dijkstra that stops when cost exceeds limit. Returns reachable nodes."""

dist = np.full(nbnode, np.inf)

dist[source] = 0.0

pq = [(0.0, source)]

reachable = []

while pq:

d, u = heapq.heappop(pq)

if d > dist[u]:

continue

if d <= limit and u != source:

reachable.append(u)

for v, w, _ in adj[u]:

nd = d + w

if nd <= limit and nd < dist[v]:

dist[v] = nd

heapq.heappush(pq, (nd, v))

"""Dijkstra minimizing main weight, then aggregate auxiliary weight along path."""

dist = np.full(nbnode, np.inf)

parent = np.full(nbnode, -1, dtype=np.intp)

parent_edge = np.full(nbnode, -1, dtype=np.intp)

dist[source] = 0.0

pq = [(0.0, source)]

while pq:

d, u = heapq.heappop(pq)

if d > dist[u]:

continue

if u == target:

break

for v, w, eidx in adj[u]:

nd = d + w

if nd < dist[v]:

dist[v] = nd

parent[v] = u

parent_edge[v] = eidx

heapq.heappush(pq, (nd, v))

# Aggregate aux weight along path

if dist[target] == np.inf:

return np.inf

aux_sum = 0.0

cur = target

while cur != source and cur != -1:

eidx = parent_edge[cur]

if eidx >= 0:

aux_sum += adj_aux_weights[eidx]

cur = parent[cur]

"""Dijkstra from one source, returning distances to all nodes."""

dist = np.full(nbnode, np.inf)

dist[source] = 0.0

pq = [(0.0, source)]

while pq:

d, u = heapq.heappop(pq)

if d > dist[u]:

continue

for v, w, _ in adj[u]:

nd = d + w

if nd < dist[v]:

dist[v] = nd

heapq.heappush(pq, (nd, v))

"""Contraction hierarchies preprocessing.

fwd = defaultdict(list)

rev = defaultdict(list)

for i in range(len(gfrom)):

fwd[gfrom[i]].append((gto[i], gw[i]))

rev[gto[i]].append((gfrom[i], gw[i]))

contracted = [False] * nbnode

rank = np.zeros(nbnode, dtype=np.intp)

shortcut_from = []

shortcut_to = []

shortcut_via = []

# All edges (including shortcuts added during contraction)

all_edges_from = list(gfrom)

all_edges_to = list(gto)

all_edges_w = list(gw)

def edge_difference(node):

"""Heuristic: shortcuts_needed - edges_removed."""

if contracted[node]:

return float('inf')

in_edges = [(u, w) for u, w in rev[node] if not contracted[u]]

out_edges = [(v, w) for v, w in fwd[node] if not contracted[v]]

shortcuts = 0

for u, wu in in_edges:

for v, wv in out_edges:

if u == v:

continue

# Check if u->v needs a shortcut (witness search)

max_cost = wu + wv

if not _witness_found(fwd, contracted, u, v, node, max_cost, nbnode):

shortcuts += 1

return shortcuts - len(in_edges) - len(out_edges)

def _witness_found(adj, contracted_arr, source, target, excluded, max_cost, nn):

"""Limited Dijkstra to check if alternative path exists."""

dist = {}

dist[source] = 0.0

pq = [(0.0, source)]

hops = 0

max_hops = 5 # limit search depth

while pq and hops < max_hops * len(pq):

d, u = heapq.heappop(pq)

if d > max_cost:

break

if u == target:

return True

if d > dist.get(u, np.inf):

continue

hops += 1

for v, w in adj.get(u, []):

if contracted_arr[v] or v == excluded:

continue

nd = d + w

if nd < dist.get(v, np.inf) and nd <= max_cost:

dist[v] = nd

heapq.heappush(pq, (nd, v))

return dist.get(target, np.inf) <= max_cost

# Compute initial ordering

order = [(edge_difference(n), n) for n in range(nbnode)]

heapq.heapify(order)

rank_counter = 0

contracted_count = 0

while order:

# Lazy update: recompute priority for top node

_, node = heapq.heappop(order)

if contracted[node]:

continue

# Recompute (lazy update)

new_prio = edge_difference(node)

if order and new_prio > order[0][0]:

heapq.heappush(order, (new_prio, node))

continue

# Contract this node

contracted[node] = True

rank[node] = rank_counter

rank_counter += 1

if verbose and rank_counter % 1000 == 0:

print(f"Contracted {rank_counter}/{nbnode} nodes")

in_edges = [(u, w) for u, w in rev[node] if not contracted[u]]

out_edges = [(v, w) for v, w in fwd[node] if not contracted[v]]

for u, wu in in_edges:

for v, wv in out_edges:

if u == v:

continue

sc_w = wu + wv

if not _witness_found(fwd, contracted, u, v, node, sc_w, nbnode):

# Add shortcut u -> v

fwd[u].append((v, sc_w))

rev[v].append((u, sc_w))

all_edges_from.append(u)

all_edges_to.append(v)

all_edges_w.append(sc_w)

shortcut_from.append(u)

shortcut_to.append(v)

shortcut_via.append(node)

contracted_count += 1

# Store all edges (original + shortcuts) for the CH query

return (

np.array(all_edges_from, dtype=np.intp),

np.array(all_edges_to, dtype=np.intp),

np.array(all_edges_w),

rank,

np.array(shortcut_from, dtype=np.intp),

np.array(shortcut_to, dtype=np.intp),

np.array(shortcut_via, dtype=np.intp),

"""Contraction hierarchies query: modified bidirectional Dijkstra on contracted graph.

if source == target:

return 0.0

# Build upward-only adjacency lists

# fwd_up[u] = [(v, w)] for edges u->v with rank[u] < rank[v]

# rev_up[v] = [(u, w)] for edges u->v with rank[v] < rank[u] (reverse search goes v->u upward)

fwd_up = [[] for _ in range(nbnode)]

rev_up = [[] for _ in range(nbnode)]

for i in range(len(c_from)):

u, v, w = c_from[i], c_to[i], c_w[i]

if rank[u] < rank[v]:

fwd_up[u].append((v, w))

if rank[v] < rank[u]:

# Reverse edge: from v we can reach u going upward

rev_up[v].append((u, w))

dist_f = np.full(nbnode, np.inf)

dist_r = np.full(nbnode, np.inf)

dist_f[source] = 0.0

dist_r[target] = 0.0

pq_f = [(0.0, source)]

pq_r = [(0.0, target)]

mu = np.inf

while pq_f or pq_r:

# Forward step (only upward edges)

if pq_f:

d, u = heapq.heappop(pq_f)

if d > dist_f[u]:

pass

else:

# Check if this node was reached by reverse search

if dist_f[u] + dist_r[u] < mu:

mu = dist_f[u] + dist_r[u]

for v, w in fwd_up[u]:

nd = d + w

if nd < dist_f[v]:

dist_f[v] = nd

heapq.heappush(pq_f, (nd, v))

# Reverse step (follow reverse edges upward)

if pq_r:

d, u = heapq.heappop(pq_r)

if d > dist_r[u]:

pass

else:

if dist_f[u] + dist_r[u] < mu:

mu = dist_f[u] + dist_r[u]

for v, w in rev_up[u]:

nd = d + w

if nd < dist_r[v]:

dist_r[v] = nd

heapq.heappush(pq_r, (nd, v))

# Termination: both queues exhausted or min keys exceed best

df_min = pq_f[0][0] if pq_f else np.inf

dr_min = pq_r[0][0] if pq_r else np.inf

if min(df_min, dr_min) >= mu:

break

"""Remove non-intersection nodes from graph.

def _one_pass(ef, et, ew, nn, keep_mask):

# Build adjacency

fwd = defaultdict(list)

rev = defaultdict(list)

for i in range(len(ef)):

fwd[ef[i]].append((et[i], ew[i], i))

rev[et[i]].append((ef[i], ew[i], i))

# Identify removable nodes: in-degree=1, out-degree=1, not kept

removable = set()

for n in range(nn):

if keep_mask[n]:

continue

in_deg = len(rev.get(n, []))

out_deg = len(fwd.get(n, []))

if in_deg == 1 and out_deg == 1:

# Check it's not a self-loop

pred = rev[n][0][0]

succ = fwd[n][0][0]

if pred != n and succ != n:

removable.add(n)

if not removable:

return ef, et, ew, 0

# Build new edges bypassing removable nodes

used = [False] * len(ef)

new_from = []

new_to = []

new_w = []

for n in removable:

# Already processed in a chain

pass

# Process chains

visited = set()

for n in removable:

if n in visited:

continue

# Walk backward to find chain start

chain_start_pred = rev[n][0][0]

chain_start_edge_w = rev[n][0][2]

cur = n

total_w = rev[cur][0][1]

visited.add(cur)

# Walk forward

nxt = fwd[cur][0][0]

total_w += fwd[cur][0][1]

# Mark edges as used

for i in range(len(ef)):

if (ef[i] == chain_start_pred and et[i] == cur) or (ef[i] == cur and et[i] == nxt):

used[i] = True

while nxt in removable and nxt not in visited:

visited.add(nxt)

for i in range(len(ef)):

if ef[i] == nxt and et[i] == fwd[nxt][0][0]:

used[i] = True

total_w_add = fwd[nxt][0][1]

nxt_next = fwd[nxt][0][0]

total_w += total_w_add

# Also mark edge from chain_start_pred to cur

nxt = nxt_next

# Need to recalculate total weight properly

# Walk the chain again from start

total_w = 0.0

cur = n

visited2 = set()

# Find the actual predecessor edge weight

for i in range(len(ef)):

if ef[i] == chain_start_pred and et[i] == n:

total_w += ew[i]

used[i] = True

break

while cur in removable and cur not in visited2:

visited2.add(cur)

succ = fwd[cur][0][0]

for i in range(len(ef)):

if ef[i] == cur and et[i] == succ:

total_w += ew[i]

used[i] = True

break

cur = succ

new_from.append(chain_start_pred)

new_to.append(cur)

new_w.append(total_w)

# Keep non-used original edges

for i in range(len(ef)):

if not used[i]:

new_from.append(ef[i])

new_to.append(et[i])

new_w.append(ew[i])

return new_from, new_to, new_w, len(removable)

ef = list(gfrom)

et = list(gto)

ew = list(gw)

keep_mask = list(keep)

total_removed = 0

iteration = 0

while True:

ef, et, ew, removed = _one_pass(ef, et, ew, nbnode, keep_mask)

total_removed += removed

iteration += 1

if verbose:

print(f"Iteration {iteration}: removed {removed} nodes")

if not iterate or removed == 0:

break

# Remove loops if requested

if rm_loop:

filtered = [(f, t, w) for f, t, w in zip(ef, et, ew) if f != t]

if filtered:

ef, et, ew = zip(*filtered)

else:

ef, et, ew = [], [], []

# Remove duplicate edges (keep minimum weight)

edge_dict = {}

for f, t, w in zip(ef, et, ew):

key = (f, t)

if key not in edge_dict or w < edge_dict[key]:

edge_dict[key] = w

result_from = []

result_to = []

result_w = []

for (f, t), w in edge_dict.items():

result_from.append(f)

result_to.append(t)

result_w.append(w)