#include <string.h>

#include "interleave.h"

/* ---------------------------------------------------------------------- */

struct ileave {
	int pipe0;
	int pipe1;
	int pipe2;
	int pipe3;
};

static struct ileave interleaver[10];

static int do_interleave(struct ileave *i, int bits)
{
	i->pipe0 = (i->pipe0 << 1) | ((bits >> 0) & 1);
	i->pipe1 = (i->pipe1 << 1) | ((bits >> 1) & 1);
	i->pipe2 = (i->pipe2 << 1) | ((bits >> 2) & 1);
	i->pipe3 = (i->pipe3 << 1) | ((bits >> 3) & 1);

	bits  = (i->pipe0 >> 3) & 1;
	bits |= (i->pipe1 >> 1) & 2;
	bits |= (i->pipe2 << 1) & 4;
	bits |= (i->pipe3 << 3) & 8;

	return bits;
}

void init_interleaver(void)
{
	int i;

	for (i = 0; i < 10; i++) {
		interleaver[i].pipe0 = 0;
		interleaver[i].pipe1 = 0;
		interleaver[i].pipe2 = 0;
		interleaver[i].pipe3 = 0;
	}
}

int interleave(int in)
{
	int i;

	for (i = 0; i < 10; i++) {
		in = do_interleave(&interleaver[i], in);
	}

	return in;
}

/* ---------------------------------------------------------------------- */

struct deileave {
	unsigned char pipe0[4];
	unsigned char pipe1[4];
	unsigned char pipe2[4];
	unsigned char pipe3[4];
};

static struct deileave deinterleaver[10];

static void deinlv(struct deileave *i, unsigned char *p)
{
	int n;

	for (n = 0; n < 3; n++) {
		i->pipe0[n] = i->pipe0[n + 1];
		i->pipe1[n] = i->pipe1[n + 1];
		i->pipe2[n] = i->pipe2[n + 1];
		i->pipe3[n] = i->pipe3[n + 1];
	}

	i->pipe0[3] = p[0];
	i->pipe1[3] = p[1];
	i->pipe2[3] = p[2];
	i->pipe3[3] = p[3];

	p[0] = i->pipe0[0];
	p[1] = i->pipe1[1];
	p[2] = i->pipe2[2];
	p[3] = i->pipe3[3];
}

void init_deinterleaver(void)
{
	int i;

	for (i = 0; i < 10; i++)
		memset(&deinterleaver[i], 0, sizeof(struct deileave));
}

void deinterleave(unsigned char *syms)
{
	int i;

	for (i = 0; i < 10; i++) {
		deinlv(&deinterleaver[i], syms);
	}
}

/* ---------------------------------------------------------------------- */
