#include "bitutils.h"

#include <iostream>

// // // // // // // // // // // // // // // // // // // // 
//
// Utilities for manipulating bits
//
// In the bit routines that follow the lsb is numbered 0.
//

// assumes that size+1 chars of space is available in space
char *btostr(char *space, unsigned int n, int size)
{
    int i;

    for (i=size-1; i>=0; i--) space[size-i-1] = (n & (1<<i)) ? '1' : '0';
    space[size] = '\0';

    return space;
}	


// WARNING: The space allocated must be deleted by the caller
char *btostr(unsigned int n, int numBits)
{
    unsigned int m;
    char *result, *t;
    
    t = result = new char [numBits+1];

    m = 1<<(numBits-1);
    while(m) {
	*t++ = (n&m) ? '1' : '0';
	m>>=1;
    }
    *t = '\0';

    return result;
}



// WARNING: The space allocated must be deleted by the caller
char *btostr(unsigned int n)
{
    return btostr(n, WORDSIZE);
}


void bitPrint(unsigned int n, int numBits)
{
    char *tmp;

    printf("%s ", tmp = btostr(n, numBits));
    delete [] tmp;
}



// read a binary string out of space up to a non zero or one or size
unsigned int strtob(char *space, int size)
{
    int n;
    
    n = 0;
    for (int i=0; i<size; i++) {
	if (*space=='0') {
	    n<<=1;
	}
	else if (*space=='1') {
	    n<<=1;
	    n++;
	}
	else break;

	space++;
    }

    return n;
}	


// read a binary string out of space up to a non zero or one or size
void strtob(char *space, int numBits, unsigned int *n)
{
    int maxElem;

    maxElem = (numBits-1)>>WORDBITSIZE;

    n[maxElem] = strtob(space, ((numBits-1)&WORDSIZEMASK)+1);
    space+=((numBits-1)&WORDSIZEMASK)+1;

    for (int i=maxElem-1; i>=0; i--) {
        n[i] = strtob(space, WORDSIZE);
        space+=WORDSIZE;
    }
}



unsigned int bin(char *s)
{
    unsigned int result;
    
    result = 0;
    while (*s) {
        result = (result<<1) + (*s++ - '0');
    }
    return result;
}



// return size number of 1's in the lsb positions
// a shift on WORDSIZE on a machine is undefined
unsigned int ones(int size)
{
    return ONES(size);
}    



// Make a mask of a contiguous set of bits between positions a and b.
// In this version: rangeMask(a, b) == rangeMask(b, a)
// Note that >>WORDSIZE may not do anything (belive it or not)
// and so must be special cased.
int rangeMask(const int a, const int b)
{
    int size;

    if (a>b) {
	size = a-b+1;
	return (ALLONES>>(WORDSIZE-size))<<b;
    }
    else {
	size = b-a+1;
	return (ALLONES>>(WORDSIZE-size))<<a;
    }
}


// Make a mask of a contiguous set of bits that starts at position
// a and procedes to more significant bits wrapping around if needed
// to get to position b.
// In this version: rangeMask(a, b) != rangeMask(b, a)
// Note that >>WORDSIZE may not do anything (belive it or not)
// and so must be special cased.
int rangeMaskWrap(const int a, const int b)
{
    int size;

    if (a>b) {
	size = a-b-1;
	if (size) return ~((ALLONES>>(WORDSIZE-size))<<(b+1));
	return ALLONES;
    }
    else {
	size = b-a+1;
	return (ALLONES>>(WORDSIZE-size))<<a;
    }
}


// produces a mask that represents the k-th bit place
// in counting in binary.  k=0 -> 10101010  k=1 -> 11001100 etc
void binaryMask(int k, unsigned int *n, int numBits)
{
    unsigned int m;

    clearAll(n, numBits);
    m = 1<<k;
    for (unsigned int i=0; i<numBits; i++) {
	if (i&m) setBit(n, i);
    }
}



// take the one's compliment in just in size number of bits (discard rest)
unsigned int bitInv(unsigned int n, int size)
{
    return (~n) & ONES(size);
}



// rotate left the WORDSIZE bit unsigned int
unsigned int bitRotateLeft(unsigned int n, int amount)
{
    return (n<<amount) | (n>>(WORDSIZE - amount));
}


//rotate left just the size number of bits (rest discarded)
unsigned int bitRotateLeft(unsigned int n, int size, int amount)
{
    return (ONES(size) & (n<<amount)) | (n>>(size - amount));
}


//rotate right just the size number of bits (rest discarded)
unsigned int bitRotateRight(unsigned int n, int size, int amount)
{
    return (ONES(size) & (n<<(size - amount))) | (n>>amount);
}



// reverses the order of the bits in the string
// NOTE: any bits not in the first size bits are zeroed
unsigned int bitReverse(unsigned int n, int size)
{
    int i;
    unsigned int b, result;

    result=0;
    for (i=0; i<size; i++) {
	b = n&0x1;
	n>>=1;
	result<<=1;
	result|=b;
    }
    return result;
}


// extract bit s thru f (f>=s)
unsigned int bitExtract(unsigned int n, int s, int f)
{
    if (s>f) {
	fprintf(stderr, "ERROR(bitExtract): start:%d > finish:%d\n", s, f);
	fflush(stderr);
    }
 
    f = WORDSIZEM1-f;
    return (n<<f)>>(f+s);
}




// NOTE: this insert routine returns the insertion the bitarray version
// actually modifies the array
unsigned int bitInsert(unsigned int n, int s, int f, unsigned int toInsert)
{
    unsigned int result;
    unsigned int mask;
    unsigned int old2I;

    old2I = toInsert;
    mask = ones(f-s+1);
    toInsert = (toInsert & mask) << s;
    mask <<= s;
    result = (n & ~mask) | toInsert;

    if ((result & mask) != toInsert) {
	printf("result %d != %d   n:%d  old2I: %d\n", result, toInsert, n, old2I);
    }

    return result;
}


    
// tests if n is a single bit.  Notably, 0 is not a power of 2.
BOOLEAN isPowerOfTwo(unsigned int n)
{
    return n ? !(n & (n-1)) : 0;
}



/*
// this bit count works will if few bits set
int bitCount(unsigned int n)
{
    int c;

    c = 0;
    while(n>0) {
	n &= (n-1);
	c++;
    }

    return c;
}
*/


// this bit count works will if lots of bits set (perhaps > 6 bits)
int bitCount(unsigned int w)
{
   w = (0x55555555 & w) + (0x55555555 & (w>> 1));
   w = (0x33333333 & w) + (0x33333333 & (w>> 2));
   w += (w>> 4);
   w &= 0x0f0f0f0f;
   w += (w>> 8);
   w += (w>>16);
   w &= 0x000000ff;
   return w;
}



/*
// What is the parity of the bits in n
int bitParity(unsigned int n)
{
    int c;

    c = 0;
    while(n>0) {
	n &= (n-1);
	c ^= 1;
    }

    return c;
}
*/

// works well for larger numbers of bits set (>3 bits?)
int bitParity(unsigned int w)
{
   w ^= w>>1;
   w ^= w>>2;
   w ^= w>>4;
   w ^= w>>8;
   w ^= w>>16;

   return w & 1;
}



// Hamming Distance
int bitHamming(unsigned int a, unsigned int b)
{
    return bitCount(a ^ b);
}


// randomly select a bit from the set bits in n
unsigned int selectRandBit(unsigned int n)
{
    unsigned int m;

    while (1) {
	m = fastRand() & n;
	if (m) {
	    if ((m & (m-1))==0) return m;
	    n = m;
	}
    }
}

// returns the index number of the left most bit
int leftMostBitIndex(unsigned int n)
{
    int l;

    if (n==0) return -1;

    if ((n & 0xffffff00) == 0) l = 7;
    else if ((n & 0xfffff000) == 0) l = 11;
    else if ((n & 0xffff0000) == 0) l = 15;
    else if ((n & 0xff000000) == 0) l = 23;
    else l = 31;

    while (!((1<<l) & n)) l--;

    return l;
}



// returns the bit in the right most bit position
unsigned int rightMostBit(unsigned int n)
{
    return n & (n ^ (n-1)); 
}


// returns the bit of the right most zero bit position
// for 101011 returns 000100
unsigned int rightMostZero(unsigned int n)
{
    return ~n & (n ^ (n+1)); 
}


// counts the number of zeros on the right of the first 1 bit
int rightMostBitIndex(unsigned int n)
{
    int r;

    if (n==0) return -1;

    if ((n & 0xff) != 0) r = 0;
    else if ((n & 0xffff) != 0) r = 7;
    else if ((n & 0xffffff) != 0) r = 15;
    else r = 23;
    
    while (!((1<<r) & n)) r++;

    return r;
}



// the number of bit positions wide the nonzero bit segment is
int bitWidth(unsigned int n)
{
    int l, r;

    if (n==0) return 0;

    if ((n & 0xffffff00) == 0) l = 7;
    else if ((n & 0xfffff000) == 0) l = 11;
    else if ((n & 0xffff0000) == 0) l = 15;
    else if ((n & 0xff000000) == 0) l = 23;
    else l = 31;

    while (!((1<<l) & n)) l--;

    r=0;
    while (!((1<<r) & n)) r++;

    return l-r+1;
}



// The Binary Reflected Gray Code of a number
unsigned int bitGray(unsigned int n)
{
    return (n^(n>>1));
}



// The inverse Binary Reflected Gray Code of a number
/*
unsigned int bitDegray(unsigned int n)
{
    unsigned int result;

    result = 0;
    while (n) {
	result ^= n;
	n >>= 1;
    }

    return result;
}
*/


// A faster version of inverse Binary Reflected Gray Code
unsigned int bitDegray(unsigned int n)
{
    n ^= (n>>1);
    n ^= (n>>2);
    n ^= (n>>4);
    n ^= (n>>8);
    n ^= (n>>16);

    return n;
}



// This assumes that all bits in odd positions go to x and even to y
// So the bits are organized xyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxy
// The result is x and y are 16 bit numbers
void bitUninterleave(unsigned int n, unsigned int &x, unsigned int &y)
{
    int s;

    y = n & 0x1;
    n >>= 1;
    x = n & 0x1;
    n >>= 1;
    s = 1;
    while (n) {
	y |= (n&0x1)<<s;
	n >>= 1;
	x |= (n&0x1)<<s;
	n >>= 1;
	s++;
    }
}


// This routine increments the number in the masked portion.
// The number resides only in the masked portion of n.
// m is the mask.
unsigned int incInMask(unsigned int &n, unsigned int m)
{
    unsigned int t;

    while (m) {
	t = m ^ (m-1);
	t ^= (t>>1);
	n ^= t;
	if (n&t) break;
	m ^= t;
    }
    return n;
}



// returns the smallest block of 1s (i.e. 2^k-1) 
// such that n is contained in the block
unsigned int bitContainingBlock(unsigned int n)
{
    int s;

    s = 1;
    while ((n+1) & n) {  // while n+1 not a power of two
	n |= (n>>s);
	s<<=1;
    }

    return n;
}


// algorithm is adapted from Knuth v. 2 2nd ed p137 Algorithm S
// Select a random mask with numBits set from the first width bits.
// This is a general choose k elements from a set.
//
// Too many calls to rand number generator and set bit
//  REALLY SLOW
void randBitMask(unsigned int *n, int width, int numBitsSet)
{
    int numSelected, b;
    int bits;

    b = 0;
    if (numBitsSet > width-numBitsSet) {
	numBitsSet = width-numBitsSet;

	for (bits=WORDSIZE; bits<=width; bits+=WORDSIZE) {
	    n[b++] = ALLONES;
	}
	if (width&WORDSIZEMASK) {
	    n[b] = ONES(width&WORDSIZEMASK);
	}
    }
    else {
	for (bits=0; bits<width; bits+=WORDSIZE) {
	    n[b++] = 0;
	}
    }

    numSelected = 0;
    for (b=0; b<width; b++) {
	if ((width-b)*fastUnitRand() < (numBitsSet - numSelected)) {
	    toggleBit(n, b);
	    numSelected++;
	    if (numSelected>=numBitsSet) break;
	}
    }
}



unsigned int randBitMask(unsigned int width, unsigned int numBitsSet)
{
    int avail, invert;
    unsigned int r, x, mask;

    avail = invert = mask = 0;

    if (numBitsSet>width/2) {
	numBitsSet = width-numBitsSet;
	invert = 1;
    }

    while (numBitsSet) {
	// one call to intRandom gets 6 5 bit numbers
	if (avail==0) {
	    r = fastRand();  
	    avail = 6;
	}
	// does the bit loc fit in the width?
	if ((r & B11111) <width) {
	    x = 1<<(r & B11111);

	    // if acceptable and not used yet then
	    if (!(x & mask)) {
		mask |= x;
		numBitsSet--;
	    }
	}
	r >>= 5;
	avail--;
    }

    if (invert) mask = ~mask & ONES(width);

    return mask;
}





// Select a random mask within the first width bits.
unsigned int randBitMask(unsigned int width)
{
    return fastMaskRand(ONES(width));
}




unsigned int randInMask(unsigned int mask)
{
    return fastRand()&mask;
}


// create a random block of 1s in a word
// e.g.  00000000111111100000000000000000
// short blocks occur more frequently than long blocks
unsigned int randBlock()
{
    int a, b;

    a=fastMaskRand(WORDSIZEMASK);
    while ((b=fastMaskRand(WORDSIZEMASK))==a){};  // can't be equal

    if (a) {
	if (b) {
	    return (ALLONES>>(WORDSIZE-a)) ^ (ALLONES>>(WORDSIZE-b));
	}
	// b==0 here  (note a!=0 here)
	return (ALLONES>>(WORDSIZE-a));
    }

    // a==0 here  (note b!=0 here)
    return (ALLONES>>(WORDSIZE-b));
}    
 


void randSwapABlock(unsigned int &m, unsigned int &n)
{
    unsigned int mask;

    mask = randBlock();
    m ^= (n & mask);
    n ^= (m & mask);
    m ^= (n & mask);
}




// create a random block of 1s in a word
// e.g.  00000000111111100000000000000000
// short blocks occur more frequently than long blocks
unsigned int randBlock(int numBits)
{
    int a, b;

    a=fastModRand(numBits);
    while ((b=fastModRand(numBits))==a){};  // can't be equal

    if (a) {
	if (b) {
	    return (ALLONES>>(WORDSIZE-a)) ^ (ALLONES>>(WORDSIZE-b));
	}
	// b==0 here  (note a!=0 here)
	return (ALLONES>>(WORDSIZE-a));
    }

    // a==0 here  (note b!=0 here)
    return (ALLONES>>(WORDSIZE-b));
}    
 


void randSwapABlock(unsigned int &m, unsigned int &n, int numBits)
{
    unsigned int mask;

    mask = randBlock(numBits);
    m ^= (n & mask);
    n ^= (m & mask);
    m ^= (n & mask);
}





// NOTE: returns the mutated bit string
unsigned int bitMutate(unsigned int n, int numBits, double mProb)
{
    int i;
    unsigned int m;
    
    m = 1;
    for (i=0; i<numBits; i++) {
	if (fastUnitRand()<mProb) n^=m;
	m<<=1;
    }

    return n;
}


// output is bits in n that match the mask m compacted and right justified
// n - number to pack
// m - is the mask
unsigned int pack(unsigned int n, unsigned int m)
{
    unsigned int b, r, t;

    b = 1;
    r = 0;
    while (m) {
	t = m ^ (m-1);
	t ^= (t>>1);         // access the lowest bit in the mask
	m ^= t;              // remove it
	r |= (n&t) ? b : 0;  // use it as a mask to get bit from number
	b <<= 1;
    }

    return r;
}


// output is bits in n that match the mask m compacted and right justified
// n - number to pack
// m - is the mask WITH exactly 3 bits set
unsigned int pack3(unsigned int n, unsigned int m)
{
    unsigned int r, t;

    r = 0;

    t = m ^ (m-1);      
    t ^= (t>>1);         // access the lowest bit in the mask
    m ^= t;              // remove it
    r |= (n&t) ? 1 : 0;  // use it as a mask to get bit from number

    t = m ^ (m-1);      
    t ^= (t>>1);         // access the lowest bit in the mask
    m ^= t;              // remove it
    r |= (n&t) ? 2 : 0;  // use it as a mask to get bit from number

    t = m ^ (m-1);      
    t ^= (t>>1);         // access the lowest bit in the mask
    m ^= t;              // remove it
    return r | (n&t) ? 4 : 0;  // use it as a mask to get bit from number
}


// output is bits in n placed in the bit positions marked by the mask m 
// n - number to pack
// m - is the mask
unsigned int unpack(unsigned int n, unsigned int m)
{
    unsigned int r, t;

    r = 0;
    while (m) {
	t = m ^ (m-1);
	t ^= (t>>1);      // t is the lowest 1 bit in m
	m ^= t;
	r |= (n&1) ? t : 0;
	n >>= 1;
    }

    return r;
}


/* experimental
int pack2(int n, int m)
{
    int b, r, t, mm;

    b = 0;
    r = 0;
    while (m) {
	t = m ^ (m-1);
	t ^= (t>>1);
	mm = ((m+t)^m)&m;
	printf("m:%d  t:%d  mm:%d  r:%d\n", m, t, mm, r);
	fflush(stdout);
	m ^= mm;
	r |= (n&mm)<<b;
	while (mm) {
	    printf("mm: %d\n", mm);
	    b++;
	    mm &= mm-1;
	}
    }

    return r;
}
*/


// // // // // // // // // // // // // // // // // // // // 
//
// tools for creating SUBSET MASKS WITH A FIXED NUMBER
// OF ONE BITS.  The firstCombSubset determines the
// initial number of ones in the subset.  lastCombSubset
// determines the full width of the set that the subset
// is a part of.  Inc moves from one subset to the next
// in increasing binary order.  These routines are meant to
// be used in a for loop.
//
unsigned int firstCombSubset(int subsetSize)
{
     return ONES(subsetSize);
}


unsigned int lastCombSubset(int bitSize, int subsetSize) 
{
     return ONES(subsetSize) << (bitSize - subsetSize);
}




// WARNING: this function does not check if you have run out of some
// set bitSize of bits.  To limit the combSubset frunction to 10 bits
// you have to play a trick:
// 
// This will pick all 3 bit combinations in a 10 bit string
// unsigned int outOfBounds = ~(ONES(10));
// for (mask=firstCombSubset(3); 
//      outOfBounds&mask;
//      incCombSubset(mask)) {
// 
// To set the limit at 32 bits you have to do the following since
// there is no overflow:
//
// This will pick all 3 bit combinations in a 32 bit string
// int notDone = 1;
// for (mask=firstCombSubset(3); 
//      notDone;
//      notDone=incCombSubset(mask)) {
//
BOOLEAN incCombSubset(unsigned int &mask)
{
    unsigned int x, z;

    if (mask==0) return FALSE;
    if (mask & 0x1) {
        x = ~mask;     // prepare to look for the least sig 01
	x ^= (x-1);    // find loc of right most 0 
	x ^= (x>>2);   // make mask for the 01
	mask ^= x;     // swap them 
    }
    else {
        x = mask ^ (mask-1);  // for the least sig 10
	x = x ^ (x>>1);       // find the least sig 1.  mask for it in x

	if (x > x+mask) return FALSE;  // test for WORDSIZE overflow
	x = mask + x;         // add the 1 so the least block of 1s -> 10s

	z = mask ^ x;         // find out how many ones you need in lsb
	while (!(z&0x1)) {
	    z >>= 1;
	}
	z >>= 2;              // block found by xor above has 2 too many bits

	mask = x | z;         // add block to modified mask that lost 1s
    }

    return TRUE;
}




// // // // // // // // // // // // // // // // // // // // // // // // 
//
// Utilities for very long bit strings represented as arrays of unsigned ints
//

// return the string encoded as if it were in base64 for compactness
// char *bto64CharTable = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz@.";  // this is the base64 encoding
char *bto64CharTable = "0abcdefghijklmnopqrstuvwxyz789@ABCDEFGHIJKLMNOPQRSTUVWXYZ456/123";
char *btob64(unsigned int *n, int numBits)
{
    char *space;
    int numchars, c, s, f;

    numchars = (numBits+5)/6;
    space = new char [numchars+1];
    
    for (c=0; c<numchars; c++) {
	s = c*6;
	f = s+5;
	if (f>numBits) f = numBits;
	space[numchars-1-c] = bto64CharTable[bitExtract(n, s, f)];
    }
    space[numchars] = '\0';

    return space;
}	


// a compressed way to view a string of bits in visible characters
// by translating to base 64
void b64tob(unsigned int *n, int numBits, char *b64)
{
    int numWords, numchars;
    int c, i, s, f;

    numWords = (numBits>>WORDBITSIZE)+1;
    numchars = strlen(b64);
    for (c=0; c<numchars; c++) {
	for (i=0; i<64; i++) if (b64[c]==bto64CharTable[i]) break;
	s = ((numchars-1)-c)*6;
	f = s+5;
	bitInsert(n, s, f, i);
    }

    return;
}



char *btostr(char *space, unsigned int *n, int numBits)
{
    int i;
    char *t;
    int maxElem;

    t = space;
    maxElem = (numBits-1)>>WORDBITSIZE;

    btostr(t, n[maxElem], ((numBits-1)&WORDSIZEMASK)+1);
    t+=((numBits-1)&WORDSIZEMASK)+1;

    for (i=maxElem-1; i>=0; i--) {
	btostr(t, n[i], WORDSIZE);
	t+=WORDSIZE;
    }

    return space;
}	




// WARNING: The space must be deleted by the caller
char *btostr(unsigned int *n, int numBits)
{
    char *result;

    result = new char [numBits+1];
    return btostr(result, n, numBits);
}



void bitPrint(unsigned int *n, int numBits)
{
    char *tmp;

    printf("%s ", tmp = btostr(n, numBits));
    delete [] tmp;
}




// WARNING: assumes last int is padded with zeros if necessary
int bitCount(unsigned int *n, int numBits)
{
    int i, bits;
    int sum;

    i=0;
    sum=0;
    for (bits=0; bits<numBits; bits+=WORDSIZE) {
	sum += bitCount(n[i]);
	i++;
    }

    return sum;
}



// // // // // // // // // // // // // // // // // // // // // // // // 



void clearAll(unsigned int *n, int numBits)
{
    int i, bits;

    i=0;
    for (bits=0; bits<numBits; bits+=WORDSIZE) {
	n[i++] = 0;
    }
}


// this will zero fill the rest of the word
void setAll(unsigned int *n, int numBits)
{
    int i, bits;

    i=0;
    for (bits=32; bits<=numBits; bits+=WORDSIZE) {
	n[i++] = ALLONES;
    }
    if (numBits&WORDSIZEMASK) {
	n[i] = ONES(numBits&WORDSIZEMASK);
    }
}



// n = ~n
// this will zero fill the rest of the word
void bitInv(unsigned int *n, int numBits)
{
    int i, bits;

    i=0;
    for (bits=WORDSIZE; bits<=numBits; bits+=WORDSIZE) {
	n[i] = ~n[i];
	i++;
    }
    if (numBits&WORDSIZEMASK) {
	n[i] = ~n[i] & ONES(numBits&WORDSIZEMASK);
    }
}


// Select a random mask within the first width bits.
void randBitMask(unsigned int *n, int numBits)
{
    int i, bits;

    i=0;
    for (bits=WORDSIZE; bits<=numBits; bits+=WORDSIZE) {
	n[i] = fastRand();
	i++;
    }
    if (numBits&WORDSIZEMASK) {
	n[i] = fastRand() & ONES(numBits&WORDSIZEMASK);
    }
}



// swap the bits indicated by the mask
void swapBits(unsigned int &m, unsigned int &n, unsigned int mask)
{
    m ^= (n & mask);
    n ^= (m & mask);
    m ^= (n & mask);
}


// swap the bits indicated by the mask
// do this with pointers and tmps
void swapBits(unsigned int *m, unsigned int *n, unsigned int *mask, int numBits)
{
    int i, bits;

    i=0;
    for (bits=WORDSIZE; bits<=numBits; bits+=WORDSIZE) {
	m[i] ^= (n[i] & mask[i]);
	n[i] ^= (m[i] & mask[i]);
	m[i] ^= (n[i] & mask[i]);
	i++;
    }

    // probably don't need the ones stuff
    if (numBits&WORDSIZEMASK) {
	m[i] ^= (n[i] & mask[i]);
	n[i] ^= (m[i] & mask[i]) & ONES(numBits&WORDSIZEMASK);
	m[i] ^= (n[i] & mask[i]) & ONES(numBits&WORDSIZEMASK);
    }
}


// n = m
void bitCopy(unsigned int *n, unsigned int *m, int numBits)
{
    int i, bits;

    // use memcpy here
    i=0;
    for (bits=WORDSIZE; bits<=numBits; bits+=WORDSIZE) {
	n[i] = m[i];
	i++;
    }
    if (numBits&WORDSIZEMASK) {
	n[i] = m[i] & ONES(numBits&WORDSIZEMASK);
    }
}


// n = n | m
unsigned int *bitOr(unsigned int *n, unsigned int *m, int numBits)
{
    int i, bits;

    i=0;
    for (bits=WORDSIZE; bits<=numBits; bits+=WORDSIZE) {
	n[i] |= m[i];
	i++;
    }
    if (numBits&WORDSIZEMASK) {
	n[i] = (n[i]|m[i]) & ONES(numBits&WORDSIZEMASK);
    }

    return n;
}


// n = n & m
unsigned int *bitAnd(unsigned int *n, unsigned int *m, int numBits)
{
    int i, bits;

    i=0;
    for (bits=WORDSIZE; bits<=numBits; bits+=WORDSIZE) {
	n[i] &= m[i];
	i++;
    }
    if (numBits&WORDSIZEMASK) {
	n[i] = (n[i]&m[i]) & ONES(numBits&WORDSIZEMASK);
    }

    return n;
}


// n = n ^ m
unsigned int *bitXor(unsigned int *n, unsigned int *m, int numBits)
{
    int i, bits;

    i=0;
    for (bits=WORDSIZE; bits<=numBits; bits+=WORDSIZE) {
	n[i] ^= m[i];
	i++;
    }
    if (numBits&WORDSIZEMASK) {
	n[i] = (n[i]^m[i]) & ONES(numBits&WORDSIZEMASK);
    }

    return n;
}



// n = compare(n, m)  returns <0 if n<m, 0 if n=m, >0 if n>m
int bitCmp(unsigned int *n, unsigned int *m, int numBits)
{
    int i, bits;

    i=0;
    for (bits=WORDSIZE; bits<=numBits; bits+=WORDSIZE) {
	// remember it is dangerous to do math with unsigned ints
	if (n[i]>m[i]) return 1;
	if (n[i]<m[i]) return -1;
	i++;
    }

    // less that 32 bits remain
    if (numBits&WORDSIZEMASK) {
	return (n[i]&ONES(numBits&WORDSIZEMASK))-(m[i]&ONES(numBits&WORDSIZEMASK));
    }

    return 0;
}



// is bitstring n in bitstring m  (1 is true, 0 is false)
int bitIsIn(unsigned int *n, unsigned int *m, int numBits)
{
    int i, bits;

    i=0;
    for (bits=WORDSIZE; bits<=numBits; bits+=WORDSIZE) {
	if (n[i]&~m[i]) return 0;
	i++;
    }
    if (numBits&WORDSIZEMASK) {
	if ((n[i]&~m[i])&ONES(numBits&WORDSIZEMASK)) return 0;
    }
    return 1;
}


// n = compare(n, m)  returns <0 if n<m, 0 if n=m, >0 if n>m
int bitHamming(unsigned int *n, unsigned int *m, int numBits)
{
    int i, bits, count;

    count = i=0;
    for (bits=WORDSIZE; bits<=numBits; bits+=WORDSIZE) {
	count += bitCount(n[i]^m[i]);
	i++;
    }
    if (numBits&WORDSIZEMASK) {
	count += bitCount((n[i]^m[i])&ONES(numBits&WORDSIZEMASK));
    }

    return count;
}


// NOTE: mutates the string in place
void bitMutate(unsigned int *n, int numBits, double mProb)
{
    int i, bits;
    int s;

//RH remove s when working
    i=0;
    for (bits=0; bits<numBits; bits+=WORDSIZE) {
	s = (bits+WORDSIZE > numBits) ?  (numBits%WORDSIZE) : WORDSIZE;
	n[i] = bitMutate(n[i], s, mProb);
	i++;
    }
}


// sets the numth bit
void setBit(unsigned int *n, int num)
{
    int w;

    w = num>>WORDBITSIZE;
    n[w] |= 1<<(num & WORDSIZEMASK);
}


unsigned int testAndSetBit(unsigned int *n, int numBits)
{
    int w;
    unsigned int bitmask;
    unsigned int result;

    w = numBits>>WORDBITSIZE;
    bitmask = 1<<(numBits & WORDSIZEMASK);
    result = (n[w] & bitmask) ? 1 : 0;
    n[w] |= bitmask;

    return result;
}


unsigned int getBit(unsigned int *n, int numBits)
{
    int w;

    w = numBits>>WORDBITSIZE;
    return (n[w] & (1<<(numBits & WORDSIZEMASK))) ? 1 : 0;
}


void toggleBit(unsigned int *n, int numBits)
{
    int w;

    w = numBits>>WORDBITSIZE;
    n[w] ^= 1<<(numBits & WORDSIZEMASK);
}

void clearBit(unsigned int *n, int numBits)
{
    int w;

    w = numBits>>WORDBITSIZE;
    n[w] &= ~(1<<(numBits & WORDSIZEMASK));
}


// output is bits in n that match the mask m compacted and right justified
// n - number to pack
// m - is the mask
// note that no more than 32 bits can be set in the mask
unsigned int pack(unsigned int *n, unsigned int *m, int numWords)
{
    int i;
    unsigned int r, loc;

    r = 0;
    loc = 0;
    for (i=0; i<numWords; i++) {
	if (m[i]) {
	    r |= (pack(n[i], m[i]) << loc);
	    loc += bitCount(m[i]);
	}
    }

    return r;
}


// The numBits is only used to determine the number
// of words to look at.  The mask is not forced
// to those number of bits.  The user should trim
// the mask if that is intended.
// The routine returns TRUE if the increment increased
// the value of the n vector and FALSE if the increment
// returned the value to zero inside the masked bits.
BOOLEAN incInMask(unsigned int *n, unsigned int *m, int numBits)
{
    int numWords, i;

    numWords = ((numBits-1)>>WORDBITSIZE)+1;

    for (i=0; i<numWords; i++) {
//	printf("I: %d  n:%d  m:%d numWords: %d %d\n", i, *n, *m, numWords, numBits);
	if (m[i]) {
	    n[i] = incInMask(n[i], m[i]);
	    if (n[i]&m[i]) break;
	}
    }
    if (i<numWords) return TRUE;
    return FALSE;
}



void bitInsert(unsigned int *n, int s, int f, unsigned int value)
{
    unsigned int mask;
    int sindex, sloc, floc;

    if (f-s+1>WORDSIZE) {
	fprintf(stderr, "ERROR(Array bitExtract): Too wide for %d bit integer start:%d > finish:%d\n", WORDSIZE, s, f);
	fflush(stderr);
	exit(1);
    }

    sloc = s & WORDSIZEMASK;
    floc = f & WORDSIZEMASK;
    sindex = s>>WORDBITSIZE;
    
    if (floc<sloc) {
	// crosses word boundary
	mask = ZEROS(sloc);
	n[sindex] = (n[sindex]&(~mask)) | ((value<<sloc)&mask);

	mask = ONES(floc+1);
	n[sindex+1] = (n[sindex+1]&(~mask)) | ((value>>(WORDSIZE-sloc))&mask);

	return;
    }

    // all in one word
    mask = MASK(s, f);
    n[sindex] = (n[sindex]&(~mask)) | ((value<<s)&mask);

    return;
}


    
// extract bit s thru f (f>=s) from array of ints that make long bit string.
// WARNING: this routine assumes that f-s+1 <= WORDSIZE
// WARNING: no error checking to see if you run off the end of the int array
// is made.
unsigned int bitExtract(unsigned int *n, int s, int f)
{
    int sindex, sloc, floc;

    if (f-s+1>WORDSIZE) {
	fprintf(stderr, "ERROR(Array bitExtract): Too wide for %d bit integer start:%d > finish:%d\n", WORDSIZE, s, f);
	fflush(stderr);
	exit(1);
    }

    sloc = s & WORDSIZEMASK;
    floc = f & WORDSIZEMASK;
    sindex = s>>WORDBITSIZE;
    
    if (floc<sloc) {
	// crosses word boundary
	floc = WORDSIZEM1-floc;
	return (n[sindex]>>sloc) | ((n[sindex+1]<<floc)>>(floc-(WORDSIZE-sloc)));
    }
    // all in one word
    return EXTRACT(n[sindex], sloc, floc);
}


void copySegment(unsigned int *s, unsigned int *t, int a, int b)
{
    int i, aw, bw;
    unsigned int amask, bmask;

    if (a>b) { i=a; a=b; b=i; }

    aw = a>>WORDBITSIZE;
    amask = ~((1<<(a&WORDSIZEMASK)) - 1);
    bw = b>>WORDBITSIZE;
    bmask = ( ( (1<<(b&WORDSIZEMASK)) -1) <<1) |0x1;
    if (aw==bw) {
	amask &= bmask;
	t[aw] = (t[aw] & ~amask) | (s[aw] & amask);
    }
    else {
	t[aw] = (t[aw] & ~amask) | (s[aw] & amask);
	t[bw] = (t[bw] & ~bmask) | (s[bw] & bmask);
	for (i=aw+1; i<bw; i++) t[i] = s[i];
    }
}



int countSegment(unsigned int *s, int a, int b)
{
    int i, aw, bw;
    unsigned int amask, bmask;

    if (a>b) { i=a; a=b; b=i; }

    aw = a>>WORDBITSIZE;
    amask = ~((1<<(a&WORDSIZEMASK)) - 1);
    bw = b>>WORDBITSIZE;
    bmask = ( ( (1<<(b&WORDSIZEMASK)) -1) <<1) |0x1;
    if (aw==bw) {
	amask &= bmask;
	return bitCount(s[aw] & amask);
    }
    else {
	int c;

	c = bitCount(s[aw] & amask) + bitCount(s[bw] & bmask);
	for (i=aw+1; i<bw; i++) c+= bitCount(s[i]);
	return c;
    }
}



// returns false when done
BOOLEAN firstCombSubset(unsigned int *mask, int subsetSize, int numBits, int *&posData)
{
    int *tmp;
    tmp = posData = new int[subsetSize+2];

    if ((subsetSize==0) || (numBits==0)) return FALSE;
    *(tmp++) = subsetSize;
    *(tmp++) = numBits;
    for (int i=0; i<subsetSize; i++) *tmp++ = i;

    for (int i=0; i<=((numBits-1)>>WORDBITSIZE); i++) mask[i] = 0;
    *mask = ONES(subsetSize);

    return TRUE;
}

// returns false when done
BOOLEAN incCombSubset(unsigned int *mask, int *posData)
{
    int subsetSize;
    int numBits;
    int *x;
    int t;

    subsetSize = *(posData++);
    numBits = *(posData++);
    x = posData;

    t = *posData;
    for (int i=1; i<subsetSize; i++) {
	t++;
	if (*(posData+1) > t) break;
	*posData = i-1;
	posData++;
    }
    (*posData)++;
    if (*posData>=numBits) return FALSE;

    // create the mask from the array of positions
    for (int i=0; i<=((numBits-1)>>WORDBITSIZE); i++) mask[i] = 0;
    for (int i=0; i<subsetSize; i++) {
	mask[x[i]>>WORDBITSIZE] |= 1<<(x[i] & WORDSIZEMASK);
//	printf("%2d ", x[i]);
    }
//    printf("\n");

    return TRUE;
}

// // // // // // // // // // // // // // // // // // // // 
//
// TEST ROUTINES
//

//b // test the incCombSubset for int*'s 
//b int main()
//b {
//b     int *x;
//b     unsigned int mask[10];
//b     char space[1000];
//b     int n = 10;
//b     int k = 5;
//b     BOOLEAN moreToDo;
//b 
//b     moreToDo = firstCombSubset(mask, k, n, x);
//b     for (int i=0; (i<10000) && moreToDo; i++) {
//b 	printf("%s %d \n", btostr(space, mask, n), bitCount(mask, n));
//b 	moreToDo = incCombSubset(mask, x);
//b     }
//b }


/*int main(int argc, char *argv[])
{

    unsigned int mask, first, last;
    int i;
    int numSets, numCovered;
    int n, k, p;
    char space[WORDSIZE*10];
    int width, nb;

    n = 35;
    k = 2;
    i = 0;
    p = 1;

    int notDone = 1;
    for (mask = firstCombSubset(k); notDone; notDone=incCombSubset2(&mask, n)) {
	printf("%4d %1d %10s %d\n", i++, p, btostr(space, mask, n), bitCount(mask));
	if (i>5000) break;
    }
    printf("%10s %d\n", btostr(space, mask, n), bitCount(mask));
  */
//a    p = 1;
//a    for (i=0; i<32; i++) {
//a	printf("%d %d\n", i, rightMostBitIndex(p));
//a	p<<=1;
//a    }
//a
//a    for (i=0; i<=32; i++) {
//a	printf("%2d %10s\n", i, btostr(space, ONES(i), 32));
//a    }
//a
//a     
//a     for (i=0; i<10; i++) printf("leftmostbit(%d) = %d\n", i, leftMostBitIndex(i));
//a 
//a     unsigned int x[4], y[4];
//a     y[0]=63;
//a     y[1]=0;
//a     y[2]=0;
//a     y[3]=0;
//a 
//a     x[0]=128;
//a     x[1]=0;
//a     x[2]=1;
//a     x[3]=6;
//a 
//a     for (i=0; i<35; i++) {
//a 	printf("i:%d  y:%s\n", i, btostr(space, y, 100));
//a 	n=incInMask(y, x, 99);
//a 	printf("n: %d\n", n);
//a 	fflush(stdout);
//a     }
//a     
//a     exit(0);
//a 
//a 
//a     n = atoi(argv[1]);
//a     k = atoi(argv[2]);
//a     p = atoi(argv[3]);
//a 
//a     first = firstCombSubset(n, k); 
//a     last = lastCombSubset(n, k); 
//a 
//a     unsigned int *ac;
//a     ac = new unsigned int [p];
//a     firstUniqueArrayCounter(ac, p, first, incCombSubset);
//a 
//a     numCovered = 0;
//a     for (numSets=1; ; numSets++) {
//a 	printf("code: %d   Cover Value: %d\n", 
//a 	       codeUniqueArrayCounter(ac, p, n),
//a 	       coverValue2Print(n, ac));
//a         printUniqueArrayCounter(ac, p, n);
//a 	if (!incUniqueArrayCounter(ac, p, last, incCombSubset)) break;
//a //        printf("   %s\n", btostr(space, unionUniqueArrayCounter(ac, p), n));
//a 	if (isCoveredUniqueArrayCounter(ac, p, n)) numCovered++;
//a     }
//a     printf("Number of sets: %d   Number covered: %d\n", numSets-1, numCovered);
//a 
//a 

//a     timeSetRandom();
//a     for (width=0; width<31; width++) {
//a 	for (nb=0; nb<=width; nb++) {
//a 	    mask = randBitMask(width, nb);
//a 	    if (bitCount(mask & ONES(width)) != nb) {
//a 		printf("%2d %2d %s\n", 
//a 		       width, 
//a 		       nb, 
//a 		       binToBtostr(space, mask, width));
//a 		fflush(stdout);
//a 	    }
//a 	    mask = randBitMask(width);
//a 	    if ((mask & ONES(width)) != mask) {
//a 		printf("%2d %s\n", 
//a 		       width, 
//a 		       mask);
//a 		fflush(stdout);
//a 	    }
//a 	}
//a     }
//a //    exit(0);
//a //    mask = atoi(argv[1]);
//a     scanf("%d", &mask);
//a     for (i=0; i<20; i++) {
//a //	printf("%d %d %d\n", i, pack(i, mask), pack2(i, mask));
//a 	printf("%d %d\n", i, pack(i, mask));
//a 	fflush(stdout);
//a     }
//a 
//a     n = 0;
//a     for (i=0; i<20; i++) {
//a 	printf("i:%d  n:%d\n", i, n);
//a 	n = incInMask(n, mask);
//a 	fflush(stdout);
//a     }
//a 
//a     n = 1;
//a     for (i=0; i<=WORDSIZE; i++) {
//a 	printf("%2d %s %d\n", i, binToBtostr(space, n, WORDSIZE), bitWidth(n));
//a 	fflush(stdout);
//a 	n = (n*3) << 1;
//a     }
//a 
//a     printf("DONE!\n");
//a     fflush(stdout);
//    return 0;
//}




/*int main(int argc, char *argv[])
{
    int size=50;
    const int bitsize=WORDSIZE*size;
    unsigned int x[size], y[size];
    int i, j;
    unsigned int seeda=3212453;
    unsigned int seedb=333331;

    setFastRand(seeda, seedb);
    size=34;
    clearAll(x, size);
    bitPrint(x, size);
    printf("\n");
    setAll(x, size-2);
    bitPrint(x, size);
    printf("\n");
    bitInv(x, 5);
    bitPrint(x, size);
    printf("\n");
    randBitMask(x, size);
    randBitMask(y, size);
    bitPrint(x, size);
    printf("\n");
    bitInv(x, size);
    bitPrint(x, size);
    printf("\n");
    bitPrint(y, size);
    printf("\n");
    bitAnd(x, y, size);
    bitPrint(x, size);
    printf("\n");
    printf("%d\n", bitCmp(x, y, size));
    printf("%d\n", bitCmp(y, x, size));
    printf("%d\n", bitCmp(x, x, size));
    printf("%d\n", bitHamming(x, y, size));
    printf("%d\n", bitHamming(x, x, size));
    exit(0);

    // set the prime numbered bits to zero
    for (i=2; i<bitsize; i++) {
	if (getBit(x, i)==0) {
	    for (j=i*i; j<bitsize; j+=i) setBit(x, j);
	}
    }
    printf("%s\n", btostr(x, bitsize));
    printf("size: %d\n", bitCount(x, bitsize));

    {
	int start=0;
	int size=7;
	int step=7;

	for (i=start; i<bitsize; i+=step) {
	    if (i+size-1>=bitsize) i = bitsize-size; 
	    printf("i:%2d  size:%2d  count:%2d\n", 
		   i, size, countSegment(x, i, i+size-1));
	    if (i==bitsize-size) break;
	}
    }


    return 0;
}

*/
int main()
{
    unsigned int n, m;

    n = 0;
    for (int i=0; i<10000000; i++) {
        n = n*3+i;
        m = bitDegray(n);
        m = bitGray(m);
        if (n != m) printf("error: 0x%08x 0x%08x\n", n, m);

        m = bitGray(n);
        m = bitDegray(m);
        if (n != m) printf("error: 0x%08x 0x%08x\n", n, m);
        printf("0x%08x \n", n);
    }
    return 0;
}