/*
This program is distributed under the terms of the 'MIT license'. The text
of this licence follows...

Copyright (c) 2007-2009 J.D.Medhurst (a.k.a. Tixy)

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

/**
@file

@brief Bit-Vector utilities.
*/

#include "common.h"
#include "bitvector.h"


void BitVectorPointer::Reset()
	{
	unsigned* bits = Bits;
	unsigned* bitsEnd = Bits+((Size+BitVectorIndexMask)>>BitVectorIndexShift);
	unsigned zero = 0;
	while(bits<bitsEnd)
		*bits++ = zero;
	}


int BitVectorPointer::Test(unsigned index)
	{
	ASSERT_DEBUG(index<Size);
	return (Bits[index>>BitVectorIndexShift]>>(index&BitVectorIndexMask))&1;
	}


void BitVectorPointer::Set(unsigned index)
	{
	ASSERT_DEBUG(index<Size);
	Bits[index>>BitVectorIndexShift] |= 1<<(index&BitVectorIndexMask);
	}


void BitVectorPointer::Clear(unsigned index)
	{
	ASSERT_DEBUG(index<Size);
	Bits[index>>BitVectorIndexShift] &= ~(1<<(index&BitVectorIndexMask));
	}


#if defined(__GNUC__) && defined(_ARM) // ARM assembler versions...


#if defined(DEBUG) && !defined(LOCAL_ASSERTFAILED_DEFINED)

#define LOCAL_ASSERTFAILED_DEFINED

static int AssertFailed(int line)
	{
	return AssertFailed(__FILE__,line);
	}

#endif


__attribute__((naked)) int BitVectorPointer::TestSet(unsigned /*index*/, size_t /*count*/)
	{
#ifdef DEBUG
	asm("ldr	r3, [r0, #%a0]" : : "i" (offsetof(BitVectorPointer,Size)));
	asm("add	r12, r1, r2");
	asm("cmp	r1, r3");
	asm("ldrhs	r0, =%a0"		: : "i" (__LINE__));
	asm("bhs	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index>=Size
	asm("cmp	r12, r1");
	asm("ldrls	r0, =%a0"		: : "i" (__LINE__));
	asm("bls	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index+count<=index
	asm("cmp	r12, r3");
	asm("ldrhi	r0, =%a0"		: : "i" (__LINE__));
	asm("bhi	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index+count>Size
#endif
	// set r3 = ptr, r2 = startMask ...
	asm("ldr	r12, [r0, #%a0]" : : "i" (offsetof(BitVectorPointer,Bits)));
	asm("mov	r0, r1, lsr #5");
	asm("add	r3, r12, r0, lsl #2");
	asm("and	r0, r1, #31");
	asm("add	r1, r1, r2");
	asm("sub	r1, r1, #1");
	asm("mvn	r2, #0");
	asm("mvn	r2, r2, lsl r0");
	// set r1 = end, r12 = endMask ...
	asm("mvn	r0, r1");
	asm("and	r0, r0, #31");
	asm("mov	r1, r1, lsr #5");
	asm("add	r1, r12, r1, lsl #2");
	asm("mvn	r12, #0");
	asm("mvn	r12, r12, lsr r0");
	// do first word...
	asm("cmp	r3, r1");
	asm("ldr	r0, [r3], #4");
	asm("orreq	r2, r2, r12");
	asm("orr	r0, r0, r2");
	asm("beq	9f");
	// do middle words...
	asm("cmp	r3, r1");
	asm("0:");
	asm("ldrlo	r2, [r3], #4");
	asm("andlo	r0, r0, r2");
	asm("cmplo	r3, r1");
	asm("blo	0b");
	// do last word...
	asm("ldr	r2, [r3]");
	asm("orr	r2, r2, r12");
	asm("and	r0, r0, r2");
	asm("9:");
	asm("cmp	r0, #0xffffffff"); // don't use "cmn r0,#0" here because GCC produces the wrong op-code for that!
	asm("moveq	r0, #1");
	asm("movne	r0, #0");
	asm("mov	pc, lr");
	dummy_return(int);
	}


__attribute__((naked)) int BitVectorPointer::TestClear(unsigned /*index*/, size_t /*count*/)
	{
#ifdef DEBUG
	asm("ldr	r3, [r0, #%a0]" : : "i" (offsetof(BitVectorPointer,Size)));
	asm("add	r12, r1, r2");
	asm("cmp	r1, r3");
	asm("ldrhs	r0, =%a0"		: : "i" (__LINE__));
	asm("bhs	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index>=Size
	asm("cmp	r12, r1");
	asm("ldrlo	r0, =%a0"		: : "i" (__LINE__));
	asm("bls	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index+count<=index
	asm("cmp	r12, r3");
	asm("ldrhi	r0, =%a0"		: : "i" (__LINE__));
	asm("bhi	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index+count>Size
#endif
	// set r3 = ptr, r2 = startMask ...
	asm("ldr	r12, [r0, #%a0]" : : "i" (offsetof(BitVectorPointer,Bits)));
	asm("mov	r0, r1, lsr #5");
	asm("add	r3, r12, r0, lsl #2");
	asm("and	r0, r1, #31");
	asm("add	r1, r1, r2");
	asm("sub	r1, r1, #1");
	asm("mvn	r2, #0");
	asm("mov	r2, r2, lsl r0");
	// set r1 = end, r12 = endMask ...
	asm("mvn	r0, r1");
	asm("and	r0, r0, #31");
	asm("mov	r1, r1, lsr #5");
	asm("add	r1, r12, r1, lsl #2");
	asm("mvn	r12, #0");
	asm("mov	r12, r12, lsr r0");
	// do first word...
	asm("cmp	r3, r1");
	asm("ldr	r0, [r3], #4");
	asm("andeq	r2, r2, r12");
	asm("and	r0, r0, r2");
	asm("beq	9f");
	// do middle words...
	asm("cmp	r3, r1");
	asm("0:");
	asm("ldrlo	r2, [r3], #4");
	asm("orrlo	r0, r0, r2");
	asm("cmplo	r3, r1");
	asm("blo	0b");
	// do last word...
	asm("ldr	r2, [r3]");
	asm("and	r2, r2, r12");
	asm("orr	r0, r0, r2");
	asm("9:");
	asm("cmp	r0, #0");
	asm("moveq	r0, #1");
	asm("movne	r0, #0");
	asm("mov	pc, lr");
	dummy_return(int);
	}


__attribute__((naked)) void BitVectorPointer::Set(unsigned /*index*/, size_t /*count*/)
	{
#ifdef DEBUG
	asm("ldr	r3, [r0, #%a0]" : : "i" (offsetof(BitVectorPointer,Size)));
	asm("add	r12, r1, r2");
	asm("cmp	r1, r3");
	asm("ldrhs	r0, =%a0"		: : "i" (__LINE__));
	asm("bhs	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index>=Size
	asm("cmp	r12, r1");
	asm("ldrlo	r0, =%a0"		: : "i" (__LINE__));
	asm("blo	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index+count<index
	asm("cmp	r12, r3");
	asm("ldrhi	r0, =%a0"		: : "i" (__LINE__));
	asm("bhi	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index+count>Size
#endif
	asm("cmp	r2, #0");
	asm("moveq	pc, lr");
	// set r3 = ptr, r2 = startMask ...
	asm("ldr	r12, [r0, #%a0]" : : "i" (offsetof(BitVectorPointer,Bits)));
	asm("mov	r0, r1, lsr #5");
	asm("add	r3, r12, r0, lsl #2");
	asm("and	r0, r1, #31");
	asm("add	r1, r1, r2");
	asm("sub	r1, r1, #1");
	asm("mvn	r2, #0");
	asm("mov	r2, r2, lsl r0");
	// set r1 = end, r12 = endMask ...
	asm("mvn	r0, r1");
	asm("and	r0, r0, #31");
	asm("mov	r1, r1, lsr #5");
	asm("add	r1, r12, r1, lsl #2");
	asm("mvn	r12, #0");
	asm("mov	r12, r12, lsr r0");
	// do first word...
	asm("ldr	r0, [r3]");
	asm("cmp	r3, r1");
	asm("andeq	r2, r2, r12");
	asm("orr	r0, r0, r2");
	asm("str	r0, [r3], #4");
	asm("moveq	pc, lr");
	// do middle words...
	asm("mvn	r0, #0");
	asm("cmp	r3, r1");
	asm("0:");
	asm("strlo	r0, [r3], #4");
	asm("cmplo	r3, r1");
	asm("blo	0b");
	// do last word...
	asm("ldr	r0, [r3]");
	asm("orr	r0, r0, r12");
	asm("str	r0, [r3]");
	asm("mov	pc, lr");
	}


__attribute__((naked)) void BitVectorPointer::Clear(unsigned /*index*/, size_t /*count*/)
	{
#ifdef DEBUG
	asm("ldr	r3, [r0, #%a0]" : : "i" (offsetof(BitVectorPointer,Size)));
	asm("add	r12, r1, r2");
	asm("cmp	r1, r3");
	asm("ldrhs	r0, =%a0"		: : "i" (__LINE__));
	asm("bhs	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index>=Size
	asm("cmp	r12, r1");
	asm("ldrlo	r0, =%a0"		: : "i" (__LINE__));
	asm("blo	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index+count<index
	asm("cmp	r12, r3");
	asm("ldrhi	r0, =%a0"		: : "i" (__LINE__));
	asm("bhi	%a0"			: : "i" ((int(*)(int))AssertFailed)); // index+count>Size
#endif
	asm("cmp	r2, #0");
	asm("moveq	pc, lr");
	// r3 = ptr, r2 = startMask
	asm("ldr	r12, [r0, #%a0]" : : "i" (offsetof(BitVectorPointer,Bits)));
	asm("mov	r0, r1, lsr #5");
	asm("add	r3, r12, r0, lsl #2");
	asm("and	r0, r1, #31");
	asm("add	r1, r1, r2");
	asm("sub	r1, r1, #1");
	asm("mvn	r2, #0");
	asm("mov	r2, r2, lsl r0");
	// r1 = end, r12 = endMask
	asm("mvn	r0, r1");
	asm("and	r0, r0, #31");
	asm("mov	r1, r1, lsr #5");
	asm("add	r1, r12, r1, lsl #2");
	asm("mvn	r12, #0");
	asm("mov	r12, r12, lsr r0");
	// do first word...
	asm("ldr	r0, [r3]");
	asm("cmp	r3, r1");
	asm("andeq	r2, r2, r12");
	asm("bic	r0, r0, r2");
	asm("str	r0, [r3], #4");
	asm("moveq	pc, lr");
	// do middle words...
	asm("mov	r0, #0");
	asm("cmp	r3, r1");
	asm("0:");
	asm("strlo	r0, [r3], #4");
	asm("cmplo	r3, r1");
	asm("blo	0b");
	// do last word...
	asm("ldr	r0, [r3]");
	asm("bic	r0, r0, r12");
	asm("str	r0, [r3]");
	asm("mov	pc, lr");
	}


#else // C++ versions...


int BitVectorPointer::TestSet(unsigned index, size_t count)
	{
	ASSERT_DEBUG(index<Size);
	ASSERT_DEBUG(index+count>index);
	ASSERT_DEBUG(index+count<=Size);

	unsigned* ptr = Bits+(index>>BitVectorIndexShift);
	unsigned startMask = ~((~0u)<<(index&BitVectorIndexMask));
	index = index+count-1;
	unsigned* end = Bits+(index>>BitVectorIndexShift);
	unsigned endMask = ~((~0u)>>((~index)&BitVectorIndexMask));

	// startMask and endMask contain bitmasks for the first and last
	// entries in the Bits array which we will use. They have set (1)
	// bits for each bit in these entries which lie outside the range
	// we are testing, so ORing with these will set the bits we
	// aren't interested in.

	unsigned result; // all bits will be set to one if test is true
	if(ptr==end)
		{
		startMask |= endMask;
		result = *ptr|startMask;
		}
	else
		{
		result = *ptr++|startMask;
		while(ptr<end)
			result &= *ptr++;
		result &= *ptr|endMask;
		}
	return result==~0u;
	}


int BitVectorPointer::TestClear(unsigned index, size_t count)
	{
	ASSERT_DEBUG(index<Size);
	ASSERT_DEBUG(index+count>index);
	ASSERT_DEBUG(index+count<=Size);

	unsigned* ptr = Bits+(index>>BitVectorIndexShift);
	unsigned startMask = (~0u)<<(index&BitVectorIndexMask);
	index = index+count-1;
	unsigned* end = Bits+(index>>BitVectorIndexShift);
	unsigned endMask = (~0u)>>((~index)&BitVectorIndexMask);

	// startMask and endMask contain bitmasks for the first and last
	// entries in the Bits array which we will use. They have set (1)
	// bits for each bit in these entries which lie inside the range
	// we are testing, so ANDing with these will clear the bits we
	// aren't interested in.

	unsigned result; // all bits will be set to zero if test is true

	if(ptr==end)
		result = *ptr&startMask&endMask;
	else
		{
		result = *ptr++&startMask;
		while(ptr<end)
			result |= *ptr++;
		result |= *ptr&endMask;
		}
	return result==0;
	}


void BitVectorPointer::Set(unsigned index, size_t count)
	{
	ASSERT_DEBUG(index<Size);
	ASSERT_DEBUG(index+count>=index);
	ASSERT_DEBUG(index+count<=Size);

	if(!count)
		return;

	unsigned* ptr = Bits+(index>>BitVectorIndexShift);
	unsigned startMask = (~0u)<<(index&BitVectorIndexMask);
	index = index+count-1;
	unsigned* end = Bits+(index>>BitVectorIndexShift);
	unsigned endMask = (~0u)>>((~index)&BitVectorIndexMask);

	// startMask and endMask contain bitmasks for the first and last
	// entries in the Bits array which we will use. They have set (1)
	// bits for each bit in these entries which lie inside the range
	// we are testing, so ORing with these will set the bits we want to.

	if(ptr==end)
		{
		startMask &= endMask;
		*ptr |= startMask;
		}
	else
		{
		*ptr++ |= startMask;
		while(ptr<end)
			*ptr++ = ~0u;
		*ptr |= endMask;
		}
	}


void BitVectorPointer::Clear(unsigned index, size_t count)
	{
	ASSERT_DEBUG(index<Size);
	ASSERT_DEBUG(index+count>=index);
	ASSERT_DEBUG(index+count<=Size);

	if(!count)
		return;

	unsigned* ptr = Bits+(index>>BitVectorIndexShift);
	unsigned startMask = (~0u)<<(index&BitVectorIndexMask);
	index = index+count-1;
	unsigned* end = Bits+(index>>BitVectorIndexShift);
	unsigned endMask = (~0u)>>((~index)&BitVectorIndexMask);

	// startMask and endMask contain bitmasks for the first and last
	// entries in the Bits array which we will use. They have set (1)
	// bits for each bit in these entries which lie inside the range
	// we are testing, so ANDing with the inverse of these will clear
	// the bits we want to.

	if(ptr==end)
		{
		startMask &= endMask;
		*ptr &= ~startMask;
		}
	else
		{
		*ptr++ &= ~startMask;
		while(ptr<end)
			*ptr++ = 0;
		*ptr &= ~endMask;
		}
	}

#endif


int BitVectorPointer::Find(size_t count, unsigned state)
	{
	if(count>Size || !count)
		return -1;

	unsigned* ptr = Bits;
	unsigned* end = ptr+((Size+BitVectorIndexMask)>>BitVectorIndexShift);
	unsigned bits;
	unsigned mask = 0;
	unsigned found = 0;
	if(state)
		state = ~0u;

_next_word:
	if(ptr>=end)
		goto _not_found;
	bits = state^*ptr++;
	mask = 1;
	if(bits&mask)
		{
_skip_set:
		found = 0;
		if(bits+mask<mask)
			goto _next_word; // rest of bits are all set
		do
			mask<<=1;
		while(bits&mask);
		}
	bits = ~bits;
	do
		{
		++found;
		mask<<=1;
		}
	while(bits&mask);
	if(found>=count)
		goto _found;
	if(!mask)
		goto _next_word;
	bits = ~bits;
	goto _skip_set;

_found:
	{
	unsigned index = (ptr-Bits)<<BitVectorIndexShift;
	if(mask)
		do --index; while(mask<<=1);
	return index-found;
	}

_not_found:
	return -1;
	}