package sauce

type Node struct {
	parent  *Node
	x, y    int
	f, g, h int
}

func FindPath(w *World, fromX, fromY int, toX, toY int) []Node {
	openList := make([]*Node, 0)
	closedList := make([]*Node, 0)

	startNode := &Node{x: fromX, y: fromY}
	endNode := &Node{x: toX, y: toY}

	openList = append(openList, startNode)

	for len(openList) > 0 {
		var cnode *Node
		var cindex int
		for i, node := range openList {
			if cnode == nil || node.f < cnode.f {
				cnode = node
				cindex = i
			}
		}
		openList = append(openList[:cindex], openList[cindex+1:]...)
		closedList = append(closedList, cnode)

		if cnode.x == endNode.x && cnode.y == endNode.y {
			path := make([]Node, 0)
			for cnode != nil {
				path = append(path, *cnode)
				cnode = cnode.parent
			}
			return path
		}

		var neighbors []Node
		for x := -1; x <= 1; x++ {
			for y := -1; y <= 1; y++ {
				if x == 0 && y == 0 {
					continue
				}
				if x != 0 && y != 0 {
					continue
				}
				if cell := w.At(cnode.x+x, cnode.y+y); cell != nil {
					neighbors = append(neighbors, Node{x: cnode.x + x, y: cnode.y + y, parent: cnode})
				}
			}
		}

		for _, neighbor := range neighbors {
			exists := false
			for _, node := range closedList {
				if node.x == neighbor.x && node.y == neighbor.y {
					exists = true
					break
				}
			}
			if exists {
				continue
			}

			exists = false
			for _, node := range openList {
				if node.x == neighbor.x && node.y == neighbor.y {
					exists = true
					break
				}
			}
			if exists {
				continue
			}

			neighbor.g = cnode.g + 1                             // this value potentially should be a float and modified if a diagonal.
			neighbor.h = (toX - neighbor.x) + (toY - neighbor.y) // Maybe should be calculated differently.
			neighbor.f = neighbor.g + neighbor.h

			openList = append(openList, &neighbor)
		}
	}
	return nil
}