Fix assertion for proper Huffman merge pattern: 0 == 1 modulo 1.
[pspp] / src / algorithm.c
1 /* PSPP - computes sample statistics.
2    Copyright (C) 1997-9, 2000 Free Software Foundation, Inc.
3    Written by Ben Pfaff <blp@gnu.org>.
4
5    This program is free software; you can redistribute it and/or
6    modify it under the terms of the GNU General Public License as
7    published by the Free Software Foundation; either version 2 of the
8    License, or (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful, but
11    WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13    General Public License for more details.
14
15    You should have received a copy of the GNU General Public License
16    along with this program; if not, write to the Free Software
17    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
18    02111-1307, USA. */
19
20 /* Copyright (C) 2001 Free Software Foundation, Inc.
21   
22    This file is part of the GNU ISO C++ Library.  This library is free
23    software; you can redistribute it and/or modify it under the
24    terms of the GNU General Public License as published by the
25    Free Software Foundation; either version 2, or (at your option)
26    any later version.
27
28    This library is distributed in the hope that it will be useful,
29    but WITHOUT ANY WARRANTY; without even the implied warranty of
30    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
31    GNU General Public License for more details.
32
33    You should have received a copy of the GNU General Public License along
34    with this library; see the file COPYING.  If not, write to the Free
35    Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
36    USA.
37
38    As a special exception, you may use this file as part of a free software
39    library without restriction.  Specifically, if other files instantiate
40    templates or use macros or inline functions from this file, or you compile
41    this file and link it with other files to produce an executable, this
42    file does not by itself cause the resulting executable to be covered by
43    the GNU General Public License.  This exception does not however
44    invalidate any other reasons why the executable file might be covered by
45    the GNU General Public License. */
46
47 /*
48  *
49  * Copyright (c) 1994
50  * Hewlett-Packard Company
51  *
52  * Permission to use, copy, modify, distribute and sell this software
53  * and its documentation for any purpose is hereby granted without fee,
54  * provided that the above copyright notice appear in all copies and
55  * that both that copyright notice and this permission notice appear
56  * in supporting documentation.  Hewlett-Packard Company makes no
57  * representations about the suitability of this software for any
58  * purpose.  It is provided "as is" without express or implied warranty.
59  *
60  *
61  * Copyright (c) 1996
62  * Silicon Graphics Computer Systems, Inc.
63  *
64  * Permission to use, copy, modify, distribute and sell this software
65  * and its documentation for any purpose is hereby granted without fee,
66  * provided that the above copyright notice appear in all copies and
67  * that both that copyright notice and this permission notice appear
68  * in supporting documentation.  Silicon Graphics makes no
69  * representations about the suitability of this software for any
70  * purpose.  It is provided "as is" without express or implied warranty.
71  */
72
73 /* Copyright (C) 1991, 1992, 1996, 1997, 1999 Free Software Foundation, Inc.
74    This file is part of the GNU C Library.
75    Written by Douglas C. Schmidt (schmidt@ics.uci.edu).
76
77    The GNU C Library is free software; you can redistribute it and/or
78    modify it under the terms of the GNU Lesser General Public
79    License as published by the Free Software Foundation; either
80    version 2.1 of the License, or (at your option) any later version.
81
82    The GNU C Library is distributed in the hope that it will be useful,
83    but WITHOUT ANY WARRANTY; without even the implied warranty of
84    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
85    Lesser General Public License for more details.
86
87    You should have received a copy of the GNU Lesser General Public
88    License along with the GNU C Library; if not, write to the Free
89    Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
90    02111-1307 USA.  */
91
92 #include <config.h>
93 #include "algorithm.h"
94 #include <gsl/gsl_rng.h>
95 #include <limits.h>
96 #include <stdlib.h>
97 #include <string.h>
98 #include "alloc.h"
99 #include "settings.h"
100
101 /* Some of the assertions in this file are very expensive.  We
102    don't use them by default. */
103 #ifdef EXTRA_CHECKS
104 #define expensive_assert(X) assert(X)
105 #else
106 #define expensive_assert(X) ((void) 0)
107 #endif
108 #include "error.h"
109 \f
110 /* Finds an element in ARRAY, which contains COUNT elements of
111    SIZE bytes each, using COMPARE for comparisons.  Returns the
112    first element in ARRAY that matches TARGET, or a null pointer
113    on failure.  AUX is passed to each comparison as auxiliary
114    data. */
115 void *
116 find (const void *array, size_t count, size_t size,
117       const void *target,
118       algo_compare_func *compare, void *aux) 
119 {
120   const unsigned char *element = array;
121
122   while (count-- > 0) 
123     {
124       if (compare (target, element, aux) == 0)
125         return (void *) element;
126
127       element += size;
128     }
129
130   return NULL;
131 }
132
133 /* Counts and return the number of elements in ARRAY, which
134    contains COUNT elements of SIZE bytes each, which are equal to
135    ELEMENT as compared with COMPARE.  AUX is passed as auxiliary
136    data to COMPARE. */
137 size_t
138 count_equal (const void *array, size_t count, size_t size,
139              const void *element,
140              algo_compare_func *compare, void *aux)
141 {
142   const unsigned char *first = array;
143   size_t equal_cnt = 0;
144
145   while (count-- > 0) 
146     {
147       if (compare (element, first, aux) == 0)
148         equal_cnt++;
149       
150       first += size;
151     }
152
153   return equal_cnt;
154 }
155
156 /* Counts and return the number of elements in ARRAY, which
157    contains COUNT elements of SIZE bytes each, for which
158    PREDICATE returns nonzero.  AUX is passed as auxiliary data to
159    PREDICATE. */
160 size_t
161 count_if (const void *array, size_t count, size_t size,
162           algo_predicate_func *predicate, void *aux) 
163 {
164   const unsigned char *first = array;
165   size_t nonzero_cnt = 0;
166
167   while (count-- > 0) 
168     {
169       if (predicate (first, aux) != 0)
170         nonzero_cnt++;
171       
172       first += size;
173     }
174
175   return nonzero_cnt;
176 }
177 \f
178 /* Byte-wise swap two items of size SIZE. */
179 #define SWAP(a, b, size)                        \
180   do                                            \
181     {                                           \
182       register size_t __size = (size);          \
183       register char *__a = (a), *__b = (b);     \
184       do                                        \
185         {                                       \
186           char __tmp = *__a;                    \
187           *__a++ = *__b;                        \
188           *__b++ = __tmp;                       \
189         } while (--__size > 0);                 \
190     } while (0)
191
192 /* Makes the elements in ARRAY unique, by moving up duplicates,
193    and returns the new number of elements in the array.  Sorted
194    arrays only.  Arguments same as for sort() above. */
195 size_t
196 unique (void *array, size_t count, size_t size,
197         algo_compare_func *compare, void *aux) 
198 {
199   char *first = array;
200   char *last = first + size * count;
201   char *result = array;
202
203   for (;;) 
204     {
205       first += size;
206       if (first >= last) 
207         {
208           assert (adjacent_find_equal (array, count,
209                                        size, compare, aux) == NULL);
210           return count; 
211         }
212
213       if (compare (result, first, aux)) 
214         {
215           result += size;
216           if (result != first)
217             memcpy (result, first, size);
218         }
219       else 
220         count--;
221     }
222 }
223
224 /* Helper function that calls sort(), then unique(). */
225 size_t
226 sort_unique (void *array, size_t count, size_t size,
227              algo_compare_func *compare, void *aux) 
228 {
229   sort (array, count, size, compare, aux);
230   return unique (array, count, size, compare, aux);
231 }
232 \f
233 /* Reorders ARRAY, which contains COUNT elements of SIZE bytes
234    each, so that the elements for which PREDICATE returns nonzero
235    precede those for which PREDICATE returns zero.  AUX is
236    passed to each predicate as auxiliary data.  Returns the
237    number of elements for which PREDICATE returns nonzero.  Not
238    stable. */
239 size_t 
240 partition (void *array, size_t count, size_t size,
241            algo_predicate_func *predicate, void *aux) 
242 {
243   size_t nonzero_cnt = count;
244   char *first = array;
245   char *last = first + nonzero_cnt * size;
246
247   for (;;)
248     {
249       /* Move FIRST forward to point to first element that fails
250          PREDICATE. */
251       for (;;) 
252         {
253           if (first == last)
254             goto done;
255           else if (!predicate (first, aux)) 
256             break;
257
258           first += size; 
259         }
260       nonzero_cnt--;
261
262       /* Move LAST backward to point to last element that passes
263          PREDICATE. */
264       for (;;) 
265         {
266           last -= size;
267
268           if (first == last)
269             goto done;
270           else if (predicate (last, aux)) 
271             break;
272           else
273             nonzero_cnt--;
274         }
275       
276       /* By swapping FIRST and LAST we extend the starting and
277          ending sequences that pass and fail, respectively,
278          PREDICATE. */
279       SWAP (first, last, size);
280       first += size;
281     }
282
283  done:
284   assert (is_partitioned (array, count, size, nonzero_cnt, predicate, aux));
285   return nonzero_cnt; 
286 }
287
288 /* Checks whether ARRAY, which contains COUNT elements of SIZE
289    bytes each, is partitioned such that PREDICATE returns nonzero
290    for the first NONZERO_CNT elements and zero for the remaining
291    elements.  AUX is passed as auxiliary data to PREDICATE. */
292 int
293 is_partitioned (const void *array, size_t count, size_t size,
294                 size_t nonzero_cnt,
295                 algo_predicate_func *predicate, void *aux) 
296 {
297   const unsigned char *first = array;
298   size_t idx;
299
300   assert (nonzero_cnt <= count);
301   for (idx = 0; idx < nonzero_cnt; idx++)
302     if (predicate (first + idx * size, aux) == 0)
303       return 0;
304   for (idx = nonzero_cnt; idx < count; idx++)
305     if (predicate (first + idx * size, aux) != 0)
306       return 0;
307   return 1;
308 }
309 \f
310 /* A algo_random_func that uses random.h. */
311 unsigned
312 algo_default_random (unsigned max, void *aux UNUSED) 
313 {
314   unsigned long r_min = gsl_rng_min (get_rng ());
315   return (gsl_rng_get (get_rng ()) - r_min) % max;
316 }
317
318 /* Randomly reorders ARRAY, which contains COUNT elements of SIZE
319    bytes each.  Uses RANDOM as a source of random data, passing
320    AUX as the auxiliary data.  RANDOM may be null to use a
321    default random source. */
322 void
323 random_shuffle (void *array_, size_t count, size_t size,
324                 algo_random_func *random, void *aux)
325 {
326   unsigned char *array = array_;
327   int i;
328
329   if (random == NULL)
330     random = algo_default_random;
331
332   for (i = 1; i < count; i++)
333     SWAP (array + i * size, array + random (i + 1, aux) * size, size);
334 }
335 \f
336 /* Copies the COUNT elements of SIZE bytes each from ARRAY to
337    RESULT, except that elements for which PREDICATE is false are
338    not copied.  Returns the number of elements copied.  AUX is
339    passed to PREDICATE as auxiliary data.  */
340 size_t 
341 copy_if (const void *array, size_t count, size_t size,
342          void *result,
343          algo_predicate_func *predicate, void *aux) 
344 {
345   const unsigned char *input = array;
346   const unsigned char *last = input + size * count;
347   unsigned char *output = result;
348   size_t nonzero_cnt = 0;
349   
350   while (input < last)
351     {
352       if (predicate (input, aux)) 
353         {
354           memcpy (output, input, size);
355           output += size;
356           nonzero_cnt++;
357         }
358
359       input += size;
360     }
361
362   assert (nonzero_cnt == count_if (array, count, size, predicate, aux));
363   assert (nonzero_cnt == count_if (result, nonzero_cnt, size, predicate, aux));
364
365   return nonzero_cnt;
366 }
367
368 /* A predicate and its auxiliary data. */
369 struct pred_aux 
370   {
371     algo_predicate_func *predicate;
372     void *aux;
373   };
374
375 static int
376 not (const void *data, void *pred_aux_) 
377 {
378   const struct pred_aux *pred_aux = pred_aux_;
379
380   return !pred_aux->predicate (data, pred_aux->aux);
381 }
382
383 /* Removes elements equal to ELEMENT from ARRAY, which consists
384    of COUNT elements of SIZE bytes each.  Returns the number of
385    remaining elements.  AUX is passed to COMPARE as auxiliary
386    data. */
387 size_t
388 remove_equal (void *array, size_t count, size_t size,
389               void *element,
390               algo_compare_func *compare, void *aux) 
391 {
392   unsigned char *first = array;
393   unsigned char *last = first + count * size;
394   unsigned char *result;
395
396   for (;;)
397     {
398       if (first >= last)
399         goto done;
400       if (compare (first, element, aux) == 0)
401         break;
402
403       first += size;
404     }
405
406   result = first;
407   count--;
408   for (;;) 
409     {
410       first += size;
411       if (first >= last)
412         goto done;
413
414       if (compare (first, element, aux) == 0) 
415         {
416           count--; 
417           continue;
418         }
419       
420       memcpy (result, first, size);
421       result += size;
422     }
423
424  done:
425   assert (count_equal (array, count, size, element, compare, aux) == 0);
426   return count;
427 }
428
429 /* Copies the COUNT elements of SIZE bytes each from ARRAY to
430    RESULT, except that elements for which PREDICATE is true are
431    not copied.  Returns the number of elements copied.  AUX is
432    passed to PREDICATE as auxiliary data.  */
433 size_t 
434 remove_copy_if (const void *array, size_t count, size_t size,
435                 void *result,
436                 algo_predicate_func *predicate, void *aux) 
437 {
438   struct pred_aux pred_aux;
439   pred_aux.predicate = predicate;
440   pred_aux.aux = aux;
441   return copy_if (array, count, size, result, not, &pred_aux);
442 }
443 \f
444 /* Searches ARRAY, which contains COUNT of SIZE bytes each, using
445    a binary search.  Returns any element that equals VALUE, if
446    one exists, or a null pointer otherwise.  ARRAY must ordered
447    according to COMPARE.  AUX is passed to COMPARE as auxiliary
448    data. */
449 void *
450 binary_search (const void *array, size_t count, size_t size,
451                void *value,
452                algo_compare_func *compare, void *aux) 
453 {
454   assert (array != NULL);
455   assert (count <= INT_MAX);
456   assert (compare != NULL);
457
458   if (count != 0) 
459     {
460       const unsigned char *first = array;
461       int low = 0;
462       int high = count - 1;
463
464       while (low <= high) 
465         {
466           int middle = (low + high) / 2;
467           const unsigned char *element = first + middle * size;
468           int cmp = compare (value, element, aux);
469
470           if (cmp > 0) 
471             low = middle + 1;
472           else if (cmp < 0)
473             high = middle - 1;
474           else
475             return (void *) element;
476         }
477     }
478
479   expensive_assert (find (array, count, size, value, compare, aux) == NULL);
480   return NULL;
481 }
482 \f
483 /* Lexicographically compares ARRAY1, which contains COUNT1
484    elements of SIZE bytes each, to ARRAY2, which contains COUNT2
485    elements of SIZE bytes, according to COMPARE.  Returns a
486    strcmp()-type result.  AUX is passed to COMPARE as auxiliary
487    data. */
488 int
489 lexicographical_compare_3way (const void *array1, size_t count1,
490                               const void *array2, size_t count2,
491                               size_t size,
492                               algo_compare_func *compare, void *aux) 
493 {
494   const unsigned char *first1 = array1;
495   const unsigned char *first2 = array2;
496   size_t min_count = count1 < count2 ? count1 : count2;
497
498   while (min_count > 0)
499     {
500       int cmp = compare (first1, first2, aux);
501       if (cmp != 0)
502         return cmp;
503
504       first1 += size;
505       first2 += size;
506       min_count--;
507     }
508
509   return count1 < count2 ? -1 : count1 > count2;
510 }
511 \f
512 /* If you consider tuning this algorithm, you should consult first:
513    Engineering a sort function; Jon Bentley and M. Douglas McIlroy;
514    Software - Practice and Experience; Vol. 23 (11), 1249-1265, 1993.  */
515
516 #ifdef HAVE_ALLOCA_H
517 #include <alloca.h>
518 #endif
519
520 #include <limits.h>
521 #include <stdlib.h>
522 #include <string.h>
523
524 /* Discontinue quicksort algorithm when partition gets below this size.
525    This particular magic number was chosen to work best on a Sun 4/260. */
526 #define MAX_THRESH 4
527
528 /* Stack node declarations used to store unfulfilled partition obligations. */
529 typedef struct
530   {
531     char *lo;
532     char *hi;
533   } stack_node;
534
535 /* The next 4 #defines implement a very fast in-line stack abstraction. */
536 /* The stack needs log (total_elements) entries (we could even subtract
537    log(MAX_THRESH)).  Since total_elements has type size_t, we get as
538    upper bound for log (total_elements):
539    bits per byte (CHAR_BIT) * sizeof(size_t).  */
540 #define STACK_SIZE      (CHAR_BIT * sizeof(size_t))
541 #define PUSH(low, high) ((void) ((top->lo = (low)), (top->hi = (high)), ++top))
542 #define POP(low, high)  ((void) (--top, (low = top->lo), (high = top->hi)))
543 #define STACK_NOT_EMPTY (stack < top)
544
545
546 /* Order size using quicksort.  This implementation incorporates
547    four optimizations discussed in Sedgewick:
548
549    1. Non-recursive, using an explicit stack of pointer that store the
550       next array partition to sort.  To save time, this maximum amount
551       of space required to store an array of SIZE_MAX is allocated on the
552       stack.  Assuming a 32-bit (64 bit) integer for size_t, this needs
553       only 32 * sizeof(stack_node) == 256 bytes (for 64 bit: 1024 bytes).
554       Pretty cheap, actually.
555
556    2. Chose the pivot element using a median-of-three decision tree.
557       This reduces the probability of selecting a bad pivot value and
558       eliminates certain extraneous comparisons.
559
560    3. Only quicksorts TOTAL_ELEMS / MAX_THRESH partitions, leaving
561       insertion sort to order the MAX_THRESH items within each partition.
562       This is a big win, since insertion sort is faster for small, mostly
563       sorted array segments.
564
565    4. The larger of the two sub-partitions is always pushed onto the
566       stack first, with the algorithm then concentrating on the
567       smaller partition.  This *guarantees* no more than log (total_elems)
568       stack size is needed (actually O(1) in this case)!  */
569
570 void
571 sort (void *array, size_t count, size_t size,
572       algo_compare_func *compare, void *aux)
573 {
574   char *const first = array;
575   const size_t max_thresh = MAX_THRESH * size;
576
577   if (count == 0)
578     /* Avoid lossage with unsigned arithmetic below.  */
579     return;
580
581   if (count > MAX_THRESH)
582     {
583       char *lo = first;
584       char *hi = &lo[size * (count - 1)];
585       stack_node stack[STACK_SIZE];
586       stack_node *top = stack + 1;
587
588       while (STACK_NOT_EMPTY)
589         {
590           char *left_ptr;
591           char *right_ptr;
592
593           /* Select median value from among LO, MID, and HI. Rearrange
594              LO and HI so the three values are sorted. This lowers the
595              probability of picking a pathological pivot value and
596              skips a comparison for both the LEFT_PTR and RIGHT_PTR in
597              the while loops. */
598
599           char *mid = lo + size * ((hi - lo) / size >> 1);
600
601           if (compare (mid, lo, aux) < 0)
602             SWAP (mid, lo, size);
603           if (compare (hi, mid, aux) < 0)
604             SWAP (mid, hi, size);
605           else
606             goto jump_over;
607           if (compare (mid, lo, aux) < 0)
608             SWAP (mid, lo, size);
609         jump_over:;
610
611           left_ptr  = lo + size;
612           right_ptr = hi - size;
613
614           /* Here's the famous ``collapse the walls'' section of quicksort.
615              Gotta like those tight inner loops!  They are the main reason
616              that this algorithm runs much faster than others. */
617           do
618             {
619               while (compare (left_ptr, mid, aux) < 0)
620                 left_ptr += size;
621
622               while (compare (mid, right_ptr, aux) < 0)
623                 right_ptr -= size;
624
625               if (left_ptr < right_ptr)
626                 {
627                   SWAP (left_ptr, right_ptr, size);
628                   if (mid == left_ptr)
629                     mid = right_ptr;
630                   else if (mid == right_ptr)
631                     mid = left_ptr;
632                   left_ptr += size;
633                   right_ptr -= size;
634                 }
635               else if (left_ptr == right_ptr)
636                 {
637                   left_ptr += size;
638                   right_ptr -= size;
639                   break;
640                 }
641             }
642           while (left_ptr <= right_ptr);
643
644           /* Set up pointers for next iteration.  First determine whether
645              left and right partitions are below the threshold size.  If so,
646              ignore one or both.  Otherwise, push the larger partition's
647              bounds on the stack and continue sorting the smaller one. */
648
649           if ((size_t) (right_ptr - lo) <= max_thresh)
650             {
651               if ((size_t) (hi - left_ptr) <= max_thresh)
652                 /* Ignore both small partitions. */
653                 POP (lo, hi);
654               else
655                 /* Ignore small left partition. */
656                 lo = left_ptr;
657             }
658           else if ((size_t) (hi - left_ptr) <= max_thresh)
659             /* Ignore small right partition. */
660             hi = right_ptr;
661           else if ((right_ptr - lo) > (hi - left_ptr))
662             {
663               /* Push larger left partition indices. */
664               PUSH (lo, right_ptr);
665               lo = left_ptr;
666             }
667           else
668             {
669               /* Push larger right partition indices. */
670               PUSH (left_ptr, hi);
671               hi = right_ptr;
672             }
673         }
674     }
675
676   /* Once the FIRST array is partially sorted by quicksort the rest
677      is completely sorted using insertion sort, since this is efficient
678      for partitions below MAX_THRESH size. FIRST points to the beginning
679      of the array to sort, and END_PTR points at the very last element in
680      the array (*not* one beyond it!). */
681
682 #define min(x, y) ((x) < (y) ? (x) : (y))
683
684   {
685     char *const end_ptr = &first[size * (count - 1)];
686     char *tmp_ptr = first;
687     char *thresh = min(end_ptr, first + max_thresh);
688     register char *run_ptr;
689
690     /* Find smallest element in first threshold and place it at the
691        array's beginning.  This is the smallest array element,
692        and the operation speeds up insertion sort's inner loop. */
693
694     for (run_ptr = tmp_ptr + size; run_ptr <= thresh; run_ptr += size)
695       if (compare (run_ptr, tmp_ptr, aux) < 0)
696         tmp_ptr = run_ptr;
697
698     if (tmp_ptr != first)
699       SWAP (tmp_ptr, first, size);
700
701     /* Insertion sort, running from left-hand-side up to right-hand-side.  */
702
703     run_ptr = first + size;
704     while ((run_ptr += size) <= end_ptr)
705       {
706         tmp_ptr = run_ptr - size;
707         while (compare (run_ptr, tmp_ptr, aux) < 0)
708           tmp_ptr -= size;
709
710         tmp_ptr += size;
711         if (tmp_ptr != run_ptr)
712           {
713             char *trav;
714
715             trav = run_ptr + size;
716             while (--trav >= run_ptr)
717               {
718                 char c = *trav;
719                 char *hi, *lo;
720
721                 for (hi = lo = trav; (lo -= size) >= tmp_ptr; hi = lo)
722                   *hi = *lo;
723                 *hi = c;
724               }
725           }
726       }
727   }
728
729   assert (is_sorted (array, count, size, compare, aux));
730 }
731
732 /* Tests whether ARRAY, which contains COUNT elements of SIZE
733    bytes each, is sorted in order according to COMPARE.  AUX is
734    passed to COMPARE as auxiliary data. */
735 int
736 is_sorted (const void *array, size_t count, size_t size,
737            algo_compare_func *compare, void *aux) 
738 {
739   const unsigned char *first = array;
740   size_t idx;
741       
742   for (idx = 0; idx + 1 < count; idx++)
743     if (compare (first + idx * size, first + (idx + 1) * size, aux) > 0)
744       return 0; 
745   
746   return 1;
747 }
748 \f
749 /* Computes the generalized set difference, ARRAY1 minus ARRAY2,
750    into RESULT, and returns the number of elements written to
751    RESULT.  If a value appears M times in ARRAY1 and N times in
752    ARRAY2, then it will appear max(M - N, 0) in RESULT.  ARRAY1
753    and ARRAY2 must be sorted, and RESULT is sorted and stable.
754    ARRAY1 consists of COUNT1 elements, ARRAY2 of COUNT2 elements,
755    each SIZE bytes.  AUX is passed to COMPARE as auxiliary
756    data. */
757 size_t set_difference (const void *array1, size_t count1,
758                        const void *array2, size_t count2,
759                        size_t size,
760                        void *result_,
761                        algo_compare_func *compare, void *aux) 
762 {
763   const unsigned char *first1 = array1;
764   const unsigned char *last1 = first1 + count1 * size;
765   const unsigned char *first2 = array2;
766   const unsigned char *last2 = first2 + count2 * size;
767   unsigned char *result = result_;
768   size_t result_count = 0;
769   
770   while (first1 != last1 && first2 != last2) 
771     {
772       int cmp = compare (first1, first2, aux);
773       if (cmp < 0)
774         {
775           memcpy (result, first1, size);
776           first1 += size;
777           result += size;
778           result_count++;
779         }
780       else if (cmp > 0)
781         first2 += size;
782       else
783         {
784           first1 += size;
785           first2 += size;
786         }
787     }
788
789   while (first1 != last1) 
790     {
791       memcpy (result, first1, size);
792       first1 += size;
793       result += size;
794       result_count++;
795     }
796
797   return result_count;
798 }
799 \f
800 /* Finds the first pair of adjacent equal elements in ARRAY,
801    which has COUNT elements of SIZE bytes.  Returns the first
802    element in ARRAY such that COMPARE returns zero when it and
803    its successor element are compared, or a null pointer if no
804    such element exists.  AUX is passed to COMPARE as auxiliary
805    data. */
806 void *
807 adjacent_find_equal (const void *array, size_t count, size_t size,
808                      algo_compare_func *compare, void *aux) 
809 {
810   const unsigned char *first = array;
811   const unsigned char *last = first + count * size;
812
813   while (first < last && first + size < last) 
814     {
815       if (compare (first, first + size, aux) == 0)
816         return (void *) first;
817       first += size;
818     }
819
820   return NULL;
821 }
822 \f
823 /* ARRAY contains COUNT elements of SIZE bytes each.  Initially
824    the first COUNT - 1 elements of these form a heap, followed by
825    a single element not part of the heap.  This function adds the
826    final element, forming a heap of COUNT elements in ARRAY.
827    Uses COMPARE to compare elements, passing AUX as auxiliary
828    data. */
829 void
830 push_heap (void *array, size_t count, size_t size,
831            algo_compare_func *compare, void *aux) 
832 {
833   unsigned char *first = array;
834   size_t i;
835   
836   expensive_assert (count < 1 || is_heap (array, count - 1,
837                                           size, compare, aux));
838   for (i = count; i > 1; i /= 2) 
839     {
840       unsigned char *parent = first + (i / 2 - 1) * size;
841       unsigned char *element = first + (i - 1) * size;
842       if (compare (parent, element, aux) < 0)
843         SWAP (parent, element, size);
844       else
845         break; 
846     }
847   expensive_assert (is_heap (array, count, size, compare, aux));
848 }
849
850 /* ARRAY contains COUNT elements of SIZE bytes each.  Initially
851    the children of ARRAY[idx - 1] are heaps, but ARRAY[idx - 1]
852    may be smaller than its children.  This function fixes that,
853    so that ARRAY[idx - 1] itself is a heap.  Uses COMPARE to
854    compare elements, passing AUX as auxiliary data. */
855 static void
856 heapify (void *array, size_t count, size_t size,
857          size_t idx,
858          algo_compare_func *compare, void *aux) 
859 {
860   unsigned char *first = array;
861   
862   for (;;) 
863     {
864       size_t left = 2 * idx;
865       size_t right = left + 1;
866       size_t largest = idx;
867
868       if (left <= count
869           && compare (first + size * (left - 1),
870                       first + size * (idx - 1), aux) > 0)
871         largest = left;
872
873       if (right <= count
874           && compare (first + size * (right - 1),
875                       first + size * (largest - 1), aux) > 0)
876         largest = right;
877
878       if (largest == idx)
879         break;
880
881       SWAP (first + size * (idx - 1), first + size * (largest - 1), size);
882       idx = largest;
883     }
884 }
885
886 /* ARRAY contains COUNT elements of SIZE bytes each.  Initially
887    all COUNT elements form a heap.  This function moves the
888    largest element in the heap to the final position in ARRAY and
889    reforms a heap of the remaining COUNT - 1 elements at the
890    beginning of ARRAY.  Uses COMPARE to compare elements, passing
891    AUX as auxiliary data. */
892 void
893 pop_heap (void *array, size_t count, size_t size,
894           algo_compare_func *compare, void *aux) 
895 {
896   unsigned char *first = array;
897
898   expensive_assert (is_heap (array, count, size, compare, aux));
899   SWAP (first, first + (count - 1) * size, size);
900   heapify (first, count - 1, size, 1, compare, aux);
901   expensive_assert (count < 1 || is_heap (array, count - 1,
902                                           size, compare, aux));
903 }
904
905 /* Turns ARRAY, which contains COUNT elements of SIZE bytes, into
906    a heap.  Uses COMPARE to compare elements, passing AUX as
907    auxiliary data. */
908 void
909 make_heap (void *array, size_t count, size_t size,
910            algo_compare_func *compare, void *aux) 
911 {
912   size_t idx;
913   
914   for (idx = count / 2; idx >= 1; idx--)
915     heapify (array, count, size, idx, compare, aux);
916   expensive_assert (count < 1 || is_heap (array, count, size, compare, aux));
917 }
918
919 /* ARRAY contains COUNT elements of SIZE bytes each.  Initially
920    all COUNT elements form a heap.  This function turns the heap
921    into a fully sorted array.  Uses COMPARE to compare elements,
922    passing AUX as auxiliary data. */
923 void
924 sort_heap (void *array, size_t count, size_t size,
925            algo_compare_func *compare, void *aux) 
926 {
927   unsigned char *first = array;
928   size_t idx;
929
930   expensive_assert (is_heap (array, count, size, compare, aux));
931   for (idx = count; idx >= 2; idx--)
932     {
933       SWAP (first, first + (idx - 1) * size, size);
934       heapify (array, idx - 1, size, 1, compare, aux);
935     }
936   expensive_assert (is_sorted (array, count, size, compare, aux));
937 }
938
939 /* ARRAY contains COUNT elements of SIZE bytes each.  This
940    function tests whether ARRAY is a heap and returns 1 if so, 0
941    otherwise.  Uses COMPARE to compare elements, passing AUX as
942    auxiliary data. */
943 int
944 is_heap (const void *array, size_t count, size_t size,
945          algo_compare_func *compare, void *aux) 
946 {
947   const unsigned char *first = array;
948   size_t child;
949   
950   for (child = 2; child <= count; child++)
951     {
952       size_t parent = child / 2;
953       if (compare (first + (parent - 1) * size,
954                    first + (child - 1) * size, aux) < 0)
955         return 0;
956     }
957
958   return 1;
959 }
960