diff --git a/aoc_test.go b/aoc_test.go index 7e9d5a1..27b6bec 100644 --- a/aoc_test.go +++ b/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}) +} diff --git a/day17/main.go b/day17/main.go index f63b8da..7b3fada 100644 --- a/day17/main.go +++ b/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("") } diff --git a/day17/main_test.go b/day17/main_test.go index 64e490d..d7fd635 100644 --- a/day17/main_test.go +++ b/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) +} diff --git a/day19/main.go b/day19/main.go index 2e7d27a..926a0f7 100644 --- a/day19/main.go +++ b/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 diff --git a/grids.go b/grids.go index 22df4ed..b8227f4 100644 --- a/grids.go +++ b/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() diff --git a/runner.go b/runner.go index d2600aa..4cb1a7f 100644 --- a/runner.go +++ b/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) } diff --git a/search.go b/search.go index a559129..80f6fb9 100644 --- a/search.go +++ b/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(¤t) - } - - 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: ¤t, - 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] { + +// } \ No newline at end of file