#include <stdio.h>
#include <stdlib.h>

// HAMMING CODE EXAMPLE.  Corrects 1 bit error.  This is a (7,4) code.

int CodeToData (unsigned char code);
unsigned char DataToCode (int data);

unsigned char CorrectCode (unsigned char code);

int EncodeBlock (char *inbuffer, char *outbuffer, int n);
int DecodeBlock (char *inbuffer, char *outbuffer, int n);


// Really only nead a 7-bit Hamming Code.
struct {
	int nData;  // 4-bit data (0 to 15), bytes will be handled nibble-by-nibble
	int nCode;  // 7-bit Hamming code.  Set MSB to zero.
} HammingCodes[16] = {
	{0,   0x00},
	{1,   0x0F},
	{2,   0x13},
	{3,   0x1C},
	{4,   0x25},
	{5,   0x2A},
	{6,   0x36},
	{7,   0x39},
	{8,   0x46},
	{9,   0x49},
	{10,  0x55},
	{11,  0x5A},
	{12,  0x63},
	{13,  0x6C},
	{14,  0x70},
	{15,  0x7F}
};

int CodeToData (unsigned char code)
{
	int i;

	for (i = 0; i < 16; i++) {
		if (code == HammingCodes[i].nCode) {
			// Got it!  Return the 4-bit data
			return i;
		}
	}
	// Not a code!
	return -1;
}

unsigned char DataToCode (int data)
{
	if (data >= 0 && data < 16) {
		return HammingCodes[data].nCode;
	}
	return 0;
}

int EncodeBlock (char *inbuffer, char *outbuffer, int n)
{
	int i;
	char *poutbuffer;

	poutbuffer = outbuffer;
	for (i = 0; i < n; i++) {
		*poutbuffer++ = DataToCode((inbuffer[i] >> 4) & 0x0F);
		*poutbuffer++ = DataToCode(inbuffer[i] & 0x0F);
	}
	return 0;
}

unsigned char CorrectCode (unsigned char code)
{
	int n, i, mask;

	n = CodeToData (code);
	if (n == -1) {
		// Error!

		// Flip each bit and see if we can fix it!
		mask = 1;
		for (i = 0; i < 8; i++) {
			n = CodeToData (code ^ mask);
			if (n != -1) {
				// Corrected it!
				return code ^ mask;
			}
			mask <<= 1;
		}
		// Must not have worked...
		return code;
	}
}

int DecodeBlock (char *inbuffer, char *outbuffer, int n)
{
	int i;
	char *poutbuffer;
	char new_code;

	int bHiNibble = 1;
	int data;

	poutbuffer = outbuffer;
	for (i = 0; i < n; i++) {
		data = CodeToData(inbuffer[i]);
		if (data == -1) {
			// We got a problem.  Attempt to correct the code
			new_code = CorrectCode (inbuffer[i]);
			data = CodeToData(new_code);
			if (data == -1) {
				data = 0;
			}
			else {
				printf ("\n\rCorrected Code!");
			}
		}

		if (bHiNibble) {
			*poutbuffer = (unsigned char)(data << 4);
			*poutbuffer &= 0xF0;
			bHiNibble = 0;
		}
		else {
			*poutbuffer |= (unsigned char)data;
			bHiNibble = 1;
			poutbuffer++;
		}
	}
	return 0;
}

char szTest1[100];
char szTest2[100];
char szTest3[100];

int main ()
{
	int i;
	szTest1[0] = 0x26;
	szTest1[1] = 0x51;
	szTest1[2] = 0xD7;
	szTest1[3] = 0x0C;

	randomize();

	printf ("\n\rInput Block     = ");
	for (i = 0; i < 4; i++) printf ("%02x ", (unsigned char)szTest1[i]);

	EncodeBlock (szTest1, szTest2, 4);
	printf ("\n\rEncoded Block   = ");
	for (i = 0; i < 8; i++) printf ("%02x ", (unsigned char)szTest2[i]);

	szTest2[random(8)] ^= (unsigned char)(1 << random(7));
	printf ("\n\rCorrupted Block = ");
	for (i = 0; i < 8; i++) printf ("%02x ", (unsigned char)szTest2[i]);

	DecodeBlock (szTest2, szTest3, 8);
	printf ("\n\rDecoded Block   = ");
	for (i = 0; i < 4; i++) printf ("%02x ", (unsigned char)szTest3[i]);
}