chore(day17): implement fibHeap for faster priority queue

aoc_test.go

@@ -85,36 +85,35 @@ func TestPriorityQueue(t *testing.T) {
 	is := is.New(t)
 
 	type elem [2]int
-	less := func(a, b elem) bool {
-		return a[0] < b[0]
+	less := func(b, a *elem) bool {
+		return (*a)[0] < (*b)[0]
 	}
 
 	pq := aoc.PriorityQueue(less)
 
-	pq.Enqueue(elem{1, 4})
-	pq.Enqueue(elem{3, 2})
-	pq.Enqueue(elem{2, 3})
-	pq.Enqueue(elem{4, 1})
+	pq.Insert(&elem{1, 4})
+	pq.Insert(&elem{3, 2})
+	pq.Insert(&elem{2, 3})
+	pq.Insert(&elem{4, 1})
 
-	v, ok := pq.Dequeue()
-	is.True(ok)
-	is.Equal(v, elem{4, 1})
+	v := pq.ExtractMin()
+	is.True(v != nil)
+	is.Equal(v, &elem{4, 1})
 
-	v, ok = pq.Dequeue()
-	is.True(ok)
-	is.Equal(v, elem{3, 2})
+	v = pq.ExtractMin()
+	is.True(v != nil)
+	is.Equal(v, &elem{3, 2})
 
-	v, ok = pq.Dequeue()
-	is.True(ok)
-	is.Equal(v, elem{2, 3})
+	v = pq.ExtractMin()
+	is.True(v != nil)
+	is.Equal(v, &elem{2, 3})
 
-	v, ok = pq.Dequeue()
-	is.True(ok)
-	is.Equal(v, elem{1, 4})
+	v = pq.ExtractMin()
+	is.True(v != nil)
+	is.Equal(v, &elem{1, 4})
 
-	v, ok = pq.Dequeue()
-	is.True(!ok)
-	is.Equal(v, elem{})
+	v = pq.ExtractMin()
+	is.True(v == nil)
 }
 
 func TestSet(t *testing.T) {
@@ -140,7 +139,7 @@ func ExamplePriorityQueue() {
 		pt    int
 		score int
 	}
-	less := func(a, b memo) bool { return b.score < a.score }
+	less := func(a, b *memo) bool { return a.score < b.score }
 
 	adj := map[int][][2]int{
 		0: {{1, 2}, {2, 6}},
@@ -156,10 +155,10 @@ func ExamplePriorityQueue() {
 	dist := aoc.DefaultMap[int](int(^uint(0) >> 1))
 
 	dist.Set(0, 0)
-	pq.Enqueue(memo{0, 0})
+	pq.Insert(&memo{0, 0})
 
 	for !pq.IsEmpty() {
-		m, _ := pq.Dequeue()
+		m := pq.ExtractMin()
 
 		u := m.pt
 		if visited.Has(u) {
@@ -175,7 +174,7 @@ func ExamplePriorityQueue() {
 
 			if !visited.Has(v) && du+w < dv {
 				dist.Set(v, du+w)
-				pq.Enqueue(memo{v, du + w})
+				pq.Insert(&memo{v, du + w})
 			}
 		}
 	}
@@ -195,3 +194,160 @@ func ExamplePriorityQueue() {
 	// point 5 is 22 steps away.
 	// point 6 is 19 steps away.
 }
+
+func TestStack(t *testing.T) {
+	is := is.New(t)
+
+	s := aoc.Stack(1, 2, 3, 4)
+	is.True(!s.IsEmpty())
+	is.Equal(s.Pop(), 4)
+	is.Equal(s.Pop(), 3)
+	is.Equal(s.Pop(), 2)
+	is.Equal(s.Pop(), 1)
+	is.True(s.IsEmpty())
+	s.Push(4, 3, 2, 1)
+	is.True(!s.IsEmpty())
+	is.Equal(s.Pop(), 1)
+	is.Equal(s.Pop(), 2)
+	is.Equal(s.Pop(), 3)
+	is.Equal(s.Pop(), 4)
+	is.True(s.IsEmpty())
+}
+
+func TestGraph(t *testing.T) {
+	is := is.New(t)
+
+	var adjacencyList = map[int][]int{
+		2: {3, 5, 1},
+		1: {2, 4},
+		3: {6, 2},
+		4: {1, 5, 7},
+		5: {2, 6, 8, 4},
+		6: {3, 0, 9, 5},
+		7: {4, 8},
+		8: {5, 9, 7},
+		9: {6, 0, 8},
+	}
+
+	g := aoc.Graph(aoc.WithAdjacencyList[int, int](adjacencyList))
+	is.Equal(g.Neighbors(1), []int{2, 4})
+	is.Equal(map[int][]int(g.AdjacencyList()), adjacencyList)
+}
+
+func ExampleFibHeap() {
+	type memo struct {
+		pt    int
+		score int
+	}
+	less := func(a, b *memo) bool { return (*a).score < (*b).score }
+
+	adj := map[int][][2]int{
+		0: {{1, 2}, {2, 6}},
+		1: {{3, 5}},
+		2: {{3, 8}},
+		3: {{4, 10}, {5, 15}},
+		4: {{6, 2}},
+		5: {{6, 6}},
+	}
+
+	pq := aoc.FibHeap(less)
+	visited := aoc.Set([]int{}...)
+	dist := aoc.DefaultMap[int](int(^uint(0) >> 1))
+
+	dist.Set(0, 0)
+	pq.Insert(&memo{0, 0})
+
+	for !pq.IsEmpty() {
+		m := pq.ExtractMin()
+
+		u := m.pt
+		if visited.Has(u) {
+			continue
+		}
+		visited.Add(u)
+
+		du, _ := dist.Get(u)
+
+		for _, edge := range adj[u] {
+			v, w := edge[0], edge[1]
+			dv, _ := dist.Get(v)
+
+			if !visited.Has(v) && du+w < dv {
+				dist.Set(v, du+w)
+				pq.Insert(&memo{v, du + w})
+			}
+		}
+	}
+
+	items := dist.Items()
+	sort.Slice(items, func(i, j int) bool { return items[i].K < items[j].K })
+	for _, v := range items {
+		fmt.Printf("point %d is %d steps away.\n", v.K, v.V)
+	}
+
+	// Output:
+	// point 0 is 0 steps away.
+	// point 1 is 2 steps away.
+	// point 2 is 6 steps away.
+	// point 3 is 7 steps away.
+	// point 4 is 17 steps away.
+	// point 5 is 22 steps away.
+	// point 6 is 19 steps away.
+}
+
+func TestFibHeap(t *testing.T) {
+	is := is.New(t)
+
+	type elem [2]int
+	less := func(a, b *elem) bool {
+		return (*a)[0] < (*b)[0]
+	}
+
+	pq := aoc.FibHeap(less)
+
+	pq.Insert(&elem{1, 4})
+	pq.Insert(&elem{3, 2})
+	pq.Insert(&elem{2, 3})
+	pq.Insert(&elem{4, 1})
+
+	v := pq.ExtractMin()
+	is.True(v != nil)
+	is.Equal(v, &elem{1, 4})
+
+	pq.Insert(&elem{5, 8})
+	pq.Insert(&elem{6, 7})
+	pq.Insert(&elem{7, 6})
+	pq.Insert(&elem{8, 5})
+
+	v = pq.ExtractMin()
+	is.True(v != nil)
+	is.Equal(v, &elem{2, 3})
+
+	v = pq.ExtractMin()
+	is.True(v != nil)
+	is.Equal(v, &elem{3, 2})
+
+	v = pq.ExtractMin()
+	is.True(v != nil)
+	is.Equal(v, &elem{4, 1})
+
+	v = pq.ExtractMin()
+	is.True(v != nil)
+	is.Equal(v, &elem{5, 8})
+
+	m := aoc.FibHeap(less)
+	m.Insert(&elem{12, 9})
+	m.Insert(&elem{11, 10})
+	m.Insert(&elem{10, 11})
+	m.Insert(&elem{9, 12})
+
+	pq.Merge(m)
+
+	var keys []int
+	for !pq.IsEmpty() {
+		v := pq.ExtractMin()
+		fmt.Println(v)
+		keys = append(keys, v[0])
+	}
+	is.Equal(keys, []int{6, 7, 8, 9, 10, 11, 12})
+}

day17/main.go

@@ -29,10 +29,10 @@ func run(scan *bufio.Scanner) (*result, error) {
 	log("start day 17")
 
 	result := result{}
-	result.valuePT1 = search(m, 1, 3)
+	result.valuePT1 = search(m, 1, 3, seenFn)
 	log("result from part 1 = ", result.valuePT1)
 
-	result.valuePT2 = search(m, 4, 10)
+	result.valuePT2 = search(m, 4, 10, nil)
 	log("result from part 2 = ", result.valuePT2)
 
 	return &result, nil
@@ -90,6 +90,7 @@ type graph struct {
 	m        Map
 	target   Point
 	reads    int
+	seenFn   func(a position) position
 }
 
 // Neighbors returns valid steps from given position. if at target returns none.
@@ -118,6 +119,7 @@ func (g *graph) Neighbors(current position) []position {
 	if forward := current.step(); current.steps < g.max && g.m.Valid(forward.loc) {
 		nbs = append(nbs, forward)
 	}
+
 	return nbs
 }
 
@@ -129,12 +131,13 @@ func (g *graph) Cost(a, b position) int16 {
 }
 
 // Potential calculates distance to target
-func (g *graph) Potential(a, b position) int16 {
-	return aoc.ManhattanDistance(a.loc, b.loc)
-}
+// func (g *graph) Potential(a position) int16 {
+// 	return aoc.ManhattanDistance(a.loc, g.target)
+// }
 
-func (g *graph) Target(a position) bool {
-	if a.loc == g.target && a.steps >= g.min {
+// Target returns true when target reached. receives node and cost.
+func (g *graph) Target(a position, c int16) bool {
+	if a.loc == g.target && a.steps >= g.min && a.steps <= g.max {
 		return true
 	}
 	return false
@@ -142,47 +145,79 @@ func (g *graph) Target(a position) bool {
 
 // Seen attempt at simplifying the seen to use horizontal/vertical and no steps.
 // It returns correct for part1 but not part 2..
-// func (g *graph) Seen(a position) position {
-// 	if a.direction == U {
-// 		a.direction = D
-// 	}
-// 	if a.direction == L {
-// 		a.direction = R
-// 	}
-// 	a.steps = 0
-// 	return a
-// }
+func (g *graph) Seen(a position) position {
+	if g.seenFn != nil {
+		return g.seenFn(a)
+	}
+	return a
+}
 
-func search(m Map, minSteps, maxSteps int8) int {
+func seenFn(a position) position {
+	if a.direction == U {
+		a.direction = D
+	}
+	if a.direction == L {
+		a.direction = R
+	}
+	// a.steps = 0
+	return a
+}
+
+func search(m Map, minSteps, maxSteps int8, seenFn func(position) position) int {
 	rows, cols := m.Size()
 	start := Point{}
 	target := Point{rows - 1, cols - 1}
 
-	g := graph{min: minSteps, max: maxSteps, m: m, target: target}
-	cost, path := aoc.FindPath[int16, position](&g, position{loc: start}, position{loc: target})
+	g := graph{min: minSteps, max: maxSteps, m: m, target: target, seenFn: seenFn}
 
-	log("total map reads = ", g.reads)
-	printGraph(m, path)
+	cost, path, closed := aoc.FindPath[int16, position](&g, position{loc: start}, position{loc: target})
+
+	log("total map reads = ", g.reads, "cost = ", cost)
+	printGraph(m, path, closed, g.seenFn)
 
 	return int(cost)
 }
 
-// printGraph with the path overlay
-func printGraph(m Map, path []position) {
+// printGraph with the path/cost overlay
+func printGraph(m Map, path []position, closed map[position]int16, seenFn func(a position) position) {
 	pts := make(map[Point]position, len(path))
 	for _, pt := range path {
 		pts[pt.loc] = pt
 	}
 
+	clpt := make(map[position]position, len(closed))
+	for pt := range closed {
+		clpt[position{loc: pt.loc, steps: pt.steps}] = pt
+	}
+
 	for r, row := range m {
+		// if r == 0 {
+		// 	for c := range row {
+		// 		if c == 0 {
+		// 			fmt.Print("     ")
+		// 		}
+		// 		fmt.Printf("% 5d", c)
+		// 	}
+		// 	fmt.Println("")
+		// }
 		for c := range row {
-			if _, ok := pts[Point{int16(r), int16(c)}]; ok {
+			// if c == 0 {
+			// 	fmt.Printf("% 5d", r)
+			// }
+
+			if pt, ok := pts[Point{int16(r), int16(c)}]; ok {
+				if seenFn != nil {
+					pt = seenFn(pt)
+				}
+				_ = pt
+				// fmt.Printf("% 5d", closed[pt])
 				fmt.Print("*")
 
 				continue
 			}
 
-			fmt.Print(".")
+			// fmt.Print(" ....")
+			fmt.Print(" ")
 		}
 		fmt.Println("")
 	}

day17/main_test.go

@@ -28,14 +28,14 @@ func TestExample(t *testing.T) {
 	is.Equal(result.valuePT2, 94)
 }
 
-// func TestSolution(t *testing.T) {
-// 	is := is.New(t)
-// 	scan := bufio.NewScanner(bytes.NewReader(input))
+func TestSolution(t *testing.T) {
+	is := is.New(t)
+	scan := bufio.NewScanner(bytes.NewReader(input))
 
-// 	result, err := run(scan)
-// 	is.NoErr(err)
+	result, err := run(scan)
+	is.NoErr(err)
 
-// 	t.Log(result)
-// 	is.Equal(result.valuePT1, 843)
-// 	is.Equal(result.valuePT2, 1017)
-// }
+	t.Log(result)
+	is.Equal(result.valuePT1, 843)
+	is.Equal(result.valuePT2, 1017)
+}

day19/main.go

@@ -186,8 +186,8 @@ func solveWorkflow(parts []part, workflows map[string][]rule) int {
 
 func solveRanges(workflows map[string][]rule) uint {
 
-	pq := aoc.PriorityQueue(func(a, b queue) bool { return false })
-	pq.Enqueue(queue{
+	pq := aoc.PriorityQueue(func(a, b *queue) bool { return false })
+	pq.Insert(&queue{
 		"in",
 		block{
 			ranger{1, 4000},
@@ -200,9 +200,9 @@ func solveRanges(workflows map[string][]rule) uint {
 	// var rejected []block
 
 	for !pq.IsEmpty() {
-		current, _ := pq.Dequeue()
+		current := pq.ExtractMin()
 		for _, rule := range workflows[current.name] {
-			next := queue{name: rule.queue, block: current.block}
+			next := &queue{name: rule.queue, block: current.block}
 
 			switch rule.match {
 			case "x":
@@ -223,14 +223,14 @@ func solveRanges(workflows map[string][]rule) uint {
 				accepted = append(accepted, next.block)
 
 			default:
-				pq.Enqueue(next)
+				pq.Insert(next)
 			}
 		}
 	}
 
 	var sum uint
 	for _, a := range accepted {
-		sum += uint((a.x[1]-a.x[0]+1) * (a.m[1]-a.m[0]+1) * (a.a[1]-a.a[0]+1) * (a.s[1]-a.s[0]+1))
+		sum += uint((a.x[1] - a.x[0] + 1) * (a.m[1] - a.m[0] + 1) * (a.a[1] - a.a[0] + 1) * (a.s[1] - a.s[0] + 1))
 	}
 
 	return sum

grids.go

@@ -1,5 +1,10 @@
 package aoc
 
+import (
+	"cmp"
+	"sort"
+)
+
 type Vector struct {
 	Offset Point[int]
 	Scale  int
@@ -58,7 +63,7 @@ func NumPoints(outline []Point[int], borderLength int) int {
 
 type Map[I integer, T any] [][]T
 
-func (m *Map[I,T]) Get(p Point[I]) (Point[I], T, bool) {
+func (m *Map[I, T]) Get(p Point[I]) (Point[I], T, bool) {
 	var zero T
 	if !m.Valid(p) {
 		return [2]I{0, 0}, zero, false
@@ -66,13 +71,104 @@ func (m *Map[I,T]) Get(p Point[I]) (Point[I], T, bool) {
 
 	return p, (*m)[p[0]][p[1]], true
 }
-func (m *Map[I,T]) Size() (I, I) {
+func (m *Map[I, T]) Size() (I, I) {
 	if m == nil || len(*m) == 0 {
 		return 0, 0
 	}
 	return I(len(*m)), I(len((*m)[0]))
 }
-func (m *Map[I,T]) Valid(p Point[I]) bool {
+func (m *Map[I, T]) Valid(p Point[I]) bool {
 	rows, cols := m.Size()
 	return p[0] >= 0 && p[0] < rows && p[1] >= 0 && p[1] < cols
 }
+
+type adjacencyList[V any, C comparable] map[C][]V
+type graph[V any, W cmp.Ordered, C comparable] map[C]*vertex[V, W]
+type graphOption[V any, W cmp.Ordered, C comparable] func(g *graph[V, W, C])
+type vertex[V any, W cmp.Ordered] struct {
+	Value V
+	Edges edges[V, W]
+}
+
+func (v *vertex[V, W]) Neighbors() []V {
+	var nbs []V
+	sort.Sort(v.Edges)
+	for _, e := range v.Edges {
+		nbs = append(nbs, e.Vertex.Value)
+	}
+	return nbs
+}
+
+type edge[V any, W cmp.Ordered] struct {
+	Vertex *vertex[V, W]
+	Weight W
+}
+type edges[V any, W cmp.Ordered] []edge[V, W]
+
+func (e edges[V, W]) Len() int           { return len(e) }
+func (e edges[V, W]) Less(i, j int) bool { return e[i].Weight < e[j].Weight }
+func (e edges[V, W]) Swap(i, j int)      { e[i], e[j] = e[j], e[i] }
+
+func Graph[V any, W cmp.Ordered, C comparable](opts ...graphOption[V, W, C]) *graph[V, W, C] {
+	g := make(graph[V, W, C])
+	for _, opt := range opts {
+		opt(&g)
+	}
+	return &g
+}
+func (g *graph[V, W, C]) AddVertex(id C, value V) {
+	(*g)[id] = &vertex[V,W]{Value: value}
+}
+func (g *graph[V, W, C]) AddEdge(from, to C, w W) {
+	if g == nil {
+		return
+	}
+	if _, ok := (*g)[from]; !ok {
+		return
+	}
+	if _, ok := (*g)[to]; !ok {
+		return
+	}
+
+	(*g)[from].Edges = append((*g)[from].Edges, edge[V,W]{(*g)[to], w})
+}
+func (g *graph[V, W, C]) Neighbors(v C) []V {
+	if g == nil {
+		return nil
+	}
+
+	return (*g)[v].Neighbors()
+}
+func (g *graph[V, W, C]) AdjacencyList() adjacencyList[V, C] {
+	m := make(map[C][]V)
+	for id, v := range *g {
+		if len(v.Edges) == 0 {
+			continue
+		}
+		m[id] = v.Neighbors()
+	}
+	return m
+}
+
+func WithAdjacencyList[W cmp.Ordered, C comparable](list adjacencyList[C, C]) graphOption[C, W, C] {
+	var zeroW W
+	return func(g *graph[C, W, C]) {
+		for vertex, edges := range list {
+			if _, ok := (*g)[vertex]; !ok {
+				g.AddVertex(vertex, vertex)
+			}
+
+			// add edges to vertex
+			for _, edge := range edges {
+				// add edge as vertex, if not added
+				if _, ok := (*g)[edge]; !ok {
+					g.AddVertex(edge, edge)
+				}
+
+				g.AddEdge(vertex, edge, zeroW) // no weights in this adjacency list
+			}
+		}
+	}
+}
+
+// func GraphFromMap()

runner.go

@@ -2,26 +2,70 @@ package aoc
 
 import (
 	"bufio"
+	"flag"
 	"fmt"
+	"log"
 	"os"
 	"path/filepath"
+	"runtime"
+	"runtime/pprof"
 	"strings"
 	"time"
 )
 
+var cpuprofile = flag.String("cpuprofile", "", "write cpu profile to `file`")
+var memprofile = flag.String("memprofile", "", "write memory profile to `file`")
+
 func Runner[R any, F func(*bufio.Scanner) (R, error)](run F) (R, error) {
-	if len(os.Args) != 2 {
+	if len(os.Args) < 2 {
 		Log("Usage:", filepath.Base(os.Args[0]), "FILE")
 		os.Exit(22)
 	}
 
-	input, err := os.Open(os.Args[1])
+	inputFilename := os.Args[1]
+	os.Args = append(os.Args[:1], os.Args[2:]...)
+
+	flag.Parse()
+	Log(cpuprofile, memprofile, *cpuprofile, *memprofile)
+	if *cpuprofile != "" {
+		Log("enabled cpu profile")
+		f, err := os.Create(*cpuprofile)
+		if err != nil {
+			log.Fatal("could not create CPU profile: ", err)
+		}
+		defer f.Close() // error handling omitted for example
+		Log("write cpu profile to", f.Name())
+		if err := pprof.StartCPUProfile(f); err != nil {
+			log.Fatal("could not start CPU profile: ", err)
+		}
+		defer pprof.StopCPUProfile()
+	}
+
+	if *memprofile != "" {
+		Log("enabled mem profile")
+		defer func() {
+			f, err := os.Create(*memprofile)
+			if err != nil {
+				log.Fatal("could not create memory profile: ", err)
+			}
+			Log("write mem profile to", f.Name())
+			defer f.Close() // error handling omitted for example
+			runtime.GC()    // get up-to-date statistics
+			if err := pprof.WriteHeapProfile(f); err != nil {
+				log.Fatal("could not write memory profile: ", err)
+			}
+		}()
+	}
+
+
+	input, err := os.Open(inputFilename)
 	if err != nil {
 		Log(err)
 		os.Exit(1)
 	}
 
 	scan := bufio.NewScanner(input)
+
 	return run(scan)
 }
 

search.go

@@ -1,12 +1,13 @@
 package aoc
 
 import (
+	"math/bits"
 	"sort"
 )
 
 type priorityQueue[T any] struct {
-	elems        []T
-	less         func(a, b T) bool
+	elems        []*T
+	less         func(a, b *T) bool
 	maxDepth     int
 	totalEnqueue int
 	totalDequeue int
@@ -16,10 +17,10 @@ type priorityQueue[T any] struct {
 // less is the function for sorting. reverse a and b to reverse the sort.
 // T is the item
 // U is a slice of T
-func PriorityQueue[T any](less func(a, b T) bool) *priorityQueue[T] {
+func PriorityQueue[T any](less func(a, b *T) bool) *priorityQueue[T] {
 	return &priorityQueue[T]{less: less}
 }
-func (pq *priorityQueue[T]) Enqueue(elem T) {
+func (pq *priorityQueue[T]) Insert(elem *T) {
 	pq.totalEnqueue++
 
 	pq.elems = append(pq.elems, elem)
@@ -28,35 +29,65 @@ func (pq *priorityQueue[T]) Enqueue(elem T) {
 func (pq *priorityQueue[T]) IsEmpty() bool {
 	return len(pq.elems) == 0
 }
-func (pq *priorityQueue[T]) Dequeue() (T, bool) {
+func (pq *priorityQueue[T]) ExtractMin() *T {
 	pq.totalDequeue++
 
-	var elem T
+	var elem *T
 	if pq.IsEmpty() {
-		return elem, false
+		return elem
 	}
 
-	sort.Slice(pq.elems, func(i, j int) bool { return pq.less(pq.elems[i], pq.elems[j]) })
+	sort.Slice(pq.elems, func(i, j int) bool { return pq.less(pq.elems[j], pq.elems[i]) })
 	pq.elems, elem = pq.elems[:len(pq.elems)-1], pq.elems[len(pq.elems)-1]
-	return elem, true
+	return elem
+}
+
+type stack[T any] []T
+
+func Stack[T any](a ...T) *stack[T] {
+	var s stack[T] = a
+	return &s
+}
+func (s *stack[T]) Push(a ...T) {
+	if s == nil {
+		return
+	}
+	*s = append(*s, a...)
+}
+func (s *stack[T]) IsEmpty() bool {
+	return s == nil || len(*s) == 0
+}
+func (s *stack[T]) Pop() T {
+	var a T
+	if s.IsEmpty() {
+		return a
+	}
+	a, *s = (*s)[len(*s)-1], (*s)[:len(*s)-1]
+	return a
 }
 
 // ManhattanDistance the distance between two points measured along axes at right angles.
 func ManhattanDistance[T integer](a, b Point[T]) T {
-	return ABS(a[1]-b[1]) + ABS(a[0]-b[0])
+	return ABS(a[0]-b[0]) + ABS(a[1]-b[1])
 }
 
 type pather[C number, N comparable] interface {
+	// Neighbors returns all neighbors to node N that should be considered next.
 	Neighbors(N) []N
+
+	// Cost returns
 	Cost(a, b N) C
-	Potential(a, b N) C
+
+	// Target returns true when target reached. receives node and cost.
+	Target(N, C) bool
 
 	// OPTIONAL:
+	// Add heuristic for running as A* search.
+	// Potential(N) C
+
 	// Seen modify value used by seen pruning.
 	// Seen(N) N
 
-	// Target returns true if target reached.
-	// Target(N) bool
 }
 
 // FindPath uses the A* path finding algorithem.
@@ -67,9 +98,18 @@ type pather[C number, N comparable] interface {
 //
 // start, end are nodes that dileniate the start and end of the search path.
 // The returned values are the calculated cost and the path taken from start to end.
-func FindPath[C integer, N comparable](g pather[C, N], start, end N) (C, []N) {
+func FindPath[C integer, N comparable](g pather[C, N], start, end N) (C, []N, map[N]C) {
 	var zero C
-	closed := make(map[N]bool)
+
+	var seenFn = func(a N) N { return a }
+	if s, ok := g.(interface{ Seen(N) N }); ok {
+		seenFn = s.Seen
+	}
+
+	var potentialFn = func(N) C { var zero C; return zero }
+	if p, ok := g.(interface{ Potential(N) C }); ok {
+		potentialFn = p.Potential
+	}
 
 	type node struct {
 		cost      C
@@ -78,7 +118,7 @@ func FindPath[C integer, N comparable](g pather[C, N], start, end N) (C, []N) {
 		position  N
 	}
 
-	NewPath := func(n *node) []N {
+	newPath := func(n *node) []N {
 		var path []N
 		for n.parent != nil {
 			path = append(path, n.position)
@@ -90,61 +130,143 @@ func FindPath[C integer, N comparable](g pather[C, N], start, end N) (C, []N) {
 		return path
 	}
 
-	less := func(a, b node) bool {
-		return b.cost+b.potential < a.cost+a.potential
+	less := func(a, b *node) bool {
+		return a.cost+a.potential < b.cost+b.potential
 	}
 
-	pq := PriorityQueue(less)
-	pq.Enqueue(node{position: start})
-	closed[start] = false
+	closed := make(map[N]C)
+	open := FibHeap(less)
 
-	defer func() {
-		Log("queue max depth = ", pq.maxDepth, "total enqueue = ", pq.totalEnqueue, "total dequeue = ", pq.totalDequeue)
-	}()
+	open.Insert(&node{position: start, potential: potentialFn(start)})
+	closed[start] = zero
 
-	var seenFn = func(a N) N { return a }
-	if s, ok := g.(interface{ Seen(N) N }); ok {
-		seenFn = s.Seen
-	}
-
-	var targetFn = func(a N) bool { return true }
-	if s, ok := g.(interface{ Target(N) bool }); ok {
-		targetFn = s.Target
-	}
-
-	for !pq.IsEmpty() {
-		current, _ := pq.Dequeue()
-		cost, potential, n := current.cost, current.potential, current.position
-
-		seen := seenFn(n)
-		if closed[seen] {
-			continue
-		}
-		closed[seen] = true
-
-		if cost > 0 && potential == zero && targetFn(current.position) {
-			return cost, NewPath(&current)
-		}
-
-		for _, nb := range g.Neighbors(n) {
-			seen := seenFn(nb)
-			if closed[seen] {
-				continue
-			}
-
-			cost := g.Cost(n, nb) + current.cost
-			nextPath := node{
+	for !open.IsEmpty() {
+		current := open.ExtractMin()
+		for _, nb := range g.Neighbors(current.position) {
+			next := &node{
 				position:  nb,
-				parent:    &current,
-				cost:      cost,
-				potential: g.Potential(nb, end),
+				parent:    current,
+				cost:      g.Cost(current.position, nb) + current.cost,
+				potential: potentialFn(nb),
 			}
-			// check if path is in open list
-			if _, open := closed[seen]; !open {
-				pq.Enqueue(nextPath)
-				closed[seen] = false // add to open list
+
+			seen := seenFn(nb)
+			cost, ok := closed[seen]
+			if !ok || next.cost < cost {
+				open.Insert(next)
+				closed[seen] = next.cost
+			}
+
+			if next.potential == zero && g.Target(next.position, next.cost) {
+				return next.cost, newPath(next), closed
 			}
 		}
 	}
-	return zero, nil
+	return zero, nil, closed
 }
+
+type fibTree[T any] struct {
+	value *T
+	parent *fibTree[T]
+	child []*fibTree[T]
+}
+
+func (t *fibTree[T]) addAtEnd(n *fibTree[T]) {
+	n.parent = t
+	t.child = append(t.child, n)
+}
+
+type fibHeap[T any] struct {
+	trees []*fibTree[T]
+	least *fibTree[T]
+	count uint
+	less  func(a, b *T) bool
+}
+
+func FibHeap[T any](less func(a, b *T) bool) *fibHeap[T] {
+	return &fibHeap[T]{less: less}
+}
+
+func (h *fibHeap[T]) GetMin() *T {
+	return h.least.value
+}
+
+func (h *fibHeap[T]) IsEmpty() bool { return h.least == nil }
+
+func (h *fibHeap[T]) Insert(v *T) {
+	ntree := &fibTree[T]{value: v}
+	h.trees = append(h.trees, ntree)
+	if h.least == nil || h.less(v, h.least.value) {
+		h.least = ntree
+	}
+	h.count++
+}
+
+func (h *fibHeap[T]) ExtractMin() *T {
+	smallest := h.least
+	if smallest != nil {
+		// Remove smallest from root trees.
+		for i := range h.trees {
+			pos := h.trees[i]
+			if pos == smallest {
+				h.trees[i] = h.trees[len(h.trees)-1]
+				h.trees = h.trees[:len(h.trees)-1]
+				break
+			}
+		}
+
+		// Add children to root
+		h.trees = append(h.trees, smallest.child...)
+		smallest.child = smallest.child[:0]
+
+		h.least = nil
+		if len(h.trees) > 0 {
+			h.consolidate()
+		}
+
+		h.count--
+		return smallest.value
+	}
+	return nil
+}
+
+func (h *fibHeap[T]) consolidate() {
+	aux := make([]*fibTree[T], bits.Len(h.count))
+	for _, x := range h.trees {
+		order := len(x.child)
+
+		// consolidate the larger roots under smaller roots of same order until we have at most one tree per order.
+		for aux[order] != nil {
+			y := aux[order]
+			if h.less(y.value, x.value) {
+				x, y = y, x
+			}
+			x.addAtEnd(y)
+			aux[order] = nil
+			order++
+		}
+		aux[order] = x
+	}
+
+	h.trees = h.trees[:0]
+	// move ordered trees to root and find least node.
+	for _, k := range aux {
+		if k != nil {
+			k.parent = nil
+			h.trees = append(h.trees, k)
+			if h.least == nil || h.less(k.value, h.least.value) {
+				h.least = k
+			}
+		}
+	}
+}
+
+func (h *fibHeap[T]) Merge(a *fibHeap[T]) {
+	h.trees = append(h.trees, a.trees...)
+	h.count += a.count
+	h.consolidate()
+}
+
+// func (h *fibHeap[T]) Find(n *T) *fibTree[T] {
+	
+// }