#Libraries To Import For Doing Network Analysis using QGis
#Using The Core QGIS functions and we're using PyQT classes for graph building and layer manipulation to learn 2 points to another
import sys
import os
from qgis.analysis import QgsGraphBuilder #Used To analyze two points
from qgis.analysis import QgsGraphAnalyzer #Uses Djikstra's Algorithm to find the most optimal path between 2 points

from qgis.core import (
	QgsApplication,
	QgsWkbTypes,
	QgsProject, #Project Manager
	QgsVectorLayer, #To Load Shapefiles and geopackages
	QgsFeature, #Single row in a vector layer
	QgsGeometry, #For mathematical geometry operations
	QgsPointXY, #Makes a 2D point
	QgsPoint,
	QgsFields,
	QgsField, #For multple attribute schemas and individual attribute field
	QgsVectorFileWriter #For Package Export
)

from qgis.PyQt.QtCore import QVariant  # Used to define field data types

#Now we load the Philippine roads dataset from a non-profit organization called Humanitarian Open Street map team
#This will give us an open dataset about roads in NCR and give us the shortest path between two points
#Setup QGIS Paths
QGIS_PREFIX_PATH = "/usr"  # Common for Ubuntu QGIS installs
QGIS_PLUGIN_PATH = "/usr/lib/qgis/plugins"

qgs = QgsApplication([], False)
qgs.setPrefixPath(QGIS_PREFIX_PATH, True)
qgs.initQgis()


layer = QgsVectorLayer("phl_roads_lines.gpkg", "roads", "ogr")

#print("Layer loaded:", layer.name()) 

#We are going to find shortest path for Quezon city
start_point = QgsPointXY(121.0365, 14.6760)
end_point = QgsPointXY(121.0000, 14.5550)

tolerance = 0.001 #to snap nearest vortex
builder = QgsGraphBuilder(layer.sourceCrs())

points = [start_point, end_point]

graph = builder.graph()

#getting the feature graphs
for feature in layer.getFeatures():
	geom = feature.geometry()
	if geom.isMultipart():
		lines = geom.asMultiPolyline()
	else:
		lines = [geom.asPolyline()]
	for line in lines:
		for i in range(len(line) -  1):
			#Then we convert it to a QgisPoint
			p1 = QgsPointXY(line[i].x(), line[i].y())
			p2 = QgsPointXY(line[i + 1].x(), line[i + 1].y())

			builder.addVertex(-1, p1)
			builder.addVertex(-1, p2)

			id1 = builder.graph().findVertex(p1)
			id2 = builder.graph().findVertex(p2)

			distance = p1.distance(p2)

			builder.addEdge(id1, p1, id2, p2, [distance])

graph = builder.graph()


# Convert the coordinates (start_point and end_point) into node indexes in the graph
start_idx = graph.findVertex(start_point)
end_idx = graph.findVertex(end_point)

# Use Dijkstra's algorithm to compute the shortest path tree from the start node
# 0 is the index for cost (in our case: distance)
tree, cost = QgsGraphAnalyzer.dijkstra(graph, start_idx, 0)

# If cost is infinite, it means there’s no possible path between start and end
if cost[end_idx] == float('inf'):
    print("No Path Found")
    sys.exit(1)

# Backtrack from end point to start to extract the shortest path
route = []
cur_pos = end_idx
while cur_pos != start_idx:
    incoming_edge = tree[cur_pos]  # The edge that leads to current vertex
    if incoming_edge == -1:
        print("No Route!")
        sys.exit(1)
    
    # Get the source vertex of this edge, and retrieve its point
    from_vertex = graph.edge(incoming_edge).fromVertex()
    point = graph.vertex(from_vertex).point()
    route.append(point)

    # Move to the previous vertex
    cur_pos = from_vertex

# Add the final starting point to complete the route
route.append(start_point)
route.reverse()  # Reverse because we built the route from end to start

# Define fields for the output layer
fields = QgsFields()
fields.append(QgsField("id", QVariant.Int))  # We'll assign an ID = 1 to this feature

# Define the GeoPackage file path for saving the shortest path result
output_path = "shortest_path.gpkg"

# Create the output vector layer writer (LineString geometry, same CRS as roads layer)
output_layer = QgsVectorFileWriter.create(
    output_path,         # File to save
    fields,              # Attribute schema
    QgsWkbTypes.LineString,  # Geometry type
    layer.sourceCrs(),   # Coordinate reference system
    "GPKG"               # Format
)

# Create a feature from the route geometry
feat = QgsFeature()
feat.setFields(fields)
feat.setAttribute("id", 1)
feat.setGeometry(QgsGeometry.fromPolylineXY(route))

# Add the feature to the output file
output_layer.addFeature(feat)

# Close and save the layer to disk
del output_layer

print(f"✅ Shortest path successfully saved to: {output_path}")

# Exit QGIS to clean up resources and avoid memory leaks
qgs.exitQgis()