#include "genetic/phenotypebinary.h"

#include <bu/random.h>
#include <bu/sio.h>

using namespace Bu;

// Bits per word
#define BPW     			(sizeof(uint_fast32_t)*8)
#define wordsForBits( x )	(((x)/BPW)+(((x)%BPW)?1:0))
#define wordWithBit( x )	((x)/BPW)

Genetic::PhenotypeBinary::PhenotypeBinary( int iSize, bool bRandom ) :
	iSize( iSize ),
	iWords( wordsForBits(iSize) ),
	aGenes( NULL )
{
	aGenes = new uint_fast32_t[iWords];
	if( bRandom )
		randomize();
	else
		memset( aGenes, 0, sizeof(uint_fast32_t)*iWords );
}

Genetic::PhenotypeBinary::~PhenotypeBinary()
{
	delete[] aGenes;
}

Genetic::Phenotype &Genetic::PhenotypeBinary::randomize()
{
	for( int j = 0; j < iWords*sizeof(uint_fast32_t); j++ )
	{
		((uint8_t *)aGenes)[j] = (uint8_t)Bu::Random::rand();
	}

	return *this;
}

void Genetic::PhenotypeBinary::mutate( int iLocation, float fMagnitude )
{
	if( fMagnitude > -0.5 && fMagnitude < 0.5 )
		return;

	aGenes[wordWithBit(iLocation)] =
		(aGenes[wordWithBit(iLocation)]&(~(1<<((iLocation)%BPW)))) |
		((~aGenes[wordWithBit(iLocation)])&((1<<((iLocation)%BPW))));
}

Genetic::Phenotype *Genetic::PhenotypeBinary::makeEmptyOffspring(
	int iNewSize )
{
	if( iNewSize < 0 )
		iNewSize = iSize;

	return new PhenotypeBinary( iSize );
}

Genetic::Phenotype &Genetic::PhenotypeBinary::copyFrom( const Phenotype &rSrc,
		int iStart, int iCount, int iDest )
{
	if( iCount <= 0 )
		return *this;

	const PhenotypeBinary &rbSrc =
		dynamic_cast<const Genetic::PhenotypeBinary &>(rSrc);
	if( iDest < 0 )
		iDest = iStart;

	// Fist draft, very sloppy: bit by bit copy, this is stupid, but easy
	/*
	for( int j = 0; j < iCount; j++ )
	{
		int wd = wordWithBit(j+iDest);
		int ws = wordWithBit(j+iStart);
		if( (rbSrc.aGenes[ws]&(1<<((j+iStart)%BPW))) == 0)
		{
			aGenes[wd] &= ~(1<<((j+iDest)%BPW));
		}
		else
		{
			aGenes[wd] |= (1<<((j+iDest)%BPW));
		}
	}
	*/

	{
		if( iDest == iStart )
		{
			if( wordWithBit(iStart) == wordWithBit(iStart+iCount-1) )
			{
				uint_fast32_t uMask = -1;
				uMask = (uMask << (iStart%BPW)) &
					(uMask >> (BPW-(((iStart+iCount))%BPW)));
//				sio << Fmt().radix(2).width(32).fill('0') << uMask << sio.nl;
				aGenes[wordWithBit( iDest )] =
					(aGenes[wordWithBit( iDest )]&(~uMask)) |
					(rbSrc.aGenes[wordWithBit( iStart )]&uMask);
			}
			else
			{
				int iStartWord = wordWithBit( iStart );
				int iEndWord = wordWithBit( iStart+iCount );

//				sio << wordWithBit( iStart+iCount ) << "/" << iWords << sio.nl;
//				sio << iStart << " + " << iCount << " = "  << iStart+iCount << " / " << iSize << sio.nl;


				uint_fast32_t uMask = -1;
				uMask = uMask << (iStart%BPW);
//				sio << Fmt().radix(2).width(32).fill('0') << uMask << sio.nl;
				aGenes[wordWithBit( iDest )] =
					(aGenes[wordWithBit( iDest )]&(~uMask)) |
					(rbSrc.aGenes[wordWithBit( iStart )]&uMask);

				for( int j = iStartWord+1; j < iEndWord; j++ )
					aGenes[j] = rbSrc.aGenes[j];
				
				uMask = ((uint_fast32_t)-1) >> (BPW-(((iStart+iCount))%BPW));
//				sio << Fmt().radix(2).width(32).fill('0') << uMask << sio.nl;
				aGenes[wordWithBit( iDest+iCount-1 )] =
					(aGenes[wordWithBit( iDest+iCount-1 )]&(~uMask)) |
					(rbSrc.aGenes[wordWithBit( iStart+iCount-1 )]&uMask);
			}
		}
		else if( iDest < iStart )
		{
			sio << "iDest < iStart not finished." << sio.nl;
		}
		else
		{
			sio << "iDest > iStart not finished." << sio.nl;
		}
	}	

	/*
	iStart%BPW
	int iWords = wordsForBits(iCount);
	for( int j = wordWithBit(iStart); j < iWords; j++ )
	{
	}
	*/

	return *this;
}

Bu::String Genetic::PhenotypeBinary::toString()
{
	Bu::String sRet;
	for( int j = 0; j < iSize; j++ )
	{
		sRet += (aGenes[j/BPW]&(1<<(j%BPW)))?'1':'0';
	}

	return sRet;
}

void Genetic::PhenotypeBinary::write( Bu::Stream &rStream )
{
	uint32_t uSize = htobe32( iSize );
	rStream.write( &uSize, 4 );
	uSize = (iSize/8) + ((iSize%8)?1:0);
	for( int j = 0; j < uSize; j++ )
	{
		rStream.write(
			&((char *)&aGenes[j/sizeof(uint_fast32_t)])[j%sizeof(uint_fast32_t)],
			1
			);
	}
}

void Genetic::PhenotypeBinary::read( Bu::Stream &rStream )
{
	uint32_t uSize;
	rStream.read( &uSize, 4 );
	iSize = be32toh( uSize );
	iWords = wordsForBits(iSize);
	delete[] aGenes;
	aGenes = new uint_fast32_t[iWords];
	uSize = (iSize/8) + ((iSize%8)?1:0);
	for( int j = 0; j < uSize; j++ )
	{
		rStream.read(
			&((char *)&aGenes[j/sizeof(uint_fast32_t)])[j%sizeof(uint_fast32_t)],
			1
			);
	}
}

void Genetic::PhenotypeBinary::extractBits( uint32_t &rTarget, int iStart,
		int iBits )
{
	rTarget = 0;
	if( iBits > sizeof(rTarget)*8 )
		iBits = sizeof(rTarget)*8;

	// This is pretty much the same problem as copyFrom, so I'm doing it the
	// same way for now.
	for( int j = 0; j < iBits; j++ )
	{
		int ws = wordWithBit(j+iStart);
		if( (aGenes[ws]&(1<<((j+iStart)%BPW))) != 0 )
			rTarget |= (1<<(j%BPW));
	}
}

void Genetic::PhenotypeBinary::extractBits( uint64_t &rTarget, int iStart,
		int iBits )
{
	rTarget = 0;
	if( iBits > sizeof(rTarget)*8 )
		iBits = sizeof(rTarget)*8;

	// This is pretty much the same problem as copyFrom, so I'm doing it the
	// same way for now.
	for( int j = 0; j < iBits; j++ )
	{
		int ws = wordWithBit(j+iStart);
		if( (aGenes[ws]&(1<<((j+iStart)%BPW))) != 0 )
			rTarget |= (1ll<<(j));
	}
}