/*
 *  ======== block_sine32.c ========
 *	Like block_sine, but adjusted for 32-bit (16-bit left, 16-bit right).
 *
 *  The coefficient A and the three initial values
 *  generate a 200 Hz tone (sine wave) when running
 *  at a sample rate of 8KHz.
 *
 *  Even though the calculations are done in floating
 *  point, this function returns a short value since
 *  this is what's needed by a 16-bit codec (DAC).
 */
 
/*
 *  ======== Prototypes ========
 */
void blockSine(short *buf, int len);
void copyData(int *inbuf, int *outbuf ,int length );
void  addData(int *inbuf0, int *inbuf1, int *outbuf ,int length );
void  addScaleData(int *inbuf0, int *inbuf1, int *outbuf, int scale ,int length );
void addSine(int *inbuf, int length);
static short sineGen(void);

/*
 *  ======== Static Global Variables ========
 */
static float y[3] = {0,0.1564345,0};
static float A = 1.9753767;

/*
 *	======== sineGen ========
 *  generate a single element of sine data
 */ 	
static short sineGen(void) 
{  
	short temp;
 
	y[0] = y[1] * A - y[2];  
	y[2] = y[1];
	y[1] = y[0];
	
	y[0] *= 32000;
    temp = (short)y[0];

	return(temp);
}

/*
 *	======== blockSine ========
 *  generate a block of sine data using sineGen
 */ 	
void blockSine(short *buf, int len) 
{ 
	int i = 0;
	
    for (i = 0;i < len; i++) { 
       buf[i]  = sineGen();
    }
}

/*
 *	======== copyData ========
 *  copy data from one buffer to the other.
 *  make sure the lsb is zeroed out so it is not
 *  interpreted by the codec as a control word
 */ 	
void copyData(int *inbuf, int *outbuf ,int length )
{
	int i = 0;
	
    for (i = 0; i < length; i++) {
    	outbuf[i]  = inbuf[i];
    }
}

/*
 *	======== addSine ========
 *  add the sine wave to the indicated buffer
 *  divide the original signal by 2, then divide the sine wave
 *  signal by 8 and add it
 */
void addSine(int *inbuf, int length)
{
	int i = 0;
	short tmp;
	int both;
	
	for (i = 0; i < length; i++) {
		tmp = sineGen()>>3;	
		both = (tmp<<16) | tmp;
		inbuf[i] = _add2(inbuf[i], both);
	}
}

/*
 *	======== addData ========
 *  add two stereo signals
 *  Format is Left:  upper 16 bits,  Right:  lower 16 bits.
 *  outbuf must have enough room.
 */
void addData(int *inbuf0, int *inbuf1, int *outbuf, int length)
{
	int i = 0;
	
	for (i = 0; i < length; i++) {
		outbuf[i] = _add2(inbuf0[i], inbuf1[i]);
	}
}

/*
 *	======== scaleData ========
 *  Multiply data by a constant
 *  Format is Left:  upper 16 bits,  Right:  lower 16 bits.
 *  outbuf must have enough room.
 */
void scaleData(int *inbuf, int scale, int length)
{
	int i = 0;
	int left;
	short right;
	
	for (i = 0; i < length; i++) {
		left  = _mpyhslu(inbuf[i], scale);
		right = _mpysu  (inbuf[i], scale)  >> 16;
		inbuf[i] = (left & 0xffff0000) | right;
	}
}

/*
 *	======== addScaleData ========
 *  add two stereo signals, scale the second one.
 *  Format is Left:  upper 16 bits,  Right:  lower 16 bits.
 *  outbuf must have enough room.
 *
 *	The left channel seems to clip, but I can't figure out why.  may 25-Mar-2003
 */
void addScaleData(int *inbuf0, int *inbuf1, int *outbuf, int scale, int length)
{
	int i = 0;
	int left;
	short right;
	
	for (i = 0; i < length; i++) {
		left  = _mpyhl(inbuf1[i], scale) & 0xffff0000;
		right = _mpy  (inbuf1[i], scale)  >> 16;
		outbuf[i] = _add2(inbuf0[i], left | right);
//		outbuf[i] = _add2(inbuf0[i], inbuf1[i]);
	}
}