#include <stdio.h>
#include <string.h>
#include <stdint.h>

/* display an array (8 bytes per line) */
void display( FILE *out, const uint8_t data[32], int len )
{
	int j, k;
	
	for(k=0; k<len; )
	{
		for(j=0; (j<8) && (k<len); ++j)
		{
			fprintf(out, "%02x ", data[k++]);
		}
		fprintf(out, "\n");
	}
	
	fprintf(out, "\n");
}

/* decode data */
void decode( const uint8_t header[4], const uint8_t *src, uint8_t dest[32] )
{
	int i, j, k, l;
	uint8_t mask;
	
	/* RLE decoding */
	/* if bit=1 we output the next byte of the compressed data */
	/* else     we output 0 */
	for(i=0, k=0, l=0; i<4; ++i)
	{
		mask = header[i];
		for(j=0; j<8; ++j, ++k)
		{
			if(mask & 0x01)
			{
				dest[k] = src[l++];
			}
			else
			{
				dest[k] = 0;
			}
			
			mask >>= 1;
		}
	}
	
	/* Perform interleaved xor fill */
	for(i=0; i<14; i+=2)
	{
		dest[i+ 2] ^= dest[i   ];
		dest[i+ 3] ^= dest[i+ 1];
		dest[i+18] ^= dest[i+16];
		dest[i+19] ^= dest[i+17];
	}
}

/* encode data and generate the associated 4 bytes header */
void encode( const uint8_t src[32], uint8_t header[4], uint8_t *dest, int *compressed_len )
{
	int i, j, k;
	uint8_t tmp[32];
	
	/* interleaved xor. This will if 2 interleaved bytes are equals
	 * we will get 0 and it will be compacted afterwards. */
	memcpy(tmp, src, 32);
	for(i=0; i<14; i+=2)
	{		
		tmp[i+ 2] = src[i+ 2] ^ src[i   ];
		tmp[i+ 3] = src[i+ 3] ^ src[i+ 1];
		tmp[i+18] = src[i+18] ^ src[i+16];
		tmp[i+19] = src[i+19] ^ src[i+17];
	}
	
	/* 0 byte compactation. The header is a bitmask where a bit equals
	 * to 1 means that the byte and non null and must be fetched from
	 * the compressed data array. */
	for(i=0, k=0, *compressed_len=0; i<4; ++i)
	{
		header[i] = 0;
		for(j=0; j<8; ++j, ++k)
		{
			if(tmp[k])
			{
				header[i] |= (1 << j);
				dest[*compressed_len]  = tmp[k];
				++(*compressed_len);
			}
		}
	}
}

int main()
{
	uint8_t orig_header[4] = 
	{
		0xAA, 0x7A, 0xEB, 0xBB 
	};
	
	uint8_t orig_data[32] = 
	{
		0x00, 0xD0, 0x00, 0x00, 0x00, 0x04, 0x00, 0x18,
		0x00, 0x60, 0x00, 0x8F, 0x80, 0x0F, 0xC0, 0x0F,
		0xE0, 0x0F, 0xE0, 0x4F, 0xE0, 0xCF, 0xC0, 0x3F,
		0x80, 0x0F, 0x80, 0x8F, 0x00, 0x0F, 0x00, 0x8C
	};
	
	uint8_t orig_compressed[] =
	{
		0xD0, 0xD0, 0x04, 0x1C, 0x78, 0xEF, 0x80, 0x80,
		0x40, 0xE0, 0x0F, 0x40, 0x80, 0x20, 0xF0, 0x40,
		0x30, 0x80, 0x80, 0x80, 0x83
	};
	
	uint8_t data[32], header[4], compressed[32];
	int compressed_len;
	
	/* Decode data */
	decode( orig_header, orig_compressed, data );
	/* And test if it succeeded */
	if( memcmp( orig_data, data, 32 ) )
	{
		fprintf(stderr, "[ERROR] decoding failed!\n\n");
		fprintf(stderr, "orig:\n");
		display(stderr, orig_data, 32);
		
		fprintf(stderr, "computed:\n");
		display(stderr, data, 32);
		
		return 1;
	}
	else
	{
		printf("[SUCCESS] decoding succeeded!\n");
	}
	
	/* Encode data */
	encode( orig_data, header, compressed, &compressed_len );
	/* And test if it didn't failed */
	if( memcmp( orig_header, header, 4 ) )
	{
		fprintf(stderr, "[ERROR] encoding failed!\n");
		fprintf(stderr, "        header mismatch!\nexpected: ");
		fprintf(stderr, "%02x %02x %02x %02x\n",
		        orig_header[0], orig_header[1],
		        orig_header[2], orig_header[3]);
		fprintf(stderr, "result  : %02x %02x %02x %02x\n",
		        header[0], header[1],
		        header[2], header[3]);
		return 1;
	}
	else
	{
		if( compressed_len != (sizeof( orig_compressed ) / sizeof( orig_compressed[0])) )
		{
			fprintf(stderr, "[ERROR] encoding failed!\n");
			fprintf(stderr, "        compressed data size mismatch!\n");
			fprintf(stderr, "expected: %d\nresult  : %d\n",
			        sizeof( orig_compressed ) / sizeof( orig_compressed[0]),
			        compressed_len ); 
			return 1;
		}
		else if( memcmp( orig_compressed, compressed, compressed_len ) )
		{
			fprintf(stderr, "[ERROR] encoding failed!\n");
			fprintf(stderr, "        compressed data mismatch!\n");
			fprintf(stderr, "orig:\n");
			display(stderr, orig_compressed, compressed_len);
			
			fprintf(stderr, "computed:\n");
			display(stderr, compressed, compressed_len);
			
			return 1;
		}
		
		printf("[SUCCESS] encoding succeeded!\n");
	}
	
	return 0;
}