#include "laplace.h"
#include "basic_op.h"
#include "rc_encode.h"
#include "rc_decode.h"


unsigned rc_laplace_get_freq1(unsigned fs0, int decay)
{

#ifndef HW_HIFI3
	   unsigned ft;
	   ft = 32736 - fs0;
   return ft*(int)(16384-decay)>>15;
#else
   short ft;
   ft = 32736 - fs0;
   return SSC_MULT16x16_Q15(ft,(16384-decay));
#endif
}



void rc_laplace_encode(ec_enc *enc, short *value, unsigned short fs, short decay)
{
	unsigned short fl;
	short val = *value;

	fl = 0;
	if(val)
	{
		short s, i;
		s = -(val<0);
		val = (val+s)^s;
		fl = fs;

		fs = rc_laplace_get_freq1(fs, decay);

		for (i=1; fs > 0 && i < val; i++)
		{
#ifndef HW_HIFI3
			fs *= 2;
			fl += fs+2;
			fs = (fs*(int)decay)>>15;
#else
			fs = fs<<1;
			fl += fs+2;
			fs = SSC_MULT16x16_Q15((short)(fs),decay);
#endif
		}
		/* Everything beyond that has probability LAPLACE_MINP. */
		if (!fs)
		{
			short di, ndi_max;
			ndi_max = (32768-fl);
			ndi_max = (ndi_max-s)>>1;
			di = SPEECH_MIN(val - i, ndi_max - 1);
#ifndef HW_HIFI3
			fl += (2*di+1+s);
#else
			fl += ((di<<1)+1+s);
#endif
			fs = SPEECH_MIN(1, 32768-fl);
			*value = (i+di+s)^s;
		}
		else
		{
			fs++;
			fl += fs&~s;
		}
	}
	rc_encode_bin_1(enc, fl, fl+fs, 15);
}

int rc_laplace_decode(ec_dec *dec, unsigned short fs, int decay)
{
	int val=0;
	unsigned fl;
	unsigned fm;
	fm = rc_dec_bin(dec, 15);
	fl = 0;
	if (fm >= fs)
	{
		val++;
		fl = fs;

		fs = rc_laplace_get_freq1(fs, decay)+1;

#ifndef HW_HIFI3
		while(fs > 1 && fm >= fl+2*fs)
#else
		while(fs > 1 && fm >= fl+(fs<<1))
#endif
		{
#ifndef HW_HIFI3
			fs *= 2;
#else
			fs = fs<<1;
#endif
			fl += fs;
#ifndef HW_HIFI3
			fs = ((fs-2)*(int)decay)>>15;
#else
			fs = SSC_MULT16x16_Q15((short)(fs-2),decay);
#endif
			fs ++;
			val++;
		}

		if (fs <= 1)
		{
			int di;
			di = (fm-fl)>>(1);
			val += di;
			fl += (di<<1);
		}
		if (fm < fl+fs) val = -val;
		else fl += fs;
	}
	rc_dec_upd(dec, fl, SPEECH_MIN(fl+fs,32768), 32768);
	return val;
}