MMDVM_CM/M172DMR/codec2/pack.cpp

/*
  Copyright (C) 2010 Perens LLC <bruce@perens.com>

  All rights reserved.

  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU Lesser General Public License version 2.1, as
  published by the Free Software Foundation.  This program is
  distributed in the hope that it will be useful, but WITHOUT ANY
  WARRANTY; without even the implied warranty of MERCHANTABILITY or
  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public
  License for more details.

  You should have received a copy of the GNU Lesser General Public License
  along with this program; if not, see <http://www.gnu.org/licenses/>.
*/

#include "defines.h"
#include "quantise.h"
#include <stdio.h>

/* Compile-time constants */
/* Size of unsigned char in bits. Assumes 8 bits-per-char. */
static const unsigned int	WordSize = 8;

/* Mask to pick the bit component out of bitIndex. */
static const unsigned int	IndexMask = 0x7;

/* Used to pick the word component out of bitIndex. */
static const unsigned int	ShiftRight = 3;

/** Pack a bit field into a bit string, encoding the field in Gray code.
 *
 * The output is an array of unsigned char data. The fields are efficiently
 * packed into the bit string. The Gray coding is a naive attempt to reduce
 * the effect of single-bit errors, we expect to do a better job as the
 * codec develops.
 *
 * This code would be simpler if it just set one bit at a time in the string,
 * but would hit the same cache line more often. I'm not sure the complexity
 * gains us anything here.
 *
 * Although field is currently of int type rather than unsigned for
 * compatibility with the rest of the code, indices are always expected to
 * be >= 0.
 */
void CQuantize::pack(
	unsigned char *bitArray,  /* The output bit string. */
	unsigned int  *bitIndex,  /* Index into the string in BITS, not bytes.*/
	int			   field,	  /* The bit field to be packed. */
	unsigned int   fieldWidth /* Width of the field in BITS, not bytes. */
)
{
	pack_natural_or_gray(bitArray, bitIndex, field, fieldWidth, 1);
}

void CQuantize::pack_natural_or_gray(
	unsigned char *bitArray,   /* The output bit string. */
	unsigned int  *bitIndex,   /* Index into the string in BITS, not bytes.*/
	int			   field,	   /* The bit field to be packed. */
	unsigned int   fieldWidth, /* Width of the field in BITS, not bytes. */
	unsigned int   gray        /* non-zero for gray coding */
)
{
	if (gray)
	{
		/* Convert the field to Gray code */
		field = (field >> 1) ^ field;
	}

	do
	{
		unsigned int bI = *bitIndex;
		unsigned int bitsLeft = WordSize - (bI & IndexMask);
		unsigned int sliceWidth = bitsLeft < fieldWidth ? bitsLeft : fieldWidth;
		unsigned int wordIndex = bI >> ShiftRight;

		bitArray[wordIndex] |= ((unsigned char)((field >> (fieldWidth - sliceWidth)) << (bitsLeft - sliceWidth)));

		*bitIndex = bI + sliceWidth;
		fieldWidth -= sliceWidth;
	}
	while ( fieldWidth != 0 );
}

/** Unpack a field from a bit string, converting from Gray code to binary.
 *
 */
int CQuantize::unpack(
	const unsigned char *bitArray, /* The input bit string. */
	unsigned int        *bitIndex, /* Index into the string in BITS, not bytes.*/
	unsigned int		 fieldWidth/* Width of the field in BITS, not bytes. */
)
{
	return unpack_natural_or_gray(bitArray, bitIndex, fieldWidth, 1);
}

/** Unpack a field from a bit string, to binary, optionally using
 * natural or Gray code.
 *
 */
int CQuantize::unpack_natural_or_gray(
	const unsigned char *bitArray,  /* The input bit string. */
	unsigned int        *bitIndex,  /* Index into the string in BITS, not bytes.*/
	unsigned int		 fieldWidth,/* Width of the field in BITS, not bytes. */
	unsigned int         gray       /* non-zero for Gray coding */
)
{
	unsigned int	field = 0;
	unsigned int	t;

	do
	{
		unsigned int bI = *bitIndex;
		unsigned int bitsLeft = WordSize - (bI & IndexMask);
		unsigned int sliceWidth = bitsLeft < fieldWidth ? bitsLeft : fieldWidth;

		field |= (((bitArray[bI >> ShiftRight] >> (bitsLeft - sliceWidth)) & ((1 << sliceWidth) - 1)) << (fieldWidth - sliceWidth));

		*bitIndex = bI + sliceWidth;
		fieldWidth -= sliceWidth;
	}
	while ( fieldWidth != 0 );

	if (gray)
	{
		/* Convert from Gray code to binary. Works for maximum 8-bit fields. */
		t = field ^ (field >> 8);
		t ^= (t >> 4);
		t ^= (t >> 2);
		t ^= (t >> 1);
	}
	else
	{
		t = field;
	}

	return t;
}