1/* Priority queue functions.
2   Copyright (C) 2003 Yasuhiro Ohara
3
4This file is part of GNU Zebra.
5
6GNU Zebra is free software; you can redistribute it and/or modify
7it under the terms of the GNU General Public License as published
8by the Free Software Foundation; either version 2, or (at your
9option) any later version.
10
11GNU Zebra is distributed in the hope that it will be useful, but
12WITHOUT ANY WARRANTY; without even the implied warranty of
13MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
14General Public License for more details.
15
16You should have received a copy of the GNU General Public License
17along with GNU Zebra; see the file COPYING.  If not, write to the
18Free Software Foundation, Inc., 59 Temple Place - Suite 330,
19Boston, MA 02111-1307, USA.  */
20
21#include <zebra.h>
22
23#include "memory.h"
24#include "pqueue.h"
25
26/* priority queue using heap sort */
27
28/* pqueue->cmp() controls the order of sorting (i.e, ascending or
29   descending). If you want the left node to move upper of the heap
30   binary tree, make cmp() to return less than 0.  for example, if cmp
31   (10, 20) returns -1, the sorting is ascending order. if cmp (10,
32   20) returns 1, the sorting is descending order. if cmp (10, 20)
33   returns 0, this library does not do sorting (which will not be what
34   you want).  To be brief, if the contents of cmp_func (left, right)
35   is left - right, dequeue () returns the smallest node.  Otherwise
36   (if the contents is right - left), dequeue () returns the largest
37   node.  */
38
39#define DATA_SIZE (sizeof (void *))
40#define PARENT_OF(x) ((x - 1) / 2)
41#define LEFT_OF(x)  (2 * x + 1)
42#define RIGHT_OF(x) (2 * x + 2)
43#define HAVE_CHILD(x,q) (x < (q)->size / 2)
44
45void
46trickle_up (int index, struct pqueue *queue)
47{
48  void *tmp;
49
50  /* Save current node as tmp node.  */
51  tmp = queue->array[index];
52
53  /* Continue until the node reaches top or the place where the parent
54     node should be upper than the tmp node.  */
55  while (index > 0 &&
56         (*queue->cmp) (tmp, queue->array[PARENT_OF (index)]) < 0)
57    {
58      /* actually trickle up */
59      queue->array[index] = queue->array[PARENT_OF (index)];
60      if (queue->update != NULL)
61	(*queue->update) (queue->array[index], index);
62      index = PARENT_OF (index);
63    }
64
65  /* Restore the tmp node to appropriate place.  */
66  queue->array[index] = tmp;
67  if (queue->update != NULL)
68    (*queue->update) (tmp, index);
69}
70
71void
72trickle_down (int index, struct pqueue *queue)
73{
74  void *tmp;
75  int which;
76
77  /* Save current node as tmp node.  */
78  tmp = queue->array[index];
79
80  /* Continue until the node have at least one (left) child.  */
81  while (HAVE_CHILD (index, queue))
82    {
83      /* If right child exists, and if the right child is more proper
84         to be moved upper.  */
85      if (RIGHT_OF (index) < queue->size &&
86          (*queue->cmp) (queue->array[LEFT_OF (index)],
87                         queue->array[RIGHT_OF (index)]) > 0)
88        which = RIGHT_OF (index);
89      else
90        which = LEFT_OF (index);
91
92      /* If the tmp node should be upper than the child, break.  */
93      if ((*queue->cmp) (queue->array[which], tmp) > 0)
94        break;
95
96      /* Actually trickle down the tmp node.  */
97      queue->array[index] = queue->array[which];
98       if (queue->update != NULL)
99	 (*queue->update) (queue->array[index], index);
100      index = which;
101    }
102
103  /* Restore the tmp node to appropriate place.  */
104  queue->array[index] = tmp;
105  if (queue->update != NULL)
106    (*queue->update) (tmp, index);
107}
108
109struct pqueue *
110pqueue_create (void)
111{
112  struct pqueue *queue;
113
114  queue = XCALLOC (MTYPE_PQUEUE, sizeof (struct pqueue));
115
116  queue->array = XCALLOC (MTYPE_PQUEUE_DATA,
117                          DATA_SIZE * PQUEUE_INIT_ARRAYSIZE);
118  queue->array_size = PQUEUE_INIT_ARRAYSIZE;
119
120  /* By default we want nothing to happen when a node changes. */
121  queue->update = NULL;
122  return queue;
123}
124
125void
126pqueue_delete (struct pqueue *queue)
127{
128  XFREE (MTYPE_PQUEUE_DATA, queue->array);
129  XFREE (MTYPE_PQUEUE, queue);
130}
131
132static int
133pqueue_expand (struct pqueue *queue)
134{
135  void **newarray;
136
137  newarray = XCALLOC (MTYPE_PQUEUE_DATA, queue->array_size * DATA_SIZE * 2);
138  if (newarray == NULL)
139    return 0;
140
141  memcpy (newarray, queue->array, queue->array_size * DATA_SIZE);
142
143  XFREE (MTYPE_PQUEUE_DATA, queue->array);
144  queue->array = newarray;
145  queue->array_size *= 2;
146
147  return 1;
148}
149
150void
151pqueue_enqueue (void *data, struct pqueue *queue)
152{
153  if (queue->size + 2 >= queue->array_size && ! pqueue_expand (queue))
154    return;
155
156  queue->array[queue->size] = data;
157  if (queue->update != NULL)
158    (*queue->update) (data, queue->size);
159  trickle_up (queue->size, queue);
160  queue->size ++;
161}
162
163void *
164pqueue_dequeue (struct pqueue *queue)
165{
166  void *data = queue->array[0];
167  queue->array[0] =  queue->array[--queue->size];
168  trickle_down (0, queue);
169  return data;
170}
171
172void
173pqueue_remove_at (int index, struct pqueue *queue)
174{
175  queue->array[index] = queue->array[--queue->size];
176
177  if (index > 0
178      && (*queue->cmp) (queue->array[index],
179                        queue->array[PARENT_OF(index)]) < 0)
180    {
181      trickle_up (index, queue);
182    }
183  else
184    {
185      trickle_down (index, queue);
186    }
187}
188