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

struct lzs_state {
	uint8_t *srcblkstart;
	uint8_t *src;
	uint32_t srcsize;

	uint8_t *dstblkstart;
	uint8_t *dst;
	uint32_t dstsize;

	uint32_t bitbuf;
	uint32_t bitcnt;
};

static uint32_t get_bits(struct lzs_state *state, int num)
{
	while (state->bitcnt < num) {
		state->bitbuf = (state->bitbuf << 8) | *(state->src)++;
		state->bitcnt += 8;
	}

	state->bitcnt -= num;
	return (state->bitbuf >> state->bitcnt) & ((1 << num) -1);
}

static uint32_t get_len(struct lzs_state *state)
{
	uint32_t bits;
	uint32_t length = 2;

	do {
		bits = get_bits(state, 2);
		length += bits;
	} while ((bits == 3) && (length < 8));

	if (length == 8) {
		do {
			bits = get_bits(state, 4);
			length += bits;
		} while (bits == 15);
	}

	return length;
}

static int get_zyxel_header(struct lzs_state *state)
{
	uint8_t *p = state->srcblkstart;
	uint32_t a = (p[0] << 24) | (p[1] << 16) | (p[2] << 8) | p[3];

	uint32_t b = ((state->dst - state->dstblkstart) << 16) & 0xFFFF0000;
	b |= ((state->src - state->srcblkstart) & 0xFFFF);

	/* remove own header */
	b -= 4;

	printf("header of previous block is=0x%08x expected=0x%08x %s\n", a, b, ((a == b) ? "OK" : "ERROR"));
	return 0;
}

/*
 * TODO: check src/dst sizes
 */
uint32_t lzs_unpack(uint8_t *srcbuf, uint32_t srcsize, uint8_t *dstbuf, uint32_t dstsize)
{
	struct lzs_state state = {
		.srcblkstart = srcbuf,
		.src = srcbuf,
		.srcsize = srcsize,

		.dstblkstart = dstbuf,
		.dst = dstbuf,
		.dstsize = dstsize,

		.bitbuf = 0,
		.bitcnt = 0,
	};

	while (1) {
		/* jump over header */
		if (state.srcblkstart == state.src)
			state.src += 4;

		uint32_t tag = get_bits(&state, 1);

		/* Uncompressed byte */
		if (tag == 0) {
			*(state.dst)++ = get_bits(&state, 8);
//			printf("uncompressed byte: 0x%02x\n", *(state.dst -1));

		/* Compressed string */
		} else {
			/* read 7 or 11 bit offset */
			tag = get_bits(&state, 1);
			uint32_t offset = get_bits(&state, (tag == 1) ? 7 : 11);

			/* end condition (7bit offset == 0x00) */
			if (tag == 1 && offset == 0) {
				/* align src to next byte */
				uint32_t cnt = state.bitcnt;
				uint32_t tmp = get_bits(&state, cnt);

//				printf("=== BLOCK END (align=%d bits=0x%x) === \n", cnt, tmp);
				get_zyxel_header(&state);
				state.srcblkstart = state.src;
				state.dstblkstart = state.dst;

				/* all src bytes used? */
				if (state.src >= srcbuf + srcsize)
					break;

				continue;
			}

			uint8_t *dict = state.dst - offset;
			if (dict < dstbuf) {
				printf("lzs_unpack: invalid dict: %p < %p (tag=%d, offset=0x%x)\n",
					dict, dstbuf, tag, offset);
				break;
			}

			uint32_t len = get_len(&state);
//			printf("compressed string, offset(%d)=0x%03x len=0x%04x\n", tag, offset, len);

			while (len--)
				*(state.dst)++ = *dict++;
		}
	}

	printf("lzs_unpack: decompressed %d (%d) bytes to %d (%d) bytes\n",
		(state.src - srcbuf), srcsize, (state.dst - dstbuf), dstsize);

	return state.dst - dstbuf;
}