#include <stdint.h>
#include <stdlib.h>
#include "twp.h"

int wrapBuffer(unsigned char *outBuf,unsigned char *inBuf,int len)
{
	unsigned char *ob;
	unsigned char ctlBits;
	int hh,p,xx;

	/* pad with 16 zero bytes */
	for (p=0;p<20;p++)
		inBuf[len+p]=0;

	ob=outBuf;
	p=0;xx=0;
	ctlBits=0;
	hh=0;
	while (p<len || hh!=0)
	{
		if (hh==0 && (xx&0x31) == 0) {
			/* output fullsync at start of frame */
			ob[0]=0xFF;
			ob[1]=0xFF;
			ob[2]=0xFF;
			ob[3]=0x7F;
			ob+=4;
		}
		if ( (xx&0xFE)==0 || p==len || ( hh<14 && p+1 == len ) ) {
			/*
			   output ID, either because forced (xx&0xFE) == 0,
			   or because we reached end of inBuf (p==len  or  h<14 && p+1==len)
			*/
			ctlBits>>=1;

			/* output ID */
			if (p+1>=len)
				ob[hh  ] = 0x1;      /* ID == 0 (dummy zero source) */
			else
				ob[hh  ] = 0x9;      /* ID == 4 */

			if (hh<14) {
				/* output data/ID pair */
				ob[hh+1] = inBuf[p]; /* data */

				/* use data,ID if we have exactly one valid data byte left (p+1==len) or       */
				/* we force the swap (xx&0x100) and we are not exactly at a ID switch (p!=len) */
				if ( p+1==len || (xx&0x100 && p!=len) )
					ctlBits|=0x80;
				hh+=2;
				p++;
				if ( p==len )
					p++;
			} else {
				hh++;
			}
			xx+=2;
		} else {
			ctlBits>>=1;
			if (inBuf[p]&0x1)
				ctlBits|=0x80;
			ob[hh  ] = (inBuf[p]&0xFE);
			if (hh<14) {
				/* output data,data */
				ob[hh+1] = inBuf[p+1];
				hh+=2;
				p+=2;
				xx+=2;
			} else {
				/* just output data */
				hh++;
				p++;
				xx++;
			}
		}
		if (hh>=15) {
			ob[15]=(unsigned char)ctlBits;
			/* write 0x7FFFFFFF, write outBuf */
			ob+=16;
			ctlBits=0;
			hh=0;
		}
	}
	return (int)(ob-outBuf);
}

int wrapMultiBuffers(uint8_t *outBuf,bufferP *buffers)
{
	uint8_t *ob;
	uint8_t ctlBits,ch;
	uint8_t *charPtrs[128];

	int      frameLen;
	int      frameCnt;
	int      i;
	int      bufNum;
	int      curLen;
	uint8_t *curChar;

	ob=outBuf;
	bufNum=0;
	curLen=0;
	curChar=NULL;
	if (buffers[bufNum]!=NULL) {
		while (buffers[bufNum]!=NULL) {
			charPtrs[bufNum]=buffers[bufNum]->buf;
			bufNum++;
		}
		bufNum--;
		curChar = charPtrs[bufNum];
		curLen  = buffers[bufNum]->len - (int)(curChar - buffers[bufNum]->buf);
	}
	i=0;
	frameLen=0;
	frameCnt=0;
	ctlBits=0;
	for(;;) {
		if (frameLen==0 && (frameCnt&0xF)==0) {
			ob[0]=0xFF;
			ob[1]=0xFF;
			ob[2]=0xFF;
			ob[3]=0x7F;
			ob+=4;
		}
		if (frameLen==15) {
			/* output ctlBits */
			*ob=ctlBits;
			ob++;
			frameLen=0;
			ctlBits=0;
			frameCnt++;
			continue;
		}

		if (i==0 || (curChar!=NULL && curLen<=0)) {
			int atbid;
			atbid=0;
			if (curChar) {
				int nextBufNum;
				charPtrs[bufNum]=curChar;
				nextBufNum=bufNum+1;
				while (nextBufNum!=bufNum) {
					if (buffers[nextBufNum]==NULL) {
						nextBufNum=0;
						continue;
					}
					curChar = charPtrs[nextBufNum];
					curLen  = buffers[nextBufNum]->len - (int)(curChar - buffers[nextBufNum]->buf);
					if (curLen>0)
						break;
					nextBufNum++;
				}
				if (nextBufNum==bufNum) {
					curChar = charPtrs[nextBufNum];
					curLen  = buffers[nextBufNum]->len - (int)(curChar - buffers[nextBufNum]->buf);
				}
				if (curLen==0) {
					/* change to NULL source if there is no more data */
					curChar=NULL;
				} else if (buffers[nextBufNum]!=NULL) {
					atbid=buffers[nextBufNum]->atbid;
					bufNum=nextBufNum;
				}
			}
			atbid<<=1;
			atbid|=1;
			if ( frameLen&0x1 ) {
				/* uneven byte of frame. need to swap with previous byte */
				/* get previous byte */
				ch=*(ob-1);
				if (ctlBits&0x80)
					ch|=1;
				/* mark that ID was swapped with data byte */
				ctlBits|=0x80;
				*(ob-1)=(uint8_t)atbid;
				*ob=ch;
			} else {
				/* even byte of frame, output ID here */
				*ob = (uint8_t)atbid;
				ctlBits>>=1; /* ID not swapped with data byte */
			}
			ob++;
			frameLen++;
			i=20; /* swap buffers every 20 byte */
			continue;
		}
		if (frameLen==0 && curChar==NULL)
			break; /* all data was output */

		/* default NULL source */
		ch=0;
		/* get data from real source if not already finished */
		if (curChar) {
			ch=*curChar;
			curChar++;
			curLen--;
		}
		if ( (frameLen&0x1)==0 ) {
			/* even byte in frame, remember ctlBit */
			ctlBits>>=1;
			if (ch&0x1)
				ctlBits|=0x80;
			/* remove lowest bit of data */
			ch&=0xFE;
		}
		/* store byte to frame */
		*ob=ch;
		ob++;
		frameLen++;
		i--;
	}
	return (int)(ob - outBuf);
}

#if defined(TESTME)
#include <stdio.h>

static bufferT buf1,buf2,buf3;
static uint8_t outBuf[2048*16];

int main()
{
	bufferP buffers[4];
	FILE *fp;
	int i,h;
	fp=fopen("twp_test.bin","wb");
	if (fp==NULL) {
		fprintf(stderr,"Could not open twp_test.bin\n");
		return 1;
	}
	buf1.atbid=4;
	buf2.atbid=5;
	buf3.atbid=6;
	for (i=0;i<512;i++) {
		buf1.buf[i]=(i&0x3F);
		buf2.buf[i]=(i&0x3F)|0x40;
		buf3.buf[i]=(i&0x3F)|0x80;
	}
	buf1.len=512;
	buf2.len=512;
	buf3.len=512;

	buffers[0]=&(buf1);
	buffers[1]=&(buf2);
	buffers[2]=&(buf3);
	buffers[3]=NULL;

	h=wrapMultiBuffers(outBuf,buffers);
	printf("Wrapped to %d output bytes\n",h);
	fwrite(outBuf,1,h,fp);
	fclose(fp);
	return 0;
}
#endif