goonscape/game/pathfinding.go

package game

import (
	"container/heap"
	"fmt"

	"gitea.boner.be/bdnugget/goonscape/types"
)

// Node represents a node in the A* pathfinding algorithm
type Node struct {
	Tile    types.Tile
	Parent  *Node
	G, H, F float32 // G = cost from start, H = heuristic to goal, F = G + H
}

// PriorityQueue implements a min-heap for nodes ordered by F value
type PriorityQueue []*Node

// Implement the heap.Interface for PriorityQueue
func (pq PriorityQueue) Len() int { return len(pq) }

func (pq PriorityQueue) Less(i, j int) bool {
	return pq[i].F < pq[j].F
}

func (pq PriorityQueue) Swap(i, j int) {
	pq[i], pq[j] = pq[j], pq[i]
}

func (pq *PriorityQueue) Push(x interface{}) {
	item := x.(*Node)
	*pq = append(*pq, item)
}

func (pq *PriorityQueue) Pop() interface{} {
	old := *pq
	n := len(old)
	item := old[n-1]
	*pq = old[0 : n-1]
	return item
}

// Helper to check if tile is in priority queue
func isInQueue(queue *PriorityQueue, tile types.Tile) (bool, *Node) {
	for _, node := range *queue {
		if node.Tile.X == tile.X && node.Tile.Y == tile.Y {
			return true, node
		}
	}
	return false, nil
}

// FindPath implements A* pathfinding algorithm with a priority queue
func FindPath(start, end types.Tile) []types.Tile {
	// Initialize open and closed sets
	openSet := &PriorityQueue{}
	heap.Init(openSet)

	closedSet := make(map[[2]int]bool)

	// Create start node and add to open set
	startNode := &Node{
		Tile:   start,
		Parent: nil,
		G:      0,
		H:      heuristic(start, end),
	}
	startNode.F = startNode.G + startNode.H
	heap.Push(openSet, startNode)

	// Main search loop
	for openSet.Len() > 0 {
		// Get node with lowest F score
		current := heap.Pop(openSet).(*Node)

		// If we reached the goal, reconstruct and return the path
		if current.Tile.X == end.X && current.Tile.Y == end.Y {
			return reconstructPath(current)
		}

		// Add current to closed set
		closedSet[[2]int{current.Tile.X, current.Tile.Y}] = true

		// Check all neighbors
		for _, neighbor := range GetNeighbors(current.Tile) {
			// Skip if in closed set or not walkable
			if !neighbor.Walkable || closedSet[[2]int{neighbor.X, neighbor.Y}] {
				continue
			}

			// Calculate tentative G score
			tentativeG := current.G + distance(current.Tile, neighbor)

			// Check if in open set
			inOpen, existingNode := isInQueue(openSet, neighbor)

			// If not in open set or better path found
			if !inOpen || tentativeG < existingNode.G {
				// Create or update the node
				var neighborNode *Node
				if inOpen {
					neighborNode = existingNode
				} else {
					neighborNode = &Node{
						Tile:   neighbor,
						Parent: current,
					}
				}

				// Update scores
				neighborNode.G = tentativeG
				neighborNode.H = heuristic(neighbor, end)
				neighborNode.F = neighborNode.G + neighborNode.H
				neighborNode.Parent = current

				// Add to open set if not already there
				if !inOpen {
					heap.Push(openSet, neighborNode)
				}
			}
		}
	}

	// No path found
	return nil
}

// reconstructPath builds the path from goal node to start
func reconstructPath(node *Node) []types.Tile {
	path := []types.Tile{}
	current := node

	// Follow parent pointers back to start
	for current != nil {
		path = append([]types.Tile{current.Tile}, path...)
		current = current.Parent
	}

	fmt.Printf("Path found: %v\n", path)
	return path
}

// heuristic estimates cost from current to goal (Manhattan distance)
func heuristic(a, b types.Tile) float32 {
	return float32(abs(a.X-b.X) + abs(a.Y-b.Y))
}

// distance calculates cost between adjacent tiles
func distance(a, b types.Tile) float32 {
	return 1.0 // uniform cost for now
}

// GetNeighbors returns walkable tiles adjacent to the given tile
func GetNeighbors(tile types.Tile) []types.Tile {
	directions := [][2]int{
		{1, 0}, {-1, 0}, {0, 1}, {0, -1},
		{1, 1}, {-1, -1}, {1, -1}, {-1, 1},
	}
	neighbors := []types.Tile{}
	grid := GetMapGrid()
	for _, dir := range directions {
		nx, ny := tile.X+dir[0], tile.Y+dir[1]
		if nx >= 0 && nx < types.MapWidth && ny >= 0 && ny < types.MapHeight {
			if grid[nx][ny].Walkable {
				neighbors = append(neighbors, grid[nx][ny])
			}
		}
	}
	return neighbors
}

// abs returns the absolute value of x
func abs(x int) int {
	if x < 0 {
		return -x
	}
	return x
}