1 /*
2  * Copyright (c) 2006-2020, RT-Thread Development Team
3  *
4  * SPDX-License-Identifier: Apache-2.0
5  *
6  * Change Logs:
7  * Date           Author       Notes
8  * 2019-10-12     Jesven       first version
9  */
10 #include <rtthread.h>
11 #include <lwp_avl.h>
12 
lwp_avl_rebalance(struct lwp_avl_struct *** nodeplaces_ptr,int count)13 static void lwp_avl_rebalance(struct lwp_avl_struct ***nodeplaces_ptr, int count)
14 {
15     for (; count > 0; count--)
16     {
17         struct lwp_avl_struct **nodeplace = *--nodeplaces_ptr;
18         struct lwp_avl_struct *node = *nodeplace;
19         struct lwp_avl_struct *nodeleft = node->avl_left;
20         struct lwp_avl_struct *noderight = node->avl_right;
21         int heightleft = heightof(nodeleft);
22         int heightright = heightof(noderight);
23         if (heightright + 1 < heightleft)
24         {
25             struct lwp_avl_struct *nodeleftleft = nodeleft->avl_left;
26             struct lwp_avl_struct *nodeleftright = nodeleft->avl_right;
27             int heightleftright = heightof(nodeleftright);
28             if (heightof(nodeleftleft) >= heightleftright)
29             {
30                 node->avl_left = nodeleftright;
31                 nodeleft->avl_right = node;
32                 nodeleft->avl_height = 1 + (node->avl_height = 1 + heightleftright);
33                 *nodeplace = nodeleft;
34             }
35             else
36             {
37                 nodeleft->avl_right = nodeleftright->avl_left;
38                 node->avl_left = nodeleftright->avl_right;
39                 nodeleftright->avl_left = nodeleft;
40                 nodeleftright->avl_right = node;
41                 nodeleft->avl_height = node->avl_height = heightleftright;
42                 nodeleftright->avl_height = heightleft;
43                 *nodeplace = nodeleftright;
44             }
45         }
46         else if (heightleft + 1 < heightright)
47         {
48             struct lwp_avl_struct *noderightright = noderight->avl_right;
49             struct lwp_avl_struct *noderightleft = noderight->avl_left;
50             int heightrightleft = heightof(noderightleft);
51             if (heightof(noderightright) >= heightrightleft)
52             {
53                 node->avl_right = noderightleft;
54                 noderight->avl_left = node;
55                 noderight->avl_height = 1 + (node->avl_height = 1 + heightrightleft);
56                 *nodeplace = noderight;
57             }
58             else
59             {
60                 noderight->avl_left = noderightleft->avl_right;
61                 node->avl_right = noderightleft->avl_left;
62                 noderightleft->avl_right = noderight;
63                 noderightleft->avl_left = node;
64                 noderight->avl_height = node->avl_height = heightrightleft;
65                 noderightleft->avl_height = heightright;
66                 *nodeplace = noderightleft;
67             }
68         }
69         else
70         {
71             int height = (heightleft < heightright ? heightright : heightleft) + 1;
72             if (height == node->avl_height)
73                 break;
74             node->avl_height = height;
75         }
76     }
77 }
78 
lwp_avl_remove(struct lwp_avl_struct * node_to_delete,struct lwp_avl_struct ** ptree)79 void lwp_avl_remove(struct lwp_avl_struct *node_to_delete, struct lwp_avl_struct **ptree)
80 {
81     avl_key_t key = node_to_delete->avl_key;
82     struct lwp_avl_struct **nodeplace = ptree;
83     struct lwp_avl_struct **stack[avl_maxheight];
84     uint32_t stack_count = 0;
85     struct lwp_avl_struct ***stack_ptr = &stack[0]; /* = &stack[stackcount] */
86     struct lwp_avl_struct **nodeplace_to_delete;
87     for (;;)
88     {
89         struct lwp_avl_struct *node = *nodeplace;
90         if (node == AVL_EMPTY)
91         {
92             return;
93         }
94 
95         *stack_ptr++ = nodeplace;
96         stack_count++;
97         if (key == node->avl_key)
98             break;
99         if (key < node->avl_key)
100             nodeplace = &node->avl_left;
101         else
102             nodeplace = &node->avl_right;
103     }
104     nodeplace_to_delete = nodeplace;
105     if (node_to_delete->avl_left == AVL_EMPTY)
106     {
107         *nodeplace_to_delete = node_to_delete->avl_right;
108         stack_ptr--;
109         stack_count--;
110     }
111     else
112     {
113         struct lwp_avl_struct ***stack_ptr_to_delete = stack_ptr;
114         struct lwp_avl_struct **nodeplace = &node_to_delete->avl_left;
115         struct lwp_avl_struct *node;
116         for (;;)
117         {
118             node = *nodeplace;
119             if (node->avl_right == AVL_EMPTY)
120                 break;
121             *stack_ptr++ = nodeplace;
122             stack_count++;
123             nodeplace = &node->avl_right;
124         }
125         *nodeplace = node->avl_left;
126         node->avl_left = node_to_delete->avl_left;
127         node->avl_right = node_to_delete->avl_right;
128         node->avl_height = node_to_delete->avl_height;
129         *nodeplace_to_delete = node;
130         *stack_ptr_to_delete = &node->avl_left;
131     }
132     lwp_avl_rebalance(stack_ptr, stack_count);
133 }
134 
lwp_avl_insert(struct lwp_avl_struct * new_node,struct lwp_avl_struct ** ptree)135 void lwp_avl_insert(struct lwp_avl_struct *new_node, struct lwp_avl_struct **ptree)
136 {
137     avl_key_t key = new_node->avl_key;
138     struct lwp_avl_struct **nodeplace = ptree;
139     struct lwp_avl_struct **stack[avl_maxheight];
140     int stack_count = 0;
141     struct lwp_avl_struct ***stack_ptr = &stack[0]; /* = &stack[stackcount] */
142     for (;;)
143     {
144         struct lwp_avl_struct *node = *nodeplace;
145         if (node == AVL_EMPTY)
146             break;
147         *stack_ptr++ = nodeplace;
148         stack_count++;
149         if (key < node->avl_key)
150             nodeplace = &node->avl_left;
151         else
152             nodeplace = &node->avl_right;
153     }
154     new_node->avl_left = AVL_EMPTY;
155     new_node->avl_right = AVL_EMPTY;
156     new_node->avl_height = 1;
157     *nodeplace = new_node;
158     lwp_avl_rebalance(stack_ptr, stack_count);
159 }
160 
lwp_avl_find(avl_key_t key,struct lwp_avl_struct * ptree)161 struct lwp_avl_struct *lwp_avl_find(avl_key_t key, struct lwp_avl_struct *ptree)
162 {
163     for (;;)
164     {
165         if (ptree == AVL_EMPTY)
166         {
167             return (struct lwp_avl_struct *)0;
168         }
169         if (key == ptree->avl_key)
170             break;
171         if (key < ptree->avl_key)
172             ptree = ptree->avl_left;
173         else
174             ptree = ptree->avl_right;
175     }
176     return ptree;
177 }
178 
lwp_avl_traversal(struct lwp_avl_struct * ptree,int (* fun)(struct lwp_avl_struct *,void *),void * arg)179 int lwp_avl_traversal(struct lwp_avl_struct *ptree, int (*fun)(struct lwp_avl_struct *, void *), void *arg)
180 {
181     int ret;
182 
183     if (!ptree)
184     {
185         return 0;
186     }
187     if (ptree->avl_left)
188     {
189         ret = lwp_avl_traversal(ptree->avl_left, fun, arg);
190         if (ret != 0)
191         {
192             return ret;
193         }
194     }
195     ret = (*fun)(ptree, arg);
196     if (ret != 0)
197     {
198         return ret;
199     }
200     if (ptree->avl_right)
201     {
202         ret = lwp_avl_traversal(ptree->avl_right, fun, arg);
203         if (ret != 0)
204         {
205             return ret;
206         }
207     }
208     return ret;
209 }
210 
lwp_map_find_first(struct lwp_avl_struct * ptree)211 rt_weak struct lwp_avl_struct* lwp_map_find_first(struct lwp_avl_struct* ptree)
212 {
213     if (ptree == AVL_EMPTY)
214     {
215         return (struct lwp_avl_struct *)0;
216     }
217     while (1)
218     {
219         if (!ptree->avl_left)
220         {
221             break;
222         }
223         ptree = ptree->avl_left;
224     }
225     return ptree;
226 }
227 
228