/*
 * Copyright (c) 2006, John P. T. Moore
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without 
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, 
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, 
 *    this list of conditions and the following disclaimer in the documentation 
 *    and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the author nor the names of its contributors may be 
 *    used to endorse or promote products derived from this software 
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 * POSSIBILITY OF SUCH DAMAGE.
*/

#include <stdio.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <string.h>
#include "encode.h"

#define WORD_BIT 32
#define WORD_BYTE 4

#ifndef CHAR_BIT
#define CHAR_BIT  8
#endif

unsigned mask32[] = {
	0,					
	~(~0<<1),					
	~(~0<<2),					
	~(~0<<3),					
	~(~0<<4),					
	~(~0<<5),					
	~(~0<<6),					
	~(~0<<7),				
	~(~0<<8),				
	~(~0<<9),				
	~(~0<<10),				
	~(~0<<11),				
	~(~0<<12),				
	~(~0<<13),				
	~(~0<<14),				
	~(~0<<15),				
	~(~0<<16),				
	~(~0<<17),				
	~(~0<<18),				
	~(~0<<19),				
	~(~0<<20),			
	~(~0<<21),			
	~(~0<<22),			
	~(~0<<23),			
	~(~0<<24),			
	~(~0<<25),			
	~(~0<<26),			
	~(~0<<27),			
	~(~0<<28),			
	~(~0<<29),			
	~(~0<<30),			
	~(~0<<31),			
	~0		
};


int bitcount(unsigned int n);
static void addBuf(packedEncode *memBuf, unsigned long int n);
static void addWord(packedEncode *memBuf, unsigned long int n);
static unsigned long int orBuf(packedEncode *memBuf);
static void resetBuf(packedEncode *memBuf);


/* can not calculate for value 0 */
int bitcount (unsigned int n)  {
	int count=0;    
	while (n) {
		count++;
		n >>= 1 ;
	}
	return count ;
}


void dumpBuffer(char *bufName, char *buf, int amount) {
  FILE *fp;
  size_t how_much;
  fp = fopen(bufName, "w");
  how_much = fwrite(buf, 1, amount, fp);         
  fclose(fp);
}

packedEncode *initializeEncode(char *pdu, int size) {
	packedEncode *memBuf;
	
	if ((memBuf = (packedEncode *)malloc(sizeof(packedEncode))) == NULL) {
		perror("failed to malloc packedEncode!");
		exit(EXIT_FAILURE);
	}
	if ((memBuf->buf = calloc(WORD_BIT, WORD_BYTE)) == NULL) {
		perror("failed to calloc buf!");
		exit(EXIT_FAILURE);
	}
	memBuf->pdu = pdu;
	memBuf->bufWords = 0;
	memBuf->size = size; /* in bytes */
	memBuf->bitsUsed = 0;
	memBuf->pduWords = 0;
				    
	return memBuf;
}



int finalizeEncode(packedEncode *memBuf) {
	int totalbytes, bufused; 

	bufused = 0;

	/* add any leftover */
	if (memBuf->bitsUsed > 0) {
		unsigned long int word;
		int buffree;
		word = orBuf(memBuf);
		addWord(memBuf, word);
		buffree = (WORD_BIT - memBuf->bitsUsed) / CHAR_BIT;
		bufused = WORD_BYTE - buffree;
		memBuf->pduWords--;
	}
	totalbytes = (memBuf->pduWords * WORD_BYTE) + bufused;
	free(memBuf->buf);	
	free(memBuf);
	return totalbytes;
}



static void addBuf(packedEncode *memBuf, unsigned long int n) {
	unsigned long int word;
	int offset;
	
	/* make sure is big endian */
	word = htonl(n << (WORD_BIT - memBuf->bitsUsed));

	offset = ((memBuf->bufWords) * WORD_BYTE);
	memcpy((memBuf->buf)+offset, &word, WORD_BYTE);
	memBuf->bufWords++;
}

/* copy word across to the pdu */
static void addWord(packedEncode *memBuf, unsigned long int n) {
	int bytes;
	
	bytes = ((memBuf->pduWords) * WORD_BYTE);
	if (bytes+WORD_BYTE > memBuf->size) {
		perror("encoder out of memory!");
		exit(EXIT_FAILURE);	
	}
	/* copy a word at pdu offset by bytes */
    memcpy((memBuf->pdu)+bytes, &n, WORD_BYTE);
    memBuf->pduWords++;
}

static unsigned long int orBuf(packedEncode *memBuf) {
	unsigned long int result;
	unsigned long int *n;
	int i;
	
	result = 0;
	
	for (i = 0 ; i < memBuf->bufWords ; i++) {
		n = (unsigned long int *)(memBuf->buf + (i * WORD_BYTE));
		result = (result | *n);
	}
	return result;
}

static void resetBuf(packedEncode *memBuf) {
	memset(memBuf->buf, 0, ((memBuf->bufWords) * WORD_BYTE));
	memBuf->bufWords = 0;
	memBuf->bitsUsed = 0;
}

int encode(packedEncode *memBuf, unsigned long int n, int bitlength) {
	int nbits, bytes;
	unsigned long int word;

	bytes = (memBuf->pduWords * WORD_BYTE);
  
	/* superapi should not call this */
	if (bitlength == 0) {
		nbits = bitcount(n);
#ifdef DEBUG		
		printf("bitcount result = %d\n", nbits);
#endif
	} else
		nbits = bitlength;
		
	memBuf->bitsUsed = memBuf->bitsUsed + nbits;
	
	if (memBuf->bitsUsed == WORD_BIT) { /* lucky fit */
		addBuf(memBuf, n);
		word = orBuf(memBuf);
		addWord(memBuf, word);
		resetBuf(memBuf);
	} else if (memBuf->bitsUsed < WORD_BIT) { /* some room */
		addBuf(memBuf, n);
	} else { /* exceeded space available, ie. memBuf->bitsUsed > WORD_BIT */
		int tail;
		tail = (memBuf->bitsUsed - WORD_BIT);		
		/* set the word to be full */
		memBuf->bitsUsed = WORD_BIT;
		/* remove the tail so we just add the head into the gap */
		addBuf(memBuf, (n >> tail));
		word = orBuf(memBuf);
		addWord(memBuf, word);
		resetBuf(memBuf);
		/* encode the leftover tail */
		memBuf->bitsUsed = tail;
		addBuf(memBuf, (n & mask32[tail]));
	}
	return EXIT_SUCCESS;	
}