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

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include <osmocom/core/utils.h>
#include <osmocom/core/bits.h>

#include "hdlc.h"

#if 0
#define DEBUGP(x, args ...) fprintf(stderr, x, ## args)
#else
#define DEBUGP(x, args ...) do {} while (0)
#endif

static const ubit_t five_ones[] = { 1,1,1,1,1 };

static int append_bit(struct hdlc_proc *hdlc, uint8_t bit, int ignore)
{
	memmove(hdlc->history+1, hdlc->history, sizeof(hdlc->history)-1);
	hdlc->history[0] = bit;
	if (ignore)
		return -1;

	memmove(hdlc->next_outbyte+1, hdlc->next_outbyte, sizeof(hdlc->next_outbyte)-1);
	hdlc->next_outbyte[0] = bit;
	hdlc->num_bits++;

	if (hdlc->num_bits == 8) {
		pbit_t out;
		/* generate one output byte */
		osmo_ubit2pbit_ext(&out, 0, hdlc->next_outbyte, 0, 8, 0);
		hdlc->num_bits = 0;
		return out;
	}

	return -1;
}

static int process_hdlc_bit(struct hdlc_proc *hdlc, uint8_t bit)
{
	int ignore = 0;
	int out, flag = 0;

	DEBUGP("bit=%u, history_in = %s, ", bit, osmo_ubit_dump(hdlc->history, sizeof(hdlc->history)));

	switch (hdlc->state) {
	case STATE_FLAG_WAIT_ZERO:
		if (bit == 0) {
			DEBUGP("F ");
			flag = 1;
			hdlc->state = STATE_PAYLOAD;
		} else {
			hdlc->state = STATE_INIT;
		}
		ignore = 1;
		hdlc->num_bits = 0;
		break;
	default:
		if (!memcmp(five_ones, hdlc->history, sizeof(five_ones))) {
			if (bit == 1) {
				//DEBUGP("F ");
				hdlc->state = STATE_FLAG_WAIT_ZERO;
				ignore = 1;
			} else {
				/* discard bit */
				ignore = 1;
			}
		}
		break;
	}
	out = append_bit(hdlc, bit, ignore);
	DEBUGP("history_out = %s", osmo_ubit_dump(hdlc->history, sizeof(hdlc->history)));
	if (out > 0)
		DEBUGP(", out 0x%02x\n", out);
	else
		DEBUGP("\n");

	if (flag)
		return -123;
	else
		return out;
}

int process_raw_hdlc(struct hdlc_proc *hdlc, uint8_t *data, unsigned int len)
{
	unsigned int i;
	int out;
	static int last_out;

	for (i = 0; i < len; i ++) {
		out = process_hdlc_bit(hdlc, data[i]);
		if (out == -123) {
			/* suppress repeating Flag characters */
			if (last_out != out)
				printf("\nF ");
			last_out = out;
		} else if (out >= 0) {
			/* suppress 0xAA and 0x55 bit pattern */
			if (out != 0xaa && out != 0x55)
				printf("%02x ", out);
			last_out = out;
		}
	}
	return 0;
}