1 /*
2 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 #include <stdio.h>
8 #include <stdlib.h>
9 #include "pico/util/pheap.h"
10
ph_create(uint max_nodes,pheap_comparator comparator,void * user_data)11 pheap_t *ph_create(uint max_nodes, pheap_comparator comparator, void *user_data) {
12 invalid_params_if(PHEAP, !max_nodes || max_nodes >= (1u << (8 * sizeof(pheap_node_id_t))));
13 pheap_t *heap = calloc(1, sizeof(pheap_t));
14 heap->nodes = calloc(max_nodes, sizeof(pheap_node_t));
15 ph_post_alloc_init(heap, max_nodes, comparator, user_data);
16 return heap;
17 }
18
ph_post_alloc_init(pheap_t * heap,uint max_nodes,pheap_comparator comparator,void * user_data)19 void ph_post_alloc_init(pheap_t *heap, uint max_nodes, pheap_comparator comparator, void *user_data) {
20 invalid_params_if(PHEAP, !max_nodes || max_nodes >= (1u << (8 * sizeof(pheap_node_id_t))));
21 heap->max_nodes = (pheap_node_id_t) max_nodes;
22 heap->comparator = comparator;
23 heap->user_data = user_data;
24 ph_clear(heap);
25 }
26
ph_clear(pheap_t * heap)27 void ph_clear(pheap_t *heap) {
28 heap->root_id = 0;
29 heap->free_head_id = 1;
30 heap->free_tail_id = heap->max_nodes;
31 for(pheap_node_id_t i = 1; i < heap->max_nodes; i++) {
32 ph_get_node(heap, i)->sibling = (pheap_node_id_t)(i + 1);
33 }
34 ph_get_node(heap, heap->max_nodes)->sibling = 0;
35 }
36
ph_destroy(pheap_t * heap)37 void ph_destroy(pheap_t *heap) {
38 free(heap->nodes);
39 free(heap);
40 }
41
ph_merge_two_pass(pheap_t * heap,pheap_node_id_t id)42 pheap_node_id_t ph_merge_two_pass(pheap_t *heap, pheap_node_id_t id) {
43 if (!id || !ph_get_node(heap, id)->sibling) {
44 return id;
45 } else {
46 pheap_node_id_t a, b, new_node;
47 a = id;
48 b = ph_get_node(heap, id)->sibling;
49 new_node = ph_get_node(heap, b)->sibling;
50 ph_get_node(heap, a)->sibling = ph_get_node(heap, b)->sibling = 0;
51 return ph_merge_nodes(heap, ph_merge_nodes(heap, a, b), ph_merge_two_pass(heap, new_node));
52 }
53 }
54
ph_remove_any_head(pheap_t * heap,pheap_node_id_t root_id,bool free)55 static pheap_node_id_t ph_remove_any_head(pheap_t *heap, pheap_node_id_t root_id, bool free) {
56 assert(root_id);
57 // printf("Removing head %d (parent %d sibling %d)\n", root_id, ph_get_node(heap, root_id)->parent, ph_get_node(heap, root_id)->sibling);
58 assert(!ph_get_node(heap, root_id)->sibling);
59 assert(!ph_get_node(heap, root_id)->parent);
60 pheap_node_id_t new_root_id = ph_merge_two_pass(heap, ph_get_node(heap, root_id)->child);
61 if (free) {
62 if (heap->free_tail_id) {
63 ph_get_node(heap, heap->free_tail_id)->sibling = root_id;
64 }
65 if (!heap->free_head_id) {
66 assert(!heap->free_tail_id);
67 heap->free_head_id = root_id;
68 }
69 heap->free_tail_id = root_id;
70 }
71 if (new_root_id) ph_get_node(heap, new_root_id)->parent = 0;
72 ph_get_node(heap, root_id)->sibling = 0;
73 return new_root_id;
74 }
75
ph_remove_head(pheap_t * heap,bool free)76 pheap_node_id_t ph_remove_head(pheap_t *heap, bool free) {
77 pheap_node_id_t old_root_id = ph_peek_head(heap);
78 heap->root_id = ph_remove_any_head(heap, old_root_id, free);
79 return old_root_id;
80 }
81
ph_remove_and_free_node(pheap_t * heap,pheap_node_id_t id)82 bool ph_remove_and_free_node(pheap_t *heap, pheap_node_id_t id) {
83 // 1) trivial cases
84 if (!id) return false;
85 if (id == heap->root_id) {
86 ph_remove_and_free_head(heap);
87 return true;
88 }
89 // 2) unlink the node from the tree
90 pheap_node_t *node = ph_get_node(heap, id);
91 if (!node->parent) return false; // not in tree
92 pheap_node_t *parent = ph_get_node(heap, node->parent);
93 if (parent->child == id) {
94 parent->child = node->sibling;
95 } else {
96 pheap_node_id_t prev_sibling_id = parent->child;
97 bool __unused found = false;
98 do {
99 pheap_node_t *prev_sibling = ph_get_node(heap, prev_sibling_id);
100 if (prev_sibling->sibling == id) {
101 prev_sibling->sibling = node->sibling;
102 found = true;
103 break;
104 }
105 prev_sibling_id = prev_sibling->sibling;
106 } while (prev_sibling_id);
107 assert(found);
108 }
109 node->sibling = node->parent = 0;
110 // ph_dump(heap, NULL, NULL);
111 // 3) remove it from the head of its own subtree
112 pheap_node_id_t new_sub_tree = ph_remove_any_head(heap, id, true);
113 assert(new_sub_tree != heap->root_id);
114 heap->root_id = ph_merge_nodes(heap, heap->root_id, new_sub_tree);
115 return true;
116 }
117
ph_dump_node(pheap_t * heap,pheap_node_id_t id,void (* dump_key)(pheap_node_id_t,void *),void * user_data,uint indent)118 static uint ph_dump_node(pheap_t *heap, pheap_node_id_t id, void (*dump_key)(pheap_node_id_t, void *), void *user_data, uint indent) {
119 uint count = 0;
120 if (id) {
121 count++;
122 for (uint i = 0; i < indent * 2; i++) {
123 putchar(' ');
124 }
125 pheap_node_t *node = ph_get_node(heap, id);
126 printf("%d (c=%d s=%d p=%d) ", id, node->child, node->sibling, node->parent);
127 if (dump_key) dump_key(id, user_data);
128 printf("\n");
129 count += ph_dump_node(heap, node->child, dump_key, user_data, indent + 1);
130 count += ph_dump_node(heap, node->sibling, dump_key, user_data, indent);
131 }
132 return count;
133 }
134
ph_dump(pheap_t * heap,void (* dump_key)(pheap_node_id_t,void *),void * user_data)135 void ph_dump(pheap_t *heap, void (*dump_key)(pheap_node_id_t, void *), void *user_data) {
136 uint count = ph_dump_node(heap, heap->root_id, dump_key, user_data, 0);
137 printf("node_count %d\n", count);
138 }
