/*
Copyright (C) 2013 by Brendan G Bohannon
Email: cr88192@gmail.com
Copying: http://pastebin.com/iJxtZHm6
 */

/*
Experimental block-based audio codec.
Encodes blocks of 64 samples into 256 bits (32 bytes).
At 44.1kHz this is 176kbps.
It can encode stereo using a "naive joint stereo" encoding.

Basic Format:
    16 bit min sample (center)
    16 bit max sample (center)
    8 bit left-center min (truncated)
    8 bit left-center max
    currently unused (16 bits)
    64 Samples, 1 bits/sample (64 bits)
    16x 4-bit min (64 bits)
    16x 4-bit max (64 bits)

The 4-bit values interpolate between the full min/max for the block.
The 1-bit samples select between the min and max value for each sample.

Note: Interpolated values are linear, thus 0=0/15, 1=1/15, 2=2/15, ..., 14=14/15, 15=15/15

Bit packing is in low-high order, and multibyte values are little-endian.
 */

#include <bgbtac.h>

void BGBTAC_EncodeBlock8MonoS16(s16 *iblk, byte *oblk)
{
	int tcma[16], tcna[16];
	int tcmb[16], tcnb[16];
	int lm, ln, cm, rm, rn, cn, tl, tr, tc;
	int cm2, cn2, ca2;
	int p0, p1, p2, p3;
	int s0, s1;
	int i, j, k;

	lm= 99999; rm= 99999; cm= 99999;
	ln=-99999; rn=-99999; cn=-99999;
	
	for(i=0; i<64; i++)
	{
		tl=iblk[i]; tr=tl;
		k=(tl+tr)>>1;
		if(k<cm) { lm=tl; rm=tr; cm=k; }
		if(k>cn) { ln=tl; rn=tr; cn=k; }
	}
	
	for(i=0; i<32; i++)
		{ oblk[i]=0; }
	
	oblk[0]=cm&0xFF;
	oblk[1]=(cm>>8)&0xFF;
	oblk[2]=cn&0xFF;
	oblk[3]=(cn>>8)&0xFF;
	oblk[4]=((lm-rm+128)>>8)&0xFF;
	oblk[5]=((ln-rn+128)>>8)&0xFF;
	
	for(i=0; i<16; i++)
	{
		cm2= 99999; cn2=-99999;
		for(j=0; j<4; j++)
		{
			tl=iblk[i*4+j]; tr=tl;
			tc=(tl+tr)>>1;
			if(tc<cm2) { cm2=tc; }
			if(tc>cn2) { cn2=tc; }
		}
		
		if(cn!=cm)
			{ s0=16777216/(cn-cm); }
		else { s0=0; }

		p0=(s0*(cm2-cm)+524288)>>20;
		p1=(s0*(cn2-cm)+524288)>>20;
		
		p0=(p0<0)?0:((p0<16)?p0:15);
		p1=(p1<0)?0:((p1<16)?p1:15);
		
		tcma[i]=p0;
		tcna[i]=p1;

		s1=(cn-cm);
		cm2=((s1*p0+8)>>4)+cm;
		cn2=((s1*p1+8)>>4)+cm;

		tcmb[i]=cm2;
		tcnb[i]=cn2;
	}

	for(i=0; i<16; i++)
	{
		oblk[16+(i>>1)]|=tcma[i]<<((i&1)*4);
		oblk[24+(i>>1)]|=tcna[i]<<((i&1)*4);
	}
	
	for(i=0; i<16; i++)
	{
		ca2=(tcmb[i]+tcnb[i])>>1;
	
		for(j=0; j<4; j++)
		{
			k=i*4+j;
		
			tl=iblk[k]; tr=tl;
//			tl=(iblk[k]+7)>>4; tr=tl;
			tc=(tl+tr)>>1;

			if(tc>=ca2)
				{ oblk[8+(k>>3)]|=1<<(k&7); }
			else
				{ oblk[8+(k>>3)]&=~(1<<(k&7)); }
		}
	}	
}

void BGBTAC_DecodeBlock8MonoS16(byte *iblk, s16 *oblk)
{
	int tcma[16], tcna[16];
	int tcmb[16], tcnb[16];
	int p0, p1, p2, p3;
	int cm, cn, ca;
	int cm2, cn2, ca2;
	int i, j, k;
	
	cm=(s16)(iblk[0]+(iblk[1]<<8));
	cn=(s16)(iblk[2]+(iblk[3]<<8));
	ca=(cm+cn)>>1;

	for(i=0; i<128; i++)
		{ oblk[i]=ca; }
		
	for(i=0; i<16; i++)
	{
		cm2=(iblk[16+(i>>1)]>>((i&1)*4))&15;
		cn2=(iblk[24+(i>>1)]>>((i&1)*4))&15;
//		cm2=tcmb[i];
//		cn2=tcnb[i];
	
		j=(cn-cm);
		p0=((j*cm2+8)>>4)+cm;
		p1=((j*cn2+8)>>4)+cm;

		p0=(p0<cm)?cm:((p0<cn)?p0:cn);
		p1=(p1<cm)?cm:((p1<cn)?p1:cn);
	
		tcma[i]=p0;
		tcna[i]=p1;
		
		p2=(p0+p1)>>1;
//		if(p2<cm)p2=cm;
//		if(p2>cn)p2=cn;
		
		for(j=0; j<4; j++)
		{
			k=i*4+j;
		
//			oblk[i*16+j]=p2;
			oblk[i*4+j]=(iblk[8+(k>>3)]&(1<<(k&7)))?p1:p0;
		}
	}
}

BTAC_API void BGBTAC_EncodeStream8MonoS16(s16 *iblk, byte *oblk, int len)
{
	int i, j, n;
	
	n=(len+63)/64;
	for(i=0; i<n; i++)
	{
		BGBTAC_EncodeBlock8MonoS16(iblk+i*64, oblk+i*32);
	}
}

BTAC_API void BGBTAC_DecodeStream8MonoS16(byte *iblk, s16 *oblk, int len)
{
	int i, j, n;
	
	n=(len+63)/64;
	for(i=0; i<n; i++)
	{
		BGBTAC_DecodeBlock8MonoS16(iblk+i*32, oblk+i*64);
	}
}



void BGBTAC_EncodeBlock8StereoS16(s16 *iblk, byte *oblk)
{
	s16 tcblk[64];
	int tcma[16], tcna[16];
	int tcmb[16], tcnb[16];
	int lm, ln, cm, rm, rn, cn, tl, tr, tc;
	int cm2, cn2, ca2, ba;
	int p0, p1, p2, p3;
	int s0, s1;
	int i, j, k;

	lm= 99999; rm= 99999; cm= 99999;
	ln=-99999; rn=-99999; cn=-99999;
	
	ba=0;
	for(i=0; i<64; i++)
	{
		tl=iblk[i*2+0];
		tr=iblk[i*2+1];
		k=(tl+tr)>>1;
		tcblk[i]=k;
		ba+=tl-k;
		if(k<cm) { lm=tl; rm=tr; cm=k; }
		if(k>cn) { ln=tl; rn=tr; cn=k; }
	}

	ba/=64;
	
	for(i=0; i<32; i++)
		{ oblk[i]=0; }

	
	p0=(lm-cm+128)>>8;
	p1=(ln-cn+128)>>8;
	p2=(ba+128)>>8;
	p0=(5*p0+11*p2)>>4;
	p1=(5*p1+11*p2)>>4;
//	p0=p2; p1=p2;

	p0=(p0<0)?0:((p0<256)?p0:255);
	p1=(p1<0)?0:((p1<256)?p1:255);
	
	oblk[0]=cm&0xFF;
	oblk[1]=(cm>>8)&0xFF;
	oblk[2]=cn&0xFF;
	oblk[3]=(cn>>8)&0xFF;
//	oblk[4]=((lm-cm+128)>>8)&0xFF;
//	oblk[5]=((ln-cn+128)>>8)&0xFF;

	oblk[4]=p0&0xFF;
	oblk[5]=p1&0xFF;
	
	for(i=0; i<16; i++)
	{
		cm2= 99999; cn2=-99999;
		for(j=0; j<4; j++)
		{
			tc=tcblk[i*4+j];
			if(tc<cm2) { cm2=tc; }
			if(tc>cn2) { cn2=tc; }
		}
		
		if(cn!=cm)
			{ s0=16777216/(cn-cm); }
		else { s0=0; }

		p0=(s0*(cm2-cm)+524288)>>20;
		p1=(s0*(cn2-cm)+524288)>>20;

//		p0=(s0*(cm2-cm)+262144)>>20;
//		p1=(s0*(cn2-cm)+786432)>>20;
		
		p0=(p0<0)?0:((p0<16)?p0:15);
		p1=(p1<0)?0:((p1<16)?p1:15);
		
		tcma[i]=p0;
		tcna[i]=p1;

		s1=(cn-cm);
		cm2=((s1*p0+8)>>4)+cm;
		cn2=((s1*p1+8)>>4)+cm;

		tcmb[i]=cm2;
		tcnb[i]=cn2;
	}

	for(i=0; i<16; i++)
	{
		oblk[16+(i>>1)]|=tcma[i]<<((i&1)*4);
		oblk[24+(i>>1)]|=tcna[i]<<((i&1)*4);
	}
	
	for(i=0; i<16; i++)
	{
		ca2=(tcmb[i]+tcnb[i])>>1;
	
		for(j=0; j<4; j++)
		{
			k=i*4+j;
			tc=tcblk[k];
			if(tc>=ca2)
				{ oblk[8+(k>>3)]|=1<<(k&7); }
			else
				{ oblk[8+(k>>3)]&=~(1<<(k&7)); }
		}
	}	
}

void BGBTAC_DecodeBlock8StereoS16(byte *iblk, s16 *oblk)
{
//	int tcma[16], tcna[16];
//	int tcmb[16], tcnb[16];
	int p0, p1, p2, p3;
	int s0, s1;
	int cm, cn, ca;
	int lm, ln, la;
	int rm, rn, ra;
	int cm2, cn2, ca2;
	int lm2, ln2, la2;
	int rm2, rn2, ra2;
	int i, j, k;
	
	cm=(s16)(iblk[0]+(iblk[1]<<8));
	cn=(s16)(iblk[2]+(iblk[3]<<8));
	ca=(cm+cn)>>1;

	i=((s16)(iblk[4]<<8));
	j=((s16)(iblk[5]<<8));

	lm=cm+i; rm=cm-i;
	ln=cn+j; rn=cn-j;

	lm=(lm<(-32768))?(-32768):((lm<32768)?lm:32767);
	ln=(ln<(-32768))?(-32768):((ln<32768)?ln:32767);
	rm=(rm<(-32768))?(-32768):((rm<32768)?rm:32767);
	rn=(rn<(-32768))?(-32768):((rn<32768)?rn:32767);

	for(i=0; i<128; i++)
	{
		oblk[i]=ca;
	}
		
	for(i=0; i<16; i++)
	{
		p0=(iblk[16+(i>>1)]>>((i&1)*4))&15;
		p1=(iblk[24+(i>>1)]>>((i&1)*4))&15;
//		cm2=tcmb[i];
//		cn2=tcnb[i];

//		s0=(cn-cm);
//		s1=(cn-cm);
	
		s0=(ln-lm);
		s1=(rn-rm);
		lm2=((s0*p0+8)>>4)+lm;
		ln2=((s0*p1+8)>>4)+lm;
		rm2=((s1*p0+8)>>4)+rm;
		rn2=((s1*p1+8)>>4)+rm;

//		cm2=((s1*p0+8)>>4)+cm;
//		cn2=((s1*p1+8)>>4)+cm;

		lm2=(lm2<lm)?lm:((lm2<ln)?lm2:ln);
		ln2=(ln2<lm)?lm:((ln2<ln)?ln2:ln);
		rm2=(rm2<rm)?rm:((rm2<rn)?rm2:rn);
		rn2=(rn2<rm)?rm:((rn2<rn)?rn2:rn);

//		cm2=(cm2<cm)?cm:((cm2<cn)?cm2:cn);
//		cn2=(cn2<cm)?cm:((cn2<cn)?cn2:cn);
	
//		p2=(p0+p1)>>1;
//		if(p2<cm)p2=cm;
//		if(p2>cn)p2=cn;
		
		for(j=0; j<4; j++)
		{
			k=i*4+j;
		
//			oblk[i*16+j]=p2;
			p0=(iblk[8+(k>>3)]&(1<<(k&7)));
			oblk[k*2+0]=p0?ln2:lm2;
			oblk[k*2+1]=p0?rn2:rm2;

//			oblk[k*2+0]=p0?cn2:cm2;
//			oblk[k*2+1]=p0?cn2:cm2;
		}
	}
}

BTAC_API void BGBTAC_EncodeStream8StereoS16(s16 *iblk, byte *oblk, int len)
{
	int i, j, n;
	
	n=(len+63)/64;
	for(i=0; i<n; i++)
	{
		BGBTAC_EncodeBlock8StereoS16(iblk+i*2*64, oblk+i*32);
	}
}

BTAC_API void BGBTAC_DecodeStream8StereoS16(byte *iblk, s16 *oblk, int len)
{
	int p0, p1, p2, p3;
	int q0, q1, q2, q3;
	int i, j, n;
	
	n=(len+63)/64;
	for(i=0; i<n; i++)
	{
		BGBTAC_DecodeBlock8StereoS16(iblk+i*32, oblk+i*2*64);
	}
	
	//deblocking...
	for(i=1; i<(n-1); i++)
		for(j=0; j<2; j++)
	{
#if 0
		p0=oblk[i*64*2-2];
		p1=oblk[i*64*2-1];
		p2=oblk[i*64*2+0];
		p3=oblk[i*64*2+1];

		q0=(11*p0+5*p2)>>4;
		q1=(11*p1+5*p3)>>4;
		q2=(11*p2+5*p0)>>4;
		q3=(11*p3+5*p1)>>4;
		
		oblk[i*64*2-2]=q0;
		oblk[i*64*2-1]=q1;
		oblk[i*64*2+0]=q2;
		oblk[i*64*2+1]=q3;
#endif

#if 1
		p0=oblk[i*64*2-4+j];
		p1=oblk[i*64*2-2+j];
		p2=oblk[i*64*2+0+j];
		p3=oblk[i*64*2+2+j];

		q0=(128*p0+ 64*p1+ 48*p2+ 16*p3)>>8;
		q1=( 48*p0+128*p1+ 48*p2+ 32*p3)>>8;
		q2=( 32*p0+ 48*p1+128*p2+ 48*p3)>>8;
		q3=( 16*p0+ 48*p1+ 64*p2+128*p3)>>8;
		
		oblk[i*64*2-4+j]=q0;
		oblk[i*64*2-2+j]=q1;
		oblk[i*64*2+0+j]=q2;
		oblk[i*64*2+2+j]=q3;
#endif
	}
}