portability fixes
[rrq/rrqmisc.git] / vector / Vector.c
1 #include <string.h>
2 #include <math.h>
3 #include <stdlib.h>
4 #include <Vector.h>
5
6 /**
7  * \brief Re-allocate memory for the index of a Single_index_level
8  * Vector.
9  */
10 static VectorPage* Vector_single_realloc(Vector *pv,size_t new_size) {
11     new_size = new_size * sizeof( void* );
12     VectorPage *p = (VectorPage*) malloc( new_size );
13     if ( p ) {
14         size_t old_size = pv->size * sizeof( void* );
15         if ( old_size < new_size ) {
16             memset( ((char*)p) + old_size, 0, new_size - old_size );
17             memcpy( p, pv->entries, old_size );
18         } else {
19             memcpy( p, pv->entries, new_size );
20         }
21         free( pv->entries );
22     }
23     return p;
24 }
25
26 /**
27  * \brief Re-allocate memory for the index of a Single_index_level
28  * Vector.
29  */
30 static VectorPage* Vector_indexpage_calloc(Vector *pv) {
31     size_t size = VECTOR_SLOTS( pv ) * sizeof( void* );
32     VectorPage *p = (VectorPage*) malloc( size );
33     if ( p ) {
34         memset( p, 0, size );
35     }
36     return p;
37 }
38
39 /**
40  * Representing a Vector of void* accessible via an indexing structure
41  * as levels of same-size pages. A "VectorPage" is a contiguous array
42  * void*, and an index is "unsigned long" (64 bits).
43  */
44
45 /** ============================================================ **/
46
47 static int VECTOR_BITS[4] = { 8, 4, 2, 64 };
48
49 typedef struct {
50     unsigned int a:2;
51     unsigned int b:2;
52     unsigned int c:2;
53     unsigned int d:2;
54 } BitPair;
55
56 typedef struct {
57     unsigned int a:4;
58     unsigned int b:4;
59 } Nibble;
60
61 typedef struct {
62     unsigned int a:8;
63 } Byte;
64
65 /**
66  * Return the index part for the given level of the Vector's leveling
67  * variant.
68  *
69  * The Vector variant indicates whether indexing uses 8, 4, or 2 bits
70  * per level.
71  */
72 unsigned long VECTOR_INDEX_PART(Vector *pv,VectorIndex *index, int level) {
73     unsigned char *px = (unsigned char *) index;
74     switch ( pv->variant ) {
75     case Byte_index_levels: {
76         Byte *pp = (Byte*)(px + level);
77         return pp->a;
78     }
79     case Nibble_index_levels: {
80         Nibble *pp = (Nibble*)( px + ( level / 2 ) );
81         switch ( level & 1 ) {
82         case 0: return pp->a;
83         case 1: return pp->b;
84         }
85         break;
86     }
87     case BitPair_index_levels: {
88         BitPair *pp = (BitPair*)( px + ( level / 4 ) );
89         switch ( level & 3 ) {
90         case 0: return pp->a;
91         case 1: return pp->b;
92         case 2: return pp->c;
93         case 3: return pp->d;
94         }
95         break;
96     }
97     case Single_index_level:
98         return (*index);
99     }
100     return 0;
101 }
102
103 /**
104  * Increment the index part at the indivated level, cyclic but not
105  * carrying over to the upper level. Returns the new level index.
106  */
107 static unsigned long VECTOR_INDEX_PART_INC(
108     Vector *pv,VectorIndex *index, int level)
109 {
110     unsigned char *px = (unsigned char *) index;
111     switch ( pv->variant ) {
112     case Byte_index_levels: {
113         Byte *pp = (Byte*)( px + level );
114         return ++(pp->a);
115     }
116     case Nibble_index_levels: {
117         Nibble *pp = (Nibble*)( px + ( level / 2 ) );
118         switch ( level & 1 ) {
119         case 0: return ++(pp->a);
120         case 1: return ++(pp->b);
121         }
122         break;
123     }
124     case BitPair_index_levels: {
125         BitPair *pp = (BitPair*)( px + level / 4 );
126         switch ( level & 3 ) {
127         case 0: return ++(pp->a);
128         case 1: return ++(pp->b);
129         case 2: return ++(pp->c);
130         case 3: return ++(pp->d);
131         }
132         break;
133     }
134     case Single_index_level:
135         return ++(*index);
136     }
137     return 0;
138 }
139
140 /**
141  * Decrement the index part at the indicated level, cyclic but not
142  * carrying over to the upper level. Returns the prior level index.
143  */
144 static unsigned long VECTOR_INDEX_PART_DEC(
145     Vector *pv,VectorIndex *index, int level)
146 {
147     unsigned char *px = (unsigned char *) index;
148     switch ( pv->variant ) {
149     case Byte_index_levels: {
150         Byte *pp = (Byte*)( px + level );
151         return (pp->a)--;
152     }
153     case Nibble_index_levels: {
154         Nibble *pp = (Nibble*)( px + ( level / 2 ) );
155         switch ( level & 1 ) {
156         case 0: return (pp->a)--;
157         case 1: return (pp->b)--;
158         }
159         break;
160     }
161     case BitPair_index_levels: {
162         BitPair *pp = (BitPair*)( px + level / 4 );
163         switch ( level & 0xf ) {
164         case 0: return (pp->a)--;
165         case 1: return (pp->b)--;
166         case 2: return (pp->c)--;
167         case 3: return (pp->d)--;
168         }
169         break;
170     }
171     case Single_index_level:
172         return (*index)--;
173     }
174     return 0;
175 }
176
177 #define ONES  (~((VectorIndex) 0))
178
179 // Set index to first value for all index parts at level and lower.
180 static void VECTOR_INDEX_FIRST(Vector *pv,VectorIndex *index, int level) {
181     (*index) &= ONES << ( VECTOR_BITS[ pv->variant ] * level );
182 }
183
184 // Set index to last value for all index parts at level and lower.
185 static void VECTOR_INDEX_LAST(Vector *pv,VectorIndex *index, int level) {
186     static unsigned long ones[] = { 255, 15, 3 };
187     unsigned long x = ones[ pv->variant ];
188     while ( level-- ) {
189         (*index) |= x;
190         x <<= VECTOR_BITS[ pv->variant ];
191     }
192     // 255, 25, 3
193     //(*index) |= ONES >> ( 64 - ( VECTOR_BITS[ pv->variant ] * level ) );
194 }
195
196 // Return number of slots for a Vector variant.
197 unsigned long VECTOR_SLOTS(Vector *pv) {
198     switch ( pv->variant ) {
199     case Byte_index_levels: return 256;
200     case Nibble_index_levels: return 16;
201     case BitPair_index_levels: return 4;
202     case Single_index_level: return pv->size;
203     }
204     return 0;
205 }
206
207 // The number of levels to span a given size for Vector pv
208 static unsigned int Vector_levels(Vector *pv,unsigned long size) {
209     if ( size < 4 || pv->variant == Single_index_level ) {
210         return 1;
211     }
212     // N:o bits in size.
213     int w = 63 - __builtin_clzl( size );
214     switch ( pv->variant ) {
215     case Byte_index_levels: return ((int)(w / 8)) + 1;
216     case Nibble_index_levels: return ((int)(w / 4)) + 1;
217     case BitPair_index_levels: return ((int)(w / 2)) + 1;
218     case Single_index_level: return 1;
219     }
220     return 0;
221 }
222
223 /** ============================================================ **/
224
225 /**
226  * Advances a Vector index to the next used slot at or below the
227  * given level, starting from the indexed entry (inclusive) and up.
228  * The function will free any empty pages it discovers, and then
229  * update the index slots accordingly. The given index is advanced
230  * cyclically to match the found slot. The function returns a slot
231  * pointer to the used slot, if any, and 0 otherwise.
232  * The last parameter is a flag that gets set when the scanning is
233  * partial (i.e. not the whole index page).
234  */
235 static void **Vector_level_next_used(
236     Vector *pv,
237     VectorPage *page,
238     VectorIndex *index,
239     int level,
240     int *partial )
241 {
242     void **p = (void**)&(*page)[ VECTOR_INDEX_PART( pv, index, level ) ];
243     if ( VECTOR_INDEX_PART( pv, index, level ) != 0 ) {
244         *partial = 1;
245     }
246     for( ; *index < pv->size; p++ ) {
247         if ( *p ) {
248             if ( level == 0 ) {
249                 return p; // This is a used entry
250             }
251             // *p is an index that needs to be inspected recursively
252             int w = 0;
253             void **x = Vector_level_next_used( pv, *p, index, level - 1, &w );
254             if ( x ) {
255                 return x; // Used slot was found; return it.
256             }
257             // If the page *p is all empty, so can/should be reclaimed.
258             if ( w == 0 ) {
259                 free( *p );
260                 *p = 0;
261             } else {
262                 *partial = 1;
263             }
264         } else {
265             if ( level > 0 ) {
266                 VECTOR_INDEX_FIRST( pv, index, level - 1 );
267             }
268         }
269         if ( VECTOR_INDEX_PART_INC( pv, index, level ) == 0 ) {
270             break; // cycling this level => nothing found
271         }
272     }
273     return 0;
274 }
275
276 // Find the next used slot at given index or later. Returns pointer to
277 // the slot. This allows for a reclaim function that may reclaim slot
278 // items on the way to next used slot.
279 void **Vector_next_used(Vector *pv,VectorIndex *index) {
280     if ( pv->entries == 0 || *index >= pv->size ) {
281         *index = pv->size;
282         return 0;
283     }
284     int levels = Vector_levels( pv, pv->size );
285     int partial = 0;
286     do {
287         void **slot = Vector_level_next_used(
288             pv, pv->entries, index, levels - 1, &partial ) ;
289         if ( slot == 0 ) {
290             break; // reached the end of the Vector
291         }
292         if ( *slot ) {
293             return slot;
294         }
295     } while ( ++(*index) < pv->size );
296     if ( partial == 0 ) {
297         free( pv->entries );
298         pv->entries = 0;
299     }
300     *index = pv->size; // reached the end of the Vector
301     return 0;
302 }
303
304 /**
305  * Advances a Vector index to the prior used slot at or below the
306  * given level, starting from the indexed entry (inclusive) and down.
307  * The function will free any empty pages it discovers, and then
308  * update the index slots accordingly. The given index is advanced
309  * cyclically to match the found slot. The function returns a slot
310  * pointer to the used slot, if any, and 0 otherwise.
311  * The last parameter is a flag that gets set when the scanning is
312  * partial (i.e. not the whole index page).
313  */
314 static void **Vector_level_prev_used(
315     Vector *pv,
316     VectorPage *page,
317     VectorIndex *index,
318     int level,
319     int *partial )
320 {
321     void **p = (void**)&(*page)[ VECTOR_INDEX_PART( pv, index, level ) ];
322     if ( VECTOR_INDEX_PART( pv, index, level ) != VECTOR_SLOTS( pv ) - 1 ) {
323         *partial = 1;
324     }
325     do {
326         if ( *p ) {
327             if ( level == 0 ) {
328                 return p; // This is a used entry
329             }
330             // *p is an index that needs to be inspected recursively
331             int w = 0;
332             void **x = Vector_level_prev_used( pv, *p, index, level - 1, &w );
333             if ( x ) {
334                 return x; // Used slot was found; return it.
335             }
336             // If the page *p is all empty, it can/should be reclaimed.
337             if ( w == 0 ) {
338                 free( *p );
339                 *p = 0;
340             } else {
341                 *partial = 1;
342             }
343         } else {
344             if ( level > 0 ) {
345                 VECTOR_INDEX_LAST( pv, index, level );
346             }
347         }
348         p--;
349     } while ( VECTOR_INDEX_PART_DEC( pv, index, level ) != 0 );
350     return 0;
351 }
352
353 // Find the next used slot at given index or later. Returns pointer to
354 // the slot. This allows for a reclaim function that may reclaim slot
355 // items on the way to next used slot.
356 void **Vector_prev_used(Vector *pv,VectorIndex *index) {
357     if ( pv->entries == 0 || *index >= pv->size ) {
358         *index = pv->size;
359         return 0;
360     }
361     int levels = Vector_levels( pv, pv->size );
362     int partial = 0;
363     do {
364         void **slot = Vector_level_prev_used(
365             pv, pv->entries, index, levels - 1, &partial ) ;
366         if ( slot == 0 ) {
367             break; // reached the end of the Vector
368         }
369         if ( *slot ) {
370             return slot;
371         }
372     } while ( (*index)-- != 0 );
373     if ( partial == 0 ) {
374         free( pv->entries );
375         pv->entries = 0;
376     }
377     *index = pv->size; // reached the end of the Vector
378     return 0;
379 }
380
381 // Reclaim tree of unused pages for a given level
382 static void Vector_reclaim(Vector *pv,VectorPage *page,unsigned int level) {
383     unsigned long i = 0;
384     if ( level > 0 ) {
385         for ( ; i < VECTOR_SLOTS( pv ); i++ ) {
386             if ( (*page)[i] ) {
387                 Vector_reclaim( pv, (VectorPage *) (*page)[i], level - 1 );
388             }
389         }
390     }
391     free( page );
392 }
393
394 // Resize Vector, using the reclaim function as needed, to handle any
395 // excess items or to veto the resize. Returns the index of the veto,
396 // if any, or <0 otherwise, with -1 indicating success and -2
397 // indicating OOM
398 //
399 // Note that resizing may result in the introduction/removal of
400 // indexing levels and pages, so as to keep the leveling accurate for
401 // the size.
402 int Vector_resize(
403     Vector *pv,VectorIndex new_size,
404     int (*reclaim)(Vector *pv,VectorIndex index,void *item,void *data),
405     void *data )
406 {
407     struct {
408         int old;
409         int new;
410     } level = {
411         Vector_levels( pv, pv->size ),
412         Vector_levels( pv, new_size )
413     };
414     VectorPage *entries = 0;
415     if ( pv->entries == 0 ) {
416         pv->size = new_size;
417         return 0;
418     }
419     // A shrinking Vector might be veto-ed
420     if ( new_size < pv->size ) {
421         VectorIndex index = new_size;
422         void **slot = Vector_next_used( pv, &index );
423         while ( slot ) {
424             if ( *slot && reclaim && reclaim( pv, index, *slot, data ) == 0 ) {
425                 *slot = 0;
426                 slot = Vector_next_used( pv, &index );
427                 continue;
428             }
429             return index;
430         }
431         // At this point we know that there are no slots used after
432         // the new_size size, so now it's time to remove and reclaim
433         // any superflouous top level pages.
434         if ( pv->variant == Single_index_level ) {
435             // Follow Vector size using realloc
436             if ( new_size > 0 ) {
437                 entries = Vector_single_realloc( pv, new_size );
438                 if ( entries == 0 ) {
439                     return -2; // OOM
440                 }
441             }
442             pv->entries = entries;
443         } else {
444             VectorPage **pp = &pv->entries;
445             int i = level.old;
446             while ( i-- > level.new ) {
447                 if ( pp ) {
448                     pp = (VectorPage **)(*pp);
449                 }
450             }
451             if ( pp != &pv->entries ) {
452                 entries = pv->entries;
453                 if ( pp ) {
454                     pv->entries = *pp;
455                     *pp = 0; // Detach subtree
456                 } else {
457                     pv->entries = 0;
458                 }
459                 Vector_reclaim( pv, entries, level.old - 1 );
460             }
461             if ( new_size == 0 && pv->entries ) {
462                 free( pv->entries );
463                 pv->entries = 0;
464             }
465         }
466     } else {
467         // Vector is growing. Maybe insert levels.
468         if ( pv->variant == Single_index_level ) {
469             // Follow Vector size using realloc
470             entries = Vector_single_realloc( pv, new_size );
471             if ( entries == 0 ) {
472                 return -2; // OOM
473             }
474             pv->entries = entries;
475             memset( &(*entries)[ pv->size ], 0,
476                     ( new_size - pv->size ) * sizeof( void* ) );
477         } else {
478             for ( ; level.old < level.new; level.old++ ) {
479                 VectorPage *p = Vector_indexpage_calloc( pv );
480                 if ( p == 0 ) {
481                     return -2; // OOM
482                 }
483                 (*p)[0] = pv->entries;
484                 pv->entries = p;
485                 // Should maybe change the size to match the level?
486                 // otherwise recovery from OOM is impossible
487             }
488         }
489     }
490     pv->size = new_size;
491     return -1;
492 }
493
494 // Return pointer to the indexed page slot at the requested level, and
495 // adding intermediate index pages if so requested. Returns 0 if
496 // addition fails (OOM), or if not requested and page is missing.
497 void **Vector_access(Vector *pv,VectorIndex index,int level,int add) {
498     if ( index >= pv->size ) {
499         return 0;
500     }
501     void **page = (void**) &pv->entries;
502     int i = Vector_levels( pv, pv->size );
503     while (  i-- > level ) {
504         if ( add && (*page) == 0 ) {
505             (*page) = Vector_indexpage_calloc( pv );
506         }
507         page = (*page);
508         if ( page == 0 ) {
509             return 0;
510         }
511         page += VECTOR_INDEX_PART( pv, &index, i );
512     }
513     return page;
514 }
515
516 // Map index into a value slot
517 void **Vector_entry(Vector *pv,VectorIndex index) {
518     return Vector_access( pv, index, 0, 1 );
519 }
520
521 inline void Vector_set(Vector *pv,VectorIndex index,void *value) {
522     void **p = Vector_entry( pv, index );
523     *p = value;
524 }
525
526 // Set value at index but return the old value
527 void *Vector_get_set(Vector *pv,VectorIndex index,void *value) {
528     void **p = Vector_entry( pv, index );
529     void *old = *p;
530     *p = value;
531     return old;
532 }
533
534 inline void *Vector_get(Vector *pv,VectorIndex index) {
535     void **p = Vector_entry( pv, index );
536     return *p;
537 }
538
539 int Vector_free_any(Vector *pv,VectorIndex ix,void *item,void *data) {
540     (void)pv; (void)ix; (void)data;
541     free( item );
542     return 0;
543 }
544
545 int Vector_clear_any(Vector *pv,VectorIndex ix,void *item,void *data) {
546     (void)pv; (void)ix; (void)item; (void)data;
547     return 0;
548 }
549
550 void Vector_append(Vector *pv,void *value) {
551     Vector_resize( pv, pv->size + 1, 0, 0 );
552     Vector_set( pv, pv->size - 1, value );
553 }
554
555 // copy block of n items from src[si] to dst[di]
556 // no efficiency hacks
557 void Vector_copy(Vector *dst,VectorIndex di,
558               Vector *src,VectorIndex si,VectorIndex n) {
559     if ( dst != src || di < si ) {
560         while ( n-- != 0 ) {
561             Vector_set( dst, di++, Vector_get( src, si++ ) );
562         }
563     } else if ( di > si ){
564         di += n - 1;
565         si += n - 1;
566         while ( n-- != 0 ) {
567             Vector_set( dst, di--, Vector_get( src, si-- ) );
568         }
569     }
570 }
571
572 Vector *Vector_clone(enum VectorVariant variant,Vector *src) {
573     Vector *dst = (Vector*) malloc( sizeof( Vector ) );
574     (*dst) = (Vector) { .variant = variant, .size = 0, .entries = 0 };
575     Vector_resize( dst, src->size, 0, 0 );
576     Vector_copy( dst, 0, src, 0, src->size );
577     return dst;
578 }
579
580 void Vector_dump(Vector *pv,
581                  void (*itemdump)(const VectorIndex,const void *)) {
582     VectorIndex index = 0;
583     for ( ; index < pv->size; index++ ) {
584         void **slot = Vector_next_used( pv, &index );
585         if ( slot == 0 ) {
586             break;
587         }
588         itemdump( index, *slot );
589     }
590 }
591
592 //// Quicksort
593
594 // Returns 1 for "in order", 0 for equal, and -1 for "wrong order"
595 typedef int (*ComparFn)(const void *,const void *);
596
597 static void Vector_qsort_part(
598     Vector *pv,ComparFn compar,
599     VectorIndex low,VectorIndex high)
600 {
601     if ( low >= high ) {
602         return;
603     }
604     VectorIndex lo = low;
605     VectorIndex m = high - 1;
606     
607     if ( lo >= m  ) {
608         return;
609     }
610
611     VectorIndex hi = m - 1;
612     void **mp = Vector_entry( pv, m );
613     void **lop = 0;
614     void **hip = 0;
615     for ( ;; ) {
616         // Find index of first item "above" mp scanning from lo and up
617         for ( ; lo < m; lo++ ) {
618             lop = Vector_entry( pv, lo );
619             if ( compar( *lop, *mp ) < 0 ) {
620                 break;
621             }
622         }
623         // if  lo == m, then lop is wrong!!
624         // Find index of first item "below" mp scanning from hi and down
625         for ( ; hi > lo; hi-- ) {
626             hip = Vector_entry( pv, hi );
627             if ( compar( *mp, *hip ) < 0 ) {
628                 break;
629             }
630         }
631         if ( lo >= hi ) {
632             if ( lo < m ) {
633                 void *x = *lop;
634                 *lop = *mp;
635                 *mp = x;
636                 m = lo;
637             }
638             break;
639         }
640         void *x = *lop;
641         *lop = *hip;
642         *hip = x;
643     }
644     Vector_qsort_part( pv, compar, low, m );
645     Vector_qsort_part( pv, compar, m+1, high );
646 }
647
648 void Vector_qsort(Vector *pv,ComparFn compar) {
649     Vector_qsort_part( pv, compar, 0, pv->size );
650 }
651
652 void Vector_iterate(Vector *pv,
653                     VectorIndex start,
654                     int (*itemfn)(VectorIndex,void*,void*),
655                     void *data )
656 {
657     VectorIndex index = start;
658     while ( index < pv->size ) {
659         int end = VECTOR_SLOTS( pv );
660         int i = index & ( end - 1 );
661         for ( ; i < end && index < pv->size; i++, index++ ) {
662             void **slot = Vector_access( pv, index, 0, 0 );
663             if ( itemfn( index, slot? *slot: 0, data ) ) {
664                 return;
665             }
666         }
667     }
668 }
669
670 struct FindData {
671     void *value;
672     VectorIndex index;
673 };
674
675 static int Vector_find_item(VectorIndex index, void *item,void *data) {
676     struct FindData *fd = (struct FindData*)data;
677     if ( fd->value != item ) {
678         return 0;
679     }
680     fd->index = index;
681     return 1;
682 }
683
684 VectorIndex Vector_find(Vector *pv,void *value) {
685     struct FindData fd = {
686         .value = value,
687         .index = pv->size
688     };
689     Vector_iterate( pv, 0, Vector_find_item, &fd );
690     return fd.index;
691 }
692
693 // Find surrounding indexes for a given item key in a sparse Vector
694 void *Vector_bsearch(Vector *pv,VectorIndex *index,const void *key,
695                      int (*compare)(const void *key, const void *item)) {
696     VectorIndex lo = 0;
697     VectorIndex hi = pv->size;
698     if ( hi-- == 0 || Vector_prev_used( pv, &hi ) == 0 ) {
699         return 0;
700     }
701     hi++;
702     while ( lo < hi ) {
703         VectorIndex m = lo + ( hi - lo ) / 2;
704         void **slot = Vector_next_used( pv, &m );
705         int c = compare( key, *slot );
706         if ( c == 0 ) {
707             *index = m;
708             return *slot;
709         }
710         if ( c < 0 ) {
711             hi = m;
712         } else {
713             lo = m + 1;
714         }
715     }
716     return 0;
717 }
718
719 // Iterator callback.
720 static int checkunused(VectorIndex index,void *item,void *data) {
721     VectorIndex *last = (VectorIndex*) data;
722     if ( item == 0 ) {
723         (*last) = index;
724         return 1;
725     }
726     if ( *last > index ) {
727         // Only on the first iteration,  with *last = Vector_sie
728         if ( index == 0 ) {
729             (*last) = 1;
730             return 0;
731         }
732         *last = 0;
733     } else if ( index == (*last) ) {
734         (*last)++;
735         return 0;
736     }
737     return 1;
738 }
739
740 // Scan forward for the next unused Vector slot
741 VectorIndex Vector_next_unused(Vector *pv,VectorIndex index) {
742     VectorIndex unused = Vector_size( pv );
743     Vector_iterate( pv, index, checkunused, &unused );
744     return unused;
745 }