#include <naive.h>


static const uint64_t B[] = {
	0x5555555555555555ULL,
	0x3333333333333333ULL,
	0x0f0f0f0f0f0f0f0fULL,
	0x00ff00ff00ff00ffULL,
	0x0000ffff0000ffffULL
};

static const uint64_t S[] = {
	1,
	1<<1,
	1<<2,
	1<<3,
	1<<4
};

/**
 * Zero Morton.
 *
 * "Traditional" bit-wise number interleaving are called Morton numbers, but in
 * our case, one of the numbers is _always_ zero
 */
uint64_t zmort(uint32_t n)
{
	uint64_t x = u64(n);
	int i;

	for (i = 4; i >= 0; i--)
		x = (x | (x << S[i])) & B[i];

	return x << 1;
}

int naive_stream_init(naive_stream *strm)
{
	strm_clear_buf(strm);
	return NAIVE_OK;	/* Nothing to do */
}

static int naive_put_data(naive_stream *strm)
{
	memcpy(strm->outp, strm->buf, strm->buf_avail);
	strm->total_out += strm->buf_avail;
	strm->avail_out -= strm->buf_avail;

	strm_clear_buf(strm);
	return NAIVE_OK;
}

int naive_encode(naive_stream *strm)
{
	if (!strm->avail_in)
		return NAIVE_OK;

	if (strm->avail_in * 2 > strm->avail_out)
		return NAIVE_NO_BUF_SPC;

	blk buf;
	int i;
	uint64_t enc[8] = {0, 0, 0, 0, 0, 0, 0, 0};
	for (; strm->avail_in >= BLKSIZ; strm->avail_in -= BLKSIZ, strm->total_in += BLKSIZ) {
		if (strm_buf_have_data(strm))
			naive_put_data(strm);

		memcpy(&buf, strm->inp, BLKSIZ);
		for (i = 0; i < 4; i++) {
			enc[i<<1]     = zmort(lo(buf.x[i]));
			enc[(i<<1)+1] = zmort(hi(buf.x[i]));
		}
		memcpy(strm->buf, enc, BLKSIZ*2);
		strm_set_buf(strm, BLKSIZ*2);
	}

	/* Less than 256 bits left */
	if (strm->avail_in > 0 && strm_buf_have_data(strm))
		naive_put_data(strm);


	byte b;
	unsigned int r;
	i = 0;
	for (; strm->avail_in > 0; strm->avail_in--, strm->total_in++) {
		b = *strm->inp++;
		r = (unsigned int)zmort((uint32_t)b);

		strm->buf[i++] = (byte)(r >> 8);
		strm->buf[i++] = (byte)(r & 0x7);
	}
	strm_set_buf(strm, (byte)i);
	return NAIVE_OK;
}

/**
 * We only keep track of the bytes, so in this case make sure the last byte has
 * either the second-highest bit set or the second bit set (not sure which is "last")
 */
int naive_encode_end(naive_stream *strm)
{
	if (!strm_buf_have_data(strm))
		/* For some reason the data has not been buffered? */
		return NAIVE_ERRNO;

	int i = strm->buf_avail - 1;
	strm->buf[i] |= 0x100;
	return naive_put_data(strm);
}

/* int naive_encode_file(naive_stream *s, FILE *in) */
/* { */
/* 	blk256_t db; */
/* 	size_t n, n_prev; */

/* 	memset(db.x, 0, BLKSIZ); */

/* 	while ((n = fread(db.x, BLKSIZ, 1, in)) > 0) { */
/* 		naive_encode_blk256(s, &db); */
/* 		write(s->out, s->buf, sizeof(uint64_t)<<3); */
/* 		memset(db.x, 0, BLKSIZ); */
/* 		n_prev = n; */
/* 	} */
/* 	printf("prev block: %lu\n", n_prev); */
/* 	printf("last block: %lu\n", n); */
/* 	naive_encode_blk256(s, &db); */
/* 	write(s->out, s->buf, n); */

/* 	return 0; */
/* } */