
#include "multitap_echo.h"


/////////////////////////////////////////////////////////////////////////////////
// Multitap delay state and initialization constants
/////////////////////////////////////////////////////////////////////////////////
EchoState echoState;
DSPfract echo_buffer[ECHO_MAX_LENGTH];
const DSPint initial_delay[N_TAP] = {1024, 1536, 2560, 3072};
const DSPfract initial_gain[N_TAP] = {FRACT_NUM(0.25), FRACT_NUM(0.125), FRACT_NUM(0.0625), FRACT_NUM(0.0625)};
const DSPfract initial_input_gain = FRACT_NUM(0.5);
/////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////
// @Author	<student name>
// @Date		<date>
//
// Function:
// multitap_echo_init
//
// @param - nothing
//
// @return - nothing
// Comment: Initialize echoState structure
//
// E-mail:	<email>
//
/////////////////////////////////////////////////////////////////////////////////
void multitap_echo_init()
{
	DSPint i;
	DSPfract* pInitEchoBuffer = echo_buffer;

	DSPint* pDelay = echoState.delay;
	const DSPint* pInitDelay = initial_delay;
	DSPfract* pGain = echoState.tap_gain;
	const DSPfract* pInitGain = initial_gain;
	DSPfract** readPtrPtr = echoState.readPointer;
	DSPfract* readPtrOffset = pInitEchoBuffer + ECHO_MAX_LENGTH - 1;

	for (i = 0; i < ECHO_MAX_LENGTH; i++)
	{
		*pInitEchoBuffer = FRACT_NUM(0.0);
		pInitEchoBuffer++;
	}

	echoState.pEchoBuff = echo_buffer;
	echoState.bufferLength = ECHO_MAX_LENGTH;
	echoState.writePointer = echoState.pEchoBuff + ECHO_MAX_LENGTH-1;
	echoState.input_gain = FRACT_NUM(0.5);
	echoState.n_tap = N_TAP;

	for(i = 0; i< N_TAP; i++)
	{
		*readPtrPtr++ = readPtrOffset - *pInitDelay;
		*pDelay++ = *pInitDelay++;
		*pGain++ = *pInitGain++;
	}
}

/////////////////////////////////////////////////////////////////////////////////
// @Author	<student name>
// @Date		<date>
//
// Function:
// multitap_echo
//
// @param - pInbuf - Buffer with input samples
//		  - pOutbuf - Buffer with output samples
//
// @return - nothing
// Comment: Apply echo to input samples
//
// E-mail:	<email>
//
/////////////////////////////////////////////////////////////////////////////////
void multitap_echo(DSPfract *pInbuf, DSPfract *pOutbuf)
{
	int i, j;
	DSPfract* writePtr = echoState.writePointer;
	DSPfract* pBufEnd = echoState.pEchoBuff+echoState.bufferLength - 1;

	for(i = 0; i < BLOCK_SIZE; i++)
	{
		DSPfract outputSample =  *pInbuf * echoState.input_gain;
		DSPfract* pGain = echoState.tap_gain;
		DSPfract** readPtrPtr = echoState.readPointer;

		*writePtr = *pInbuf;
		writePtr++;
		writePtr = (writePtr > pBufEnd)? echoState.pEchoBuff:writePtr;

		for(j = 0; j < echoState.n_tap; j++)
		{
			DSPfract* readPtr = *readPtrPtr;
			outputSample +=  *pGain * *readPtr;
			pGain++;
			readPtr++;
			readPtr = (readPtr > pBufEnd)? echoState.pEchoBuff:readPtr;
			*readPtrPtr = readPtr;
			readPtrPtr++;
		}

		*pOutbuf = outputSample;
		pOutbuf++;
		pInbuf++;
	}

	 echoState.writePointer = writePtr;
}
/////////////////////////////////////////////////////////////////////////////////

