Graph.go

// SPDX-License-Identifier: MIT
// Copyright (c) 2026 MaIII Themd

package dijkstra

import (
	"fmt"
	"os"
	"sync"
)

// StPath is one node in a returned shortest path. Cost is the
// cumulative weight from the source up to and including this node.
type StPath struct {
	Name    string
	X       float64
	Y       float64
	Heading float64
	Cost    float64
}

// StEdge is a directed edge from the owning vertex to ToVertexName.
//
// IsOneWay = true means the edge may only be traversed from the
// owning vertex to ToVertexName during search; the reverse direction
// will not be promoted to the shortest-path tree.
//
// isShort is set internally by DijkstraRun; it is unexported and not
// part of the public contract.
type StEdge struct {
	ToVertexName string
	Weight       float64
	isShort      bool
	IsOneWay     bool
}

// StVertex is one node in the graph.
//
// X / Y / Heading are optional metadata for spatial graphs. They are
// returned verbatim in StPath but are not consulted by the algorithm
// itself, so non-geometric users can leave them at zero.
type StVertex struct {
	Name       string
	Weight     float64
	Visited    bool
	X          float64
	Y          float64
	Heading    float64
	Edges      []StEdge
	MaskSearch bool
	Parent     string
}

// StGraph is a directed graph supporting Dijkstra shortest-path
// search and optional vertex blocking. The zero value is a usable
// empty graph.
//
// All exported methods are safe for concurrent use. Internally,
// methods named with a "Locked" suffix assume the caller already
// holds g.mu and are used by the search internals to avoid
// re-entrant locking.
type StGraph struct {
	mu     sync.RWMutex
	vertex []StVertex
	// index maps a vertex name to its slice position in vertex, giving
	// O(1) name lookups instead of an O(V) linear scan. It is kept in
	// sync by VertexAdd and VertexRemove. The zero value is nil and
	// reads of a nil map are safe, so an empty StGraph needs no
	// initialisation.
	index   map[string]int
	debugEn bool
	// blocked is the set of off-limits vertex names. A map gives O(1)
	// membership tests, which matters because the search consults it for
	// every edge it relaxes. The zero value is nil; reads of a nil map
	// are safe.
	blocked map[string]struct{}
	// search caches the integer-indexed working state (distances, parent
	// pointers, visited stamps and the priority-queue backing array) so a
	// search reuses them instead of allocating fresh ones every call. It
	// is only ever read or written while g.mu is held for writing (every
	// search takes the write lock), so it adds no new shared state outside
	// the existing locking. See search.go.
	search stSearch
	// topoVer is bumped on every structural change (add/remove a vertex or
	// edge). The search caches an integer-indexed adjacency built from the
	// name-keyed edges; comparing topoVer lets it reuse that adjacency
	// across searches on an unchanged graph instead of re-resolving every
	// edge's ToVertexName through the name map each time.
	topoVer uint64
}

// ---------------------------------------------------------------- //
// Vertex blocking
// ---------------------------------------------------------------- //
//
// Blocked vertices are not removed from the graph; instead, the search
// skips every edge leading into a blocked vertex, so a blocked vertex
// is simply unreachable (you may still route out of one if it happens
// to be the search's start).
//
// Blocking is dynamic: the blocked set is consulted on each
// DijkstraSearch / DijkstraRun, so vertices can be blocked or unblocked
// at any time -- before or after VertexAdd, and between searches -- and
// the next search reflects the change. (Earlier versions baked a cost
// penalty into edges at VertexAdd time and so required blocking up
// front; that is no longer the case.)

// VertexBLockClear empties the blocked-vertex set. The change takes
// effect on the next DijkstraSearch / DijkstraRun.
func (g *StGraph) VertexBLockClear() {
	g.mu.Lock()
	defer g.mu.Unlock()
	g.blocked = nil
}

// VertexBLockLoad adds names to the blocked-vertex set.
// Always returns true.
func (g *StGraph) VertexBLockLoad(name []string) bool {
	g.mu.Lock()
	defer g.mu.Unlock()
	if g.blocked == nil {
		g.blocked = make(map[string]struct{}, len(name))
	}
	for _, n := range name {
		g.blocked[n] = struct{}{}
	}
	return true
}

// VertexIsBLock reports whether name is currently in the blocked set.
func (g *StGraph) VertexIsBLock(name string) bool {
	g.mu.RLock()
	defer g.mu.RUnlock()
	return g.vertexIsBLockLocked(name)
}

func (g *StGraph) vertexIsBLockLocked(name string) bool {
	_, ok := g.blocked[name]
	return ok
}

// VertexBLock adds an existing vertex to the blocked set.
// Returns false if name does not exist or is already blocked.
func (g *StGraph) VertexBLock(name string) bool {
	g.mu.Lock()
	defer g.mu.Unlock()
	if g.vertexIsBLockLocked(name) {
		return false
	}
	if !g.vertexIsExistLocked(name) {
		return false
	}
	if g.blocked == nil {
		g.blocked = make(map[string]struct{})
	}
	g.blocked[name] = struct{}{}
	return true
}

// VertexBLockRemove removes name from the blocked set.
// Returns true if the set was already empty or the name was removed,
// false if name was present in neither case.
func (g *StGraph) VertexBLockRemove(name string) bool {
	g.mu.Lock()
	defer g.mu.Unlock()
	if len(g.blocked) == 0 {
		return true
	}
	if _, ok := g.blocked[name]; ok {
		delete(g.blocked, name)
		return true
	}
	return false
}

// ---------------------------------------------------------------- //
// Debug + printing
// ---------------------------------------------------------------- //

// Debug toggles verbose printing during graph operations and search.
func (g *StGraph) Debug(debugEn bool) {
	g.mu.Lock()
	defer g.mu.Unlock()
	g.debugEn = debugEn
}

// Print writes the whole graph to stdout. It is shorthand for
// Show(os.Stdout); use Show to write to any io.Writer, or ShowFunc to
// customise the per-vertex format.
func (g *StGraph) Print() { g.Show(os.Stdout) }

// PrintDijkstra writes every vertex's post-search state and its
// shortest-path-tree edges to stdout. Shorthand for
// ShowDijkstra(os.Stdout).
func (g *StGraph) PrintDijkstra() { g.ShowDijkstra(os.Stdout) }

// ---------------------------------------------------------------- //
// Vertex management
// ---------------------------------------------------------------- //

// VertexLength returns the number of vertices in the graph.
func (g *StGraph) VertexLength() int {
	g.mu.RLock()
	defer g.mu.RUnlock()
	return len(g.vertex)
}

// VertexFind returns the slice index of the vertex named `name`,
// or -1 if not found.
func (g *StGraph) VertexFind(name string) int {
	g.mu.RLock()
	defer g.mu.RUnlock()
	return g.vertexFindLocked(name)
}

func (g *StGraph) vertexFindLocked(name string) int {
	// Reading from a nil map is safe and reports the zero value, so this
	// also handles a freshly zero-valued graph before any VertexAdd.
	if i, ok := g.index[name]; ok {
		return i
	}
	return -1
}

// VertexIsExist reports whether a vertex with that name exists.
func (g *StGraph) VertexIsExist(name string) bool {
	g.mu.RLock()
	defer g.mu.RUnlock()
	return g.vertexIsExistLocked(name)
}

func (g *StGraph) vertexIsExistLocked(name string) bool {
	return g.vertexFindLocked(name) >= 0
}

// VertexAdd inserts a new vertex with the given name, optional
// (x, y, heading) metadata, and zero or more outgoing edges.
//
// Returns false if a vertex with this name already exists, or if any
// edge has a negative weight (Dijkstra requires non-negative weights).
//
// Blocking is applied at search time, not here, so edges may be added
// in any order relative to VertexBLock / VertexBLockLoad.
func (g *StGraph) VertexAdd(name string, x, y, heading float64, edges ...StEdge) bool {
	g.mu.Lock()
	defer g.mu.Unlock()

	if g.debugEn {
		fmt.Printf("name = %s\r\n", name)
	}
	if g.vertexIsExistLocked(name) {
		if g.debugEn {
			fmt.Printf("Duplicate vertex %s\r\n", name)
		}
		return false
	}

	// Dijkstra's algorithm only produces correct results with
	// non-negative edge weights, so reject negative ones up front
	// rather than silently returning a wrong shortest path later.
	for _, e := range edges {
		if e.Weight < 0 {
			if g.debugEn {
				fmt.Printf("rejecting vertex %s: negative weight %f on edge to %s\r\n",
					name, e.Weight, e.ToVertexName)
			}
			return false
		}
	}

	v := StVertex{
		Name:    name,
		X:       x,
		Y:       y,
		Heading: heading,
	}

	v.Edges = append(v.Edges, edges...)
	if g.debugEn {
		for _, e := range edges {
			fmt.Printf("edge add %s weight %f\r\n", e.ToVertexName, e.Weight)
		}
	}

	g.vertex = append(g.vertex, v)
	if g.index == nil {
		g.index = make(map[string]int, 1)
	}
	g.index[name] = len(g.vertex) - 1
	g.topoVer++
	return true
}

// VertexRemove deletes the named vertex from the graph together with
// every edge (from any vertex) that pointed at it, and drops it from the
// blocked set. Returns false if the vertex does not exist.
//
// The next DijkstraSearch / DijkstraRun recomputes from scratch, so any
// stale Dijkstra state left on other vertices is harmless.
func (g *StGraph) VertexRemove(name string) bool {
	g.mu.Lock()
	defer g.mu.Unlock()

	i := g.vertexFindLocked(name)
	if i < 0 {
		return false
	}

	// Splice the vertex out, preserving the order of the rest.
	g.vertex = append(g.vertex[:i], g.vertex[i+1:]...)

	// Drop any edges that pointed at the removed vertex.
	for vi := range g.vertex {
		edges := g.vertex[vi].Edges
		kept := edges[:0]
		for _, e := range edges {
			if e.ToVertexName != name {
				kept = append(kept, e)
			}
		}
		g.vertex[vi].Edges = kept
	}

	// Positions shifted, so rebuild the name index.
	g.index = make(map[string]int, len(g.vertex))
	for vi := range g.vertex {
		g.index[g.vertex[vi].Name] = vi
	}

	// Forget any blocked-set entry for the removed vertex.
	delete(g.blocked, name)

	g.topoVer++
	if g.debugEn {
		fmt.Printf("removed vertex %s\r\n", name)
	}
	return true
}

// VertexAddEdge adds an outgoing edge from an existing vertex to
// toVertex. As with VertexAdd, toVertex need not exist yet. Returns
// false if fromVertex does not exist, the weight is negative, or an edge
// from fromVertex to toVertex already exists.
func (g *StGraph) VertexAddEdge(fromVertex, toVertex string, weight float64, isOneWay bool) bool {
	g.mu.Lock()
	defer g.mu.Unlock()

	if weight < 0 {
		return false
	}
	i := g.vertexFindLocked(fromVertex)
	if i < 0 {
		return false
	}
	for _, e := range g.vertex[i].Edges {
		if e.ToVertexName == toVertex {
			return false // edge already exists
		}
	}
	g.vertex[i].Edges = append(g.vertex[i].Edges, StEdge{
		ToVertexName: toVertex,
		Weight:       weight,
		IsOneWay:     isOneWay,
	})
	g.topoVer++
	return true
}

// VertexRemoveEdge removes the edge from fromVertex to toVertex.
// Returns false if fromVertex or the edge does not exist.
func (g *StGraph) VertexRemoveEdge(fromVertex, toVertex string) bool {
	g.mu.Lock()
	defer g.mu.Unlock()

	i := g.vertexFindLocked(fromVertex)
	if i < 0 {
		return false
	}
	edges := g.vertex[i].Edges
	for j := range edges {
		if edges[j].ToVertexName == toVertex {
			g.vertex[i].Edges = append(edges[:j], edges[j+1:]...)
			g.topoVer++
			return true
		}
	}
	return false
}

// VertexSetXY updates the X and Y coordinates of an existing vertex.
// No-op if the vertex doesn't exist.
func (g *StGraph) VertexSetXY(name string, x, y float64) {
	g.mu.Lock()
	defer g.mu.Unlock()
	i := g.vertexFindLocked(name)
	if i >= 0 {
		g.vertex[i].X = x
		g.vertex[i].Y = y
	}
}

// VertexToStPath builds a snapshot StPath from the named vertex.
// Cost is taken from the vertex's current Weight (cumulative cost
// after a Dijkstra pass).
func (g *StGraph) VertexToStPath(name string) (bool, StPath) {
	g.mu.RLock()
	defer g.mu.RUnlock()
	return g.vertexToStPathLocked(name)
}

func (g *StGraph) vertexToStPathLocked(name string) (bool, StPath) {
	i := g.vertexFindLocked(name)
	if i < 0 {
		return false, StPath{}
	}
	return true, StPath{
		Name:    g.vertex[i].Name,
		X:       g.vertex[i].X,
		Y:       g.vertex[i].Y,
		Heading: g.vertex[i].Heading,
		Cost:    g.vertex[i].Weight,
	}
}

// ---------------------------------------------------------------- //
// Per-vertex Dijkstra state accessors
// ---------------------------------------------------------------- //

// VertexGetParent returns the parent name set during the last
// DijkstraRun, or "" if name doesn't exist or has no parent.
func (g *StGraph) VertexGetParent(name string) string {
	g.mu.RLock()
	defer g.mu.RUnlock()
	return g.vertexGetParentLocked(name)
}

func (g *StGraph) vertexGetParentLocked(name string) string {
	i := g.vertexFindLocked(name)
	if i < 0 {
		return ""
	}
	return g.vertex[i].Parent
}

// VertexSetParent records that `child`'s parent in the shortest-path
// tree is `parent`. Returns false if child doesn't exist.
func (g *StGraph) VertexSetParent(child, parent string) bool {
	g.mu.Lock()
	defer g.mu.Unlock()
	return g.vertexSetParentLocked(child, parent)
}

func (g *StGraph) vertexSetParentLocked(child, parent string) bool {
	i := g.vertexFindLocked(child)
	if i < 0 {
		return false
	}
	g.vertex[i].Parent = parent
	return true
}

// VertexGetWeight returns the cumulative cost recorded on the vertex
// during the last DijkstraRun, or 0 if the vertex doesn't exist.
func (g *StGraph) VertexGetWeight(name string) float64 {
	g.mu.RLock()
	defer g.mu.RUnlock()
	return g.vertexGetWeightLocked(name)
}

func (g *StGraph) vertexGetWeightLocked(name string) float64 {
	i := g.vertexFindLocked(name)
	if i < 0 {
		return 0
	}
	return g.vertex[i].Weight
}

// VertexSetWeight overwrites the cumulative cost on a vertex.
func (g *StGraph) VertexSetWeight(name string, weight float64) {
	g.mu.Lock()
	defer g.mu.Unlock()
	g.vertexSetWeightLocked(name, weight)
}

func (g *StGraph) vertexSetWeightLocked(name string, weight float64) {
	i := g.vertexFindLocked(name)
	if i >= 0 {
		g.vertex[i].Weight = weight
	}
}

// VertexIsVisited reports whether the vertex has been settled by
// the last DijkstraRun.
func (g *StGraph) VertexIsVisited(name string) bool {
	g.mu.RLock()
	defer g.mu.RUnlock()
	return g.vertexIsVisitedLocked(name)
}

func (g *StGraph) vertexIsVisitedLocked(name string) bool {
	i := g.vertexFindLocked(name)
	if i < 0 {
		return false
	}
	return g.vertex[i].Visited
}

// VertexSetVisited marks a vertex as settled.
func (g *StGraph) VertexSetVisited(name string) {
	g.mu.Lock()
	defer g.mu.Unlock()
	g.vertexSetVisitedLocked(name)
}

func (g *StGraph) vertexSetVisitedLocked(name string) {
	i := g.vertexFindLocked(name)
	if i >= 0 {
		g.vertex[i].Visited = true
	}
}

// VertexIsMasked reports whether a vertex has been touched by the
// path-reconstruction stack walk.
func (g *StGraph) VertexIsMasked(name string) bool {
	g.mu.RLock()
	defer g.mu.RUnlock()
	return g.vertexIsMaskedLocked(name)
}

func (g *StGraph) vertexIsMaskedLocked(name string) bool {
	i := g.vertexFindLocked(name)
	if i < 0 {
		return false
	}
	return g.vertex[i].MaskSearch
}

// VertexSetMask marks a vertex as touched by the path-reconstruction
// stack walk.
func (g *StGraph) VertexSetMask(name string) {
	g.mu.Lock()
	defer g.mu.Unlock()
	g.vertexSetMaskLocked(name)
}

func (g *StGraph) vertexSetMaskLocked(name string) {
	i := g.vertexFindLocked(name)
	if i >= 0 {
		g.vertex[i].MaskSearch = true
	}
}

// ---------------------------------------------------------------- //
// Edge accessors
// ---------------------------------------------------------------- //

// EdgeIsOneWay reports whether the edge from fromVertex to toVertex
// is marked one-way. Returns false if no such edge exists.
func (g *StGraph) EdgeIsOneWay(fromVertex, toVertex string) bool {
	g.mu.RLock()
	defer g.mu.RUnlock()
	return g.edgeIsOneWayLocked(fromVertex, toVertex)
}

func (g *StGraph) edgeIsOneWayLocked(fromVertex, toVertex string) bool {
	i := g.vertexFindLocked(fromVertex)
	if i < 0 {
		return false
	}
	for _, e := range g.vertex[i].Edges {
		if e.ToVertexName == toVertex {
			return e.IsOneWay
		}
	}
	return false
}

// MaskShortEdge marks the edge from fromVertex to toVertex as part
// of the shortest-path tree. Returns false if no such edge exists.
func (g *StGraph) MaskShortEdge(fromVertex, toVertex string) bool {
	g.mu.Lock()
	defer g.mu.Unlock()
	return g.maskShortEdgeLocked(fromVertex, toVertex)
}

func (g *StGraph) maskShortEdgeLocked(fromVertex, toVertex string) bool {
	i := g.vertexFindLocked(fromVertex)
	if i < 0 {
		return false
	}
	for j := range g.vertex[i].Edges {
		if g.vertex[i].Edges[j].ToVertexName == toVertex {
			g.vertex[i].Edges[j].isShort = true
			return true
		}
	}
	return false
}

// EdgeExist reports whether an edge from fromVertex to toVertex exists.
func (g *StGraph) EdgeExist(fromVertex, toVertex string) bool {
	g.mu.RLock()
	defer g.mu.RUnlock()
	i := g.vertexFindLocked(fromVertex)
	if i < 0 {
		return false
	}
	for _, e := range g.vertex[i].Edges {
		if e.ToVertexName == toVertex {
			return true
		}
	}
	return false
}

// EdgeGetWeight returns the weight of the edge from fromVertex to
// toVertex, or 0 if no such edge exists.
func (g *StGraph) EdgeGetWeight(fromVertex, toVertex string) float64 {
	g.mu.RLock()
	defer g.mu.RUnlock()
	return g.edgeGetWeightLocked(fromVertex, toVertex)
}

func (g *StGraph) edgeGetWeightLocked(fromVertex, toVertex string) float64 {
	i := g.vertexFindLocked(fromVertex)
	if i < 0 {
		return 0
	}
	for _, e := range g.vertex[i].Edges {
		if e.ToVertexName == toVertex {
			return e.Weight
		}
	}
	return 0
}

// ---------------------------------------------------------------- //
// Spatial helpers (only useful when X / Y are populated)
// ---------------------------------------------------------------- //

// NearPoint returns the name and distance (in cm) of the vertex
// whose (X, Y) is closest to (x, y). Returns "" and a very large
// number on an empty graph.
func (g *StGraph) NearPoint(x, y float64) (string, float64) {
	g.mu.RLock()
	defer g.mu.RUnlock()
	min := 1e9
	minName := ""
	for _, v := range g.vertex {
		d := DistanceCM(v.X, v.Y, x, y)
		if d < min {
			min = d
			minName = v.Name
		}
	}
	return minName, min
}

// DistanceCMToVertex returns the cm distance from (x, y) to the
// vertex named `name`. Returns false, 0 if the vertex doesn't exist.
func (g *StGraph) DistanceCMToVertex(x, y float64, name string) (bool, float64) {
	g.mu.RLock()
	defer g.mu.RUnlock()
	i := g.vertexFindLocked(name)
	if i < 0 {
		return false, 0
	}
	return true, DistanceCM(g.vertex[i].X, g.vertex[i].Y, x, y)
}