//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+
//+   Copyright (c) CML Microsystems Plc 2015
//+
//+   This firmware was designed by CML Microcircuits (UK) Ltd, based on
//+   UK originated technology and is only intended for use on CML’s  Evaluation kits.
//+
//+   The supply and use of this Firmware is subject to CML's
//+   Licence Agreement, a copy of which can be obtained from CML on request.
//+
//+   Contact details:
//+   Technical:  techsupport@cmlmicro.com
//+   Sales:  sales@cmlmicro.com
//+
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++



#include "chip.h"
#include "pe0003.h"
#include "pe0003_config.h"
#include "ftdi.h"
#include "cbus.h"
#include "gpio.h"
#include "timer.h"
#include "lpc_types.h"

#include "mon_file.h"
#include "mon_general.h"


#include "ProgTools.h"
#include "Settings.h"
#include "Registers.h"

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <cr_section_macros.h>


//Global variables
//Use static buffers
//	-	as global to get more control in the memory allocation
//		(KEY TO HELP DURING THE MEMORY ALLOCATION AND MAXIMIZE ITS USE (LPC4330))
//	-	all the buffers share the same static buffers
///Static Buffers allocated in RAM2
__BSS(RAM2) uint16_t sBufferOut[OUTSTATICBUFFER_SIZE];
__BSS(RAM2) uint16_t sBufferIn[INSTATICBUFFER_SIZE];




//-------------------------------------------------------------------
//
//		MENUES
//
//-------------------------------------------------------------------



uint8_t Menu_DefaultSelection(uint8_t defOpt)
{
	uint32_t val = 0;
	while(defOpt == 0)
	{
		mprintf("\n");
		mprintf("Select configuration:\n");
		mprintf("	1. Encode G711: Input->Analog Audio, Output->G729A data file\n");
		mprintf("	2. Encode G723.1: Input->Analog Audio, Output->G723.1 data file\n");
		mprintf("	3. Decode G711: Input->G711 data file, Output->Analog Audio\n");
		mprintf("	4. Decode G711: (loop) Input->G711 data file, Output->Analog Audio\n");
		mprintf("	5. Decode G723.1: Input->G711 data file, Output->Analog Audio\n");
		mprintf("	6. Decode G723.1: (loop) Input->G711 data file, Output->Analog Audio\n");
		mprintf("	7. Real-tile Test G711\n");
		mprintf("	8. Full-duplex Transcoding G723.1\n");
		mscanf("%d\n", &val);
		defOpt = (uint8_t) val;
	}

	return defOpt;
}


//-------------------------------------------------------------------
//
//		PRECONFIGURE OPERATIONS - HELP FUNCTIONS
//
//-------------------------------------------------------------------

/**
 * @brief Open a file for reading according to the configuration set at Settings.h. (FOR PR7261)
 * @param file_id	- File identifier
 * @param filename	- Filename (not used)
 * @param fileOperation - FILE_WRITE or FILE_READ to open for writing or reading respectively
 * @param fileType - Type of file - CFG_WAV_FILE, CFG_TEXT_FILE, CFG_BIN_FILE(Default)
 * @return GUI_FILE_OK if there is not errors
 */
uint8_t PR7261_FileDialogOpen(uint8_t *file_id,  const char* filename, uint8_t fileOperation, uint8_t fileType)
{
	uint8_t res = 0;

	//WAV FILE
	if (fileType == CFG_WAV_FILE)
	{
		uint8_t isStereo = 0;

		if (fileOperation == FILE_WRITE)
			mprintf("Select WAV File to store the data...\n");
		else
			mprintf("Select WAV File to load data from...\n");

		if (WAV_CHANNELS == 1)
			isStereo = FILEWAV_MONO;
		else
			isStereo = FILEWAV_STEREO;

		res = Gui_FileWavDialogOpen(file_id, filename, fileOperation, isStereo, FILEWAV_BITSPERSAMPLE_16B, WAV_SAMPLERATE);
	}
	else
	{
		//TEXT FILE
		if (fileType == CFG_TEXT_FILE)
		{
			if (fileOperation == FILE_WRITE)
				mprintf("Select file to store data...\n");
			else
				mprintf("Select file to load data from...\n");
			res = Gui_FileTxtDialogOpen(file_id, filename, fileOperation, "%d");
		}
		else
		{
			//DEFAULT BIN FILE
			if (fileOperation == FILE_WRITE)
				mprintf("Select binary file to store data...\n");
			else
				mprintf("Select binary file to load data from...\n");
			res = Gui_FileBinDialogOpen(file_id, filename, fileOperation);
		}
	}

	return res;
}

/**
 * @brief Read samples from a file. (FOR PR7261)
 * @param file_id - file identifier
 * @param data - Data array to hold the samples (word size)
 * @param length - Number of samples
 * @param nDataRead - Number of samples read back
 * @param fileType - Type of file - CFG_WAV_FILE, CFG_TEXT_FILE, CFG_BIN_FILE(Default)
 * @return GUI_FILE_OK if there is not errors
 */
uint8_t PR7261_FileReadSamples(uint8_t file_id, uint16_t *data, uint32_t length, uint32_t *nDataRead, uint8_t fileType )
{
	uint8_t res;

	//Read some data first
	if (fileType == CFG_WAV_FILE)
		res = Gui_FileWavReadWordSamples(file_id, (uint16_t *)data, OUTBUFFER_SIZE , nDataRead);
	else
	{
		if (fileType == CFG_TEXT_FILE)
			res = Gui_FileTxtRead(file_id, (uint8_t *)data, OUTBUFFER_SIZE , nDataRead, FILE_TYPE_WORD);
		else
			res = Gui_FileReadWord(file_id, data, OUTBUFFER_SIZE , nDataRead);
	}

	return res;
}

/**
 * @brief Write samples into a file. (FOR PR7261)
 * @param file_id - file identifier
 * @param data - Data array with the samples (word size)
 * @param length - Number of samples
 * @param nDataWritten - Number of samples to write
 * @param fileType - Type of file - CFG_WAV_FILE, CFG_TEXT_FILE, CFG_BIN_FILE(Default)
 * @return GUI_FILE_OK if there is not errors
 */
uint8_t PR7261_FileWriteSamples(uint8_t file_id, uint16_t *data, uint32_t length, uint32_t *nDataWritten, uint8_t fileType)
{
	uint8_t res;

	if (fileType == CFG_WAV_FILE)
		res = Gui_FileWavWriteWordSamples(file_id, data, INBUFFER_SIZE, nDataWritten);
	else
	{
		if (fileType == CFG_TEXT_FILE)
			res = Gui_FileTxtWrite(file_id, (uint8_t *) data, INBUFFER_SIZE, nDataWritten, FILE_TYPE_WORD);
		else
			res = Gui_FileWriteWord(file_id, data, INBUFFER_SIZE, nDataWritten);	//BIN FILE DEFAULT
	}
	return res;
}


/**
 * @brief Close file. (FOR PR7261)
 * @param file_id - file identifier
 * @param fileType - Type of file - CFG_WAV_FILE, CFG_TEXT_FILE, CFG_BIN_FILE(Default)
 * @return GUI_FILE_OK if there is not errors
 */
uint8_t PR7261_FileClose(uint8_t file_id, uint8_t fileType)
{
	uint8_t res;

	if (fileType == CFG_WAV_FILE)
		res = Gui_FileWavClose(file_id);
	else
	{
		if (fileType == CFG_TEXT_FILE)
			res = Gui_FileClose(file_id);
		else
			res = Gui_FileClose(file_id);
	}

	return res;
}




//-------------------------------------------------------------------
//
//		PRECONFIGURE OPERATIONS
//
//-------------------------------------------------------------------
//
//	PACKED G711
//
//-------------------------------------------------------------------


uint8_t G711_Encoder_Op(uint32_t nSamples)
{
	uint8_t res;
	uint32_t i = 0;
	uint8_t level = 0;
	uint16_t val = 0;
	uint8_t file_id = 0;
	uint32_t nDataWritten;
	uint16_t nstatus = 0;

	uint16_t bufferIn[INBUFFER_SIZE];
	uint32_t j = 0;

	//Open file for writting
	char filename[] = "dummy.dat";
	res = PR7261_FileDialogOpen(&file_id, filename, FILE_WRITE, CFG_OUT_FILE);
	if (res != GUI_FILE_RESULT_OK)
	{
		mprintf("ERROR - Opening file for writing\n");
	}


	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0);						//Idle mode to configure
	//ANAIN REGISTER
	val = ANAIN_CONF_VAL;												//Value set from Settings.h
	Pe0003_CbusWriteWord(CBUSPORT, ANAIN_CONF, val);					//Power up AnlogIn
	mprintf("ANAIN_CONF_REG 0x%x\n", val);
	//ANAIN_COARSE_GAIN REGISTER
	val = ANAIN_COARSE_GAIN_VAL;
	Pe0003_CbusWriteWord(CBUSPORT, ANAIN_COARSE_GAIN, val);				//XdB gain
	mprintf("ANAIN_COARSE_GAIN 0x%x\n", val);


	Pe0003_CbusWriteWord(CBUSPORT, FIFO_CONTROL, 0x8080);				//Flush FIFO

	Pe0003_TimerDelayUs(10000);

	val = INPUT_TYPE_16BIT_SIGNED_PCM;
	mprintf("INPUT_REG 0x%x\n", val);
	Pe0003_CbusWriteWord(CBUSPORT, INPUT_TYPE, val);					//16 Bit linear PCM
	val = OUTPUT_TYPE_PACKED_G711A;
	mprintf("OUTPUT_REG 0x%x\n", val);
	Pe0003_CbusWriteWord(CBUSPORT, OUTPUT_TYPE, val);					//G711 Packed samples

	val = 0x211;
	mprintf("MODE_REG 0x%x\n", val);
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, val);

	Pe0003_TimerDelayUs(10000);

	while(i < nSamples)
	{
		//Read level of the FIFO
		level = Pe0003_CbusReadByte(CBUSPORT, FIFO_OUT_LEVEL);

		if (level > 0)
		{
			//Recover sample
			val = Pe0003_CbusReadWord(CBUSPORT, FIFO_OUT_WORD);

			bufferIn[j++] = val;
			i++;

			//Store data in blocks of 10
			if (j == INBUFFER_SIZE)
			{
				j = 0;
				res = PR7261_FileWriteSamples(file_id, (uint16_t *) &bufferIn[0], INBUFFER_SIZE, &nDataWritten, CFG_OUT_FILE);
				if (res != GUI_FILE_RESULT_OK)
				{
					mprintf("ERROR - Writing data into a file\n");
					return FALSE;
				}

			}

			if (DEBUG_DISP)
			{
				if ((i % 255) == 0)
					mprintf("s%d \n", i);
			}
		}

		/*
		if (level == WARNING_FIFO_LEVEL)
		{
			mprintf("ERROR - Buffer overflow\n");
		}
		*/
		//Check status for underflows
		nstatus = Pe0003_CbusReadWord(CBUSPORT, STATUS);

		if (nstatus & IRQ_OFLOW)			//Check for overflows
		{
			mprintf("ERROR - Buffer overflow\n");
		}
	}



	//Store data left
	if (j > 0)
	{
		//Save data into a file
		res = PR7261_FileWriteSamples(file_id, (uint16_t *) &bufferIn[0], j, &nDataWritten, CFG_OUT_FILE);
		if (res != GUI_FILE_RESULT_OK)
		{
			mprintf("ERROR - Writing data into a file\n");
		}
	}


	//Close file
	res = PR7261_FileClose(file_id, CFG_OUT_FILE);

	mprintf("G711 packed samples captured\n");

	return TRUE;
}



uint8_t G711_Decoder_Op(uint32_t nSamples)
{
	uint8_t res;
	uint8_t level = 0;
	uint8_t file_id = 0;
	uint32_t nDataRead;
	uint16_t nstatus = 0;
	uint32_t count = 0;
	uint32_t dispCount = 0;
	uint16_t val = 0;

	uint16_t bufferOut[OUTBUFFER_SIZE];
	uint32_t j = 0;

	//Open file for reading
	char filename[] = "dummy.dat";
	res = PR7261_FileDialogOpen(&file_id, filename, FILE_READ, CFG_IN_FILE);
	if (res != GUI_FILE_RESULT_OK)
	{
		mprintf("ERROR - Opening file for writing\n");
		return FALSE;
	}

	//Configuration
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0);								//Idle mode to configure
	Pe0003_CbusWriteWord(CBUSPORT, FIFO_CONTROL, 0x8080);						//Flush FIFO

	Pe0003_TimerDelayUs(10000);


	Pe0003_CbusWriteWord(CBUSPORT, INPUT_TYPE, INPUT_TYPE_PACKED_G711A);			//G711 samples
	mprintf("INPUT_REG 0x%x\n", INPUT_TYPE_PACKED_G711A);
	Pe0003_CbusWriteWord(CBUSPORT, OUTPUT_TYPE, OUTPUT_TYPE_16BIT_SIGNED_PCM);		//PCM 16 bit signed linear
	mprintf("OUTPUT_REG 0x%x\n", OUTPUT_TYPE_16BIT_SIGNED_PCM);

	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_CONF, ANAOUT_CONF_VAL);					//ANAOUT driver
	mprintf("ANAOUT_CONF_REG 0x%x\n", ANAOUT_CONF_VAL);
	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_COARSE_GAIN, ANAOUT_COARSE_GAIN_VAL);		//XdB attenuation
	mprintf("ANAOUT_COARSE_GAIN_REG 0x%x\n", ANAOUT_COARSE_GAIN_VAL);

	mprintf("MODE_REG 0x%x\n", 0x121);
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0x121);



	//Read some data first
	res = PR7261_FileReadSamples(file_id, (uint16_t *)bufferOut, OUTBUFFER_SIZE , &nDataRead, CFG_IN_FILE);

	//Normal process
	while(nDataRead != 0)
	{
		while(nDataRead != 0)
		{
			//Check fifo level
			level = Pe0003_CbusReadByte(CBUSPORT, FIFO_IN_LEVEL);

			if (level < MAX_FIFO_SIZE)
			{
				if ((MAX_FIFO_SIZE - level) > nDataRead)
				{
					//Stream some data in
					Pe0003_CbusWriteWstream(CBUSPORT,  FIFO_IN_WORD, &bufferOut[j], nDataRead);		//Stream data into the buffer
					count += nDataRead;
					j = 0;
					nDataRead = 0;
				}
				/*
				else
				{
					uint8_t spaceLeft = MAX_FIFO_SIZE - level;
					Pe0003_CbusWriteWstream(CBUSPORT,  FIFO_IN_WORD, &bufferOut[j], spaceLeft);		//Stream part of the data into the buffer
					j = spaceLeft;
					count += spaceLeft;
					nDataRead -= spaceLeft;
				}
				*/
			}

			//Check status for underflows
			nstatus = Pe0003_CbusReadWord(CBUSPORT, STATUS);

			if (nstatus & IRQ_UFLOW)			//Check for underflow
			{
				mprintf("ERROR - Data underflow\n");
			}

			if (DEBUG_DISP)
			{
				if (count > dispCount)
				{
					dispCount += 255;
					mprintf("s%d \n", count);
				}
			}

			if (count >= nSamples)
				break;
		}

		if (count >= nSamples)
			break;

		//Get more data
		res = PR7261_FileReadSamples(file_id, (uint16_t *)bufferOut, OUTBUFFER_SIZE , &nDataRead, CFG_IN_FILE);
		if (res != GUI_FILE_RESULT_OK)
		{
			mprintf("ERROR - Reading data from file\n");
			return FALSE;
		}
	}

	//Close file
	res = PR7261_FileClose(file_id, CFG_IN_FILE);

	mprintf("G711 samples sent\n");

	return TRUE;
}



uint8_t G711_Decoder_Op_Loop(uint32_t nSamples)
{
	uint8_t res;
	uint8_t level = 0;
	uint8_t file_id = 0;
	uint32_t nDataRead;
	uint16_t nstatus = 0;
	uint32_t count = 0;
	uint32_t dispCount = 0;
	uint32_t i = 0;
	uint32_t maxSamples = nSamples;
	uint32_t dataLeft;
	uint16_t tmpBufferOut[OUTBUFFER_SIZE];			//Small buffer to deal with small chunks of data
	//uint16_t sBufferOut[OUTSTATICBUFFER_SIZE];

	if (maxSamples > OUTSTATICBUFFER_SIZE)
		maxSamples = OUTSTATICBUFFER_SIZE;

	//Open file for reading
	char filename[] = "dummy.dat";
	res = PR7261_FileDialogOpen(&file_id, filename, FILE_READ, CFG_IN_FILE);
	if (res != GUI_FILE_RESULT_OK)
	{
		mprintf("ERROR - Opening file for writing\n");
		return FALSE;
	}

	//Set configuration
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0);				//Idle mode to configure
	Pe0003_CbusWriteWord(CBUSPORT, FIFO_CONTROL, 0x8080);		//Flush FIFO

	Pe0003_TimerDelayUs(10000);

	Pe0003_CbusWriteWord(CBUSPORT, INPUT_TYPE, INPUT_TYPE_PACKED_G711A);			//711A samples
	mprintf("INPUT_REG 0x%x\n", INPUT_TYPE_PACKED_G711A);
	Pe0003_CbusWriteWord(CBUSPORT, OUTPUT_TYPE, OUTPUT_TYPE_16BIT_SIGNED_PCM);		//PCM 16 bit signed linear
	mprintf("OUTPUT_REG 0x%x\n", OUTPUT_TYPE_16BIT_SIGNED_PCM);

	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_CONF, ANAOUT_CONF_VAL);						//0x802 power up ANAOUT driver
	mprintf("ANAOUT_CONF_REG 0x%x\n", ANAOUT_CONF_VAL);
	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_COARSE_GAIN, ANAOUT_COARSE_GAIN_VAL);							//0dB gain
	mprintf("ANAOUT_COARSE_GAIN_REG 0x%x\n", ANAOUT_COARSE_GAIN_VAL);

	mprintf("MODE_REG 0x%x\n", 0x121);
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0x121);//(MODE_AUDIO_SRC_CBUS << 8) | (MODE_AUDIO_DST_ANA << 4) | MODE_HALF_TRANSC);



	//First get some data from file
	res = PR7261_FileReadSamples(file_id, (uint16_t *)tmpBufferOut, OUTBUFFER_SIZE , &nDataRead, CFG_IN_FILE);

	i = 0;
	while(nDataRead != 0)
	{
		//Copy data from a temporal buffer into final staticBuffer
		memcpy(&sBufferOut[i], tmpBufferOut, nDataRead * sizeof(uint16_t));
		i += nDataRead;

		//Read more samples
		res = PR7261_FileReadSamples(file_id, (uint16_t *)tmpBufferOut, OUTBUFFER_SIZE , &nDataRead, CFG_IN_FILE);
		if(i + nDataRead > maxSamples)
			break;
	}

	//Add data left
	if (nDataRead != 0)
	{
		if (i != maxSamples)
		{
			dataLeft = maxSamples - i;
			memcpy(&sBufferOut[i], tmpBufferOut, dataLeft * sizeof(uint16_t));

		}
	}

	//Close file
	res = PR7261_FileClose(file_id, CFG_IN_FILE);



	//Normal process. Infinite loop
	i = 0;
	while(1)
	{
		//Check fifo level
		level = Pe0003_CbusReadByte(CBUSPORT, FIFO_IN_LEVEL);

		if (level < MAX_FIFO_SIZE)
		{
			if ((MAX_FIFO_SIZE - level) > OUTBUFFER_SIZE)
			{
				memcpy(tmpBufferOut, &sBufferOut[i], OUTBUFFER_SIZE * sizeof(uint16_t));

				//Stream samples into CBus
				Pe0003_CbusWriteWstream(CBUSPORT,  FIFO_IN_WORD, tmpBufferOut, OUTBUFFER_SIZE);		//Streaming data
				i += OUTBUFFER_SIZE;
				count += OUTBUFFER_SIZE;


				if ((i + OUTBUFFER_SIZE) > maxSamples)
					i = 0;

			}

			//Check status for underflows
			nstatus = Pe0003_CbusReadWord(CBUSPORT, STATUS);

			if (nstatus & IRQ_UFLOW)			//Check for underflow
			{
				mprintf("ERROR - Data underflow\n");
			}

			if (DEBUG_DISP)
			{
				if (count > dispCount)
				{
					dispCount += 255;
					mprintf("s%d \n", count);
				}
			}
		}
	}

	return TRUE;
}




uint8_t G711_EncoderDecoder_Op()
{
	uint16_t val;

	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0);				//Idle mode to configure

	Pe0003_CbusWriteWord(CBUSPORT, ANAIN_CONF, ANAIN_CONF_VAL);			//Power up AnlogIn
	mprintf("ANAIN_CONF_REG 0x%x\n", ANAIN_CONF_VAL);
	Pe0003_CbusWriteWord(CBUSPORT, ANAIN_COARSE_GAIN, ANAIN_COARSE_GAIN_VAL);		//0dB gain
	mprintf("ANAIN_COARSE_GAIN_REG 0x%x\n", ANAIN_COARSE_GAIN_VAL);

	Pe0003_CbusWriteWord(CBUSPORT, FIFO_CONTROL, 0x8080);		//Flush FIFO

	Pe0003_TimerDelayUs(10000);

	val = INPUT_TYPE_PACKED_G711A | INPUT_RATE_VAL;
	Pe0003_CbusWriteWord(CBUSPORT, INPUT_TYPE, val);			//Packed G711A
	mprintf("INPUT_REG 0x%x\n", val);

	val = OUTPUT_TYPE_PACKED_G711A | INPUT_RATE_VAL;
	Pe0003_CbusWriteWord(CBUSPORT, OUTPUT_TYPE, val);			//Packed G711A
	mprintf("OUTPUT_REG 0x%x\n", val);


	val = ANAOUT_CONF_VAL;
	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_CONF, val);	//0x802 power up ANAOUT driver
	mprintf("ANAOUT_CONF_REG 0x%x\n", val);

	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_COARSE_GAIN_VAL, ANAOUT_COARSE_GAIN_VAL);							//0dB gain
	mprintf("ANAOUT_COARSE_GAIN_REG 0x%x\n", ANAOUT_COARSE_GAIN_VAL);

	val = (MODE_AUDIO_SRC_ANA << 8) | (MODE_AUDIO_DST_ANA << 4) | MODE_ENCDEC;
	mprintf("MODE_REG 0x%x\n", val);
	mprintf("Encoder-Decoder Running\n");
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, val);			//Mode register


	return TRUE;
}



//--------------------------------------------------------------------------------------------
//
//	UNPACKED G723.1
//
//--------------------------------------------------------------------------------------------




uint8_t G723_Encoder_Op(uint32_t nSamples)
{
	uint8_t res;
	uint32_t i = 0;
	uint8_t level = 0;
	uint16_t val = 0;
	uint8_t control = 0;
	uint8_t file_id = 0;
	uint32_t nDataWritten;
	uint16_t nstatus = 0;
	uint8_t G723_table_sizes[] = {24,20,4,1};

	uint16_t bufferIn[INBUFFER_SIZE];
	uint32_t j = 0;

	//Open file for writting
	char filename[] = "dummy.dat";
	res = PR7261_FileDialogOpen(&file_id, filename, FILE_WRITE, CFG_OUT_FILE);
	if (res != GUI_FILE_RESULT_OK)
	{
		mprintf("ERROR - Opening file for writing\n");
	}


	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0);						//Idle mode to configure
	Pe0003_CbusWriteWord(CBUSPORT, ANAIN_CONF, ANAIN_CONF_VAL);			//Power up AnlogIn
	mprintf("ANAIN_CONF_REG 0x%x\n", ANAIN_CONF_VAL);
	Pe0003_CbusWriteWord(CBUSPORT, ANAIN_COARSE_GAIN, ANAIN_COARSE_GAIN_VAL);	//Gain
	mprintf("ANAIN_COARSE_GAIN 0x%x\n", ANAIN_COARSE_GAIN_VAL);


	Pe0003_CbusWriteWord(CBUSPORT, FIFO_CONTROL, 0x8080);						//Flush FIFO

	Pe0003_TimerDelayUs(10000);

	//Configure vocoder G273.1
	Pe0003_CbusWriteWord(CBUSPORT, G723_VOCODER_OPS, G723OPT_VAD);
	mprintf("G723_VOCODER_OPS 0x%x\n", G723OPT_VAD);

	mprintf("INPUT_REG 0x%x\n", INPUT_TYPE_16BIT_SIGNED_PCM);
	Pe0003_CbusWriteWord(CBUSPORT, INPUT_TYPE, INPUT_TYPE_16BIT_SIGNED_PCM);	//16 Bit linear PCM
	mprintf("OUTPUT_REG 0x%x\n", OUTPUT_TYPE_UNPACKED_G723);
	Pe0003_CbusWriteWord(CBUSPORT, OUTPUT_TYPE, OUTPUT_TYPE_UNPACKED_G723);		//G723.1 samples



	mprintf("MODE_REG 0x%x\n", 0x211);
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0x211);							//Analogue input, CBus output


	Pe0003_TimerDelayUs(10000);

	uint8_t packet_length = 0, packet_data = 0;

	while((i < nSamples) || (packet_length != packet_data))
	{
		//Read level of the FIFO
		level = Pe0003_CbusReadByte(CBUSPORT, FIFO_OUT_LEVEL);

		if (level > 0)
		{
			if (packet_length == packet_data)
			{
				val = Pe0003_CbusReadWord(CBUSPORT, FIFO_OUT_WORD);

				packet_length = G723_table_sizes[ (control & 0x03)];

				packet_data = 1;
				bufferIn[j++] = val;
				i++;
			}
			else
			{
				//Recover sample
				val = Pe0003_CbusReadWord(CBUSPORT, FIFO_OUT_WORD);
				packet_data++;
				bufferIn[j++] = val;
				i++;
			}

			if (j == INBUFFER_SIZE)
			{
				j = 0;
				res = PR7261_FileWriteSamples(file_id, (uint16_t *) &bufferIn[0], INBUFFER_SIZE, &nDataWritten, CFG_OUT_FILE);
				if (res != GUI_FILE_RESULT_OK)
				{
					mprintf("ERROR - Writing data into a file\n");
					return FALSE;
				}

			}

			if (DEBUG_DISP)
			{
				if ((i % 255) == 0)
					mprintf("s%d \n", i);
			}
		}

		//Check status for underflows
		nstatus = Pe0003_CbusReadWord(CBUSPORT, STATUS);

		if (nstatus & IRQ_OFLOW)			//Check for overflows
		{
			mprintf("ERROR - Buffer overflow\n");
		}
	}



	//Store data left
	if (j > 0)
	{
		//Save data into a file
		res = PR7261_FileWriteSamples(file_id, (uint16_t *) &bufferIn[0], j, &nDataWritten, CFG_OUT_FILE);
		if (res != GUI_FILE_RESULT_OK)
		{
			mprintf("ERROR - Writing data into a file\n");
		}
	}


	//Close file
	res = PR7261_FileClose(file_id, CFG_OUT_FILE);

	mprintf("G723 samples captured\n");

	return TRUE;
}



uint8_t GetLastPacketDataLength(uint8_t *packetCurLengthLeft, uint16_t *bufferOut, uint8_t nDataRead, uint8_t lastPacketLen)
{
	uint8_t nData = nDataRead;
	uint8_t G723_table_sizes[] = {24,20,4,1};
	uint32_t index =  0;
	uint8_t len = lastPacketLen;
	uint8_t val = 0;
	uint8_t dataLeft = (*packetCurLengthLeft);


	while(nData != 0)
	{
		if (dataLeft == 0)
		{
			val = (bufferOut[index + 1] >> 8) & 0xFF;


			len = G723_table_sizes[val];
			dataLeft = len;
		}



		dataLeft--;
		nData--;
	}

	*packetCurLengthLeft = dataLeft;

	return len;
}




uint8_t G723_Decoder_Op(uint32_t nSamples)
{
	uint8_t res;
	uint8_t level = 0;
	uint8_t file_id = 0;
	uint32_t nDataRead;
	uint16_t nstatus = 0;
	uint32_t count = 0;
	uint32_t dispCount = 0;
	uint8_t G723_table_sizes[] = {24,20,4,1};

	uint16_t bufferOut[OUTBUFFER_SIZE];
	uint32_t j = 0;

	//Open file for reading
	char filename[] = "dummy.dat";
	res = PR7261_FileDialogOpen(&file_id, filename, FILE_READ, CFG_IN_FILE);
	if (res != GUI_FILE_RESULT_OK)
	{
		mprintf("ERROR - Opening file for writing\n");
		return FALSE;
	}

	//Configuration
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0);				//Idle mode to configure
	Pe0003_CbusWriteWord(CBUSPORT, FIFO_CONTROL, 0x8080);		//Flush FIFO

	Pe0003_TimerDelayUs(10000);

	Pe0003_CbusWriteWord(CBUSPORT, INPUT_TYPE, INPUT_TYPE_UNPACKED_G723);			//G723 samples
	mprintf("INPUT_REG 0x%x\n", INPUT_TYPE_UNPACKED_G723);
	Pe0003_CbusWriteWord(CBUSPORT, OUTPUT_TYPE, OUTPUT_TYPE_16BIT_SIGNED_PCM);		//PCM 16 bit signed linear
	mprintf("OUTPUT_REG 0x%x\n", OUTPUT_TYPE_16BIT_SIGNED_PCM);


	//Configure vocoder G273.1
	Pe0003_CbusWriteWord(CBUSPORT, G723_VOCODER_OPS, G723OPT_VAD);
	mprintf("G723_VOCODER_OPS 0x%x\n", G723OPT_VAD);

	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_CONF, ANAOUT_CONF_VAL);					//power up ANAOUT driver
	mprintf("ANAOUT_CONF_REG 0x%x\n", ANAOUT_CONF_VAL);
	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_COARSE_GAIN, ANAOUT_COARSE_GAIN_VAL);		//0dB attenuation
	mprintf("ANAOUT_COARSE_GAIN_REG 0x%x\n", ANAOUT_COARSE_GAIN_VAL);

	mprintf("MODE_REG 0x%x\n", 0x121);
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0x121);



	//Read some data first
	res = PR7261_FileReadSamples(file_id, (uint16_t *)bufferOut, OUTBUFFER_SIZE , &nDataRead, CFG_IN_FILE);

	//Normal process
	while(nDataRead != 0)
	{
		while(nDataRead != 0)
		{
			//Check fifo level
			level = Pe0003_CbusReadByte(CBUSPORT, FIFO_IN_LEVEL);

			if (level < MAX_FIFO_SIZE)
			{
				if ((MAX_FIFO_SIZE - level) > nDataRead)
				{
					//Stream some data in
					Pe0003_CbusWriteWstream(CBUSPORT,  FIFO_IN_WORD, &bufferOut[j], nDataRead);		//Stream data into the buffer
					count += nDataRead;
					j = 0;
					nDataRead = 0;
				}
			}

			//Check status for underflows
			nstatus = Pe0003_CbusReadWord(CBUSPORT, STATUS);

			if (nstatus & IRQ_UFLOW)			//Check for underflow
			{
				mprintf("ERROR - Data underflow\n");
			}

			if (DEBUG_DISP)
			{
				if (count > dispCount)
				{
					dispCount += 255;
					mprintf("s%d \n", count);
				}
			}

			if (count >= nSamples)
				break;
		}

		if (count >= nSamples)
			break;

		//Get more data
		res = PR7261_FileReadSamples(file_id, (uint16_t *)bufferOut, OUTBUFFER_SIZE , &nDataRead, CFG_IN_FILE);
		if (res != GUI_FILE_RESULT_OK)
		{
			mprintf("ERROR - Reading data from file\n");
			return FALSE;
		}
	}

	//Close file
	res = PR7261_FileClose(file_id, CFG_IN_FILE);

	mprintf("G723 samples sent\n");

	return TRUE;
}





uint8_t G723_Decoder_Op_Loop(uint32_t nSamples)
{
	uint8_t res;
	uint8_t level = 0;
	uint8_t file_id = 0;
	uint32_t nDataRead;
	uint16_t nstatus = 0;
	uint32_t count = 0;
	uint32_t dispCount = 0;
	uint32_t i = 0;
	uint32_t maxSamples = nSamples;
	uint32_t dataLeft;
	uint8_t G723_table_sizes[] = {24,20,4,1};
	uint16_t tmpBufferOut[OUTBUFFER_SIZE];			//Small buffer to deal with small chunks of data

	uint8_t packetLeft = 0;
	uint8_t lastPacketLen = 0;

	if (maxSamples > OUTSTATICBUFFER_SIZE)
		maxSamples = OUTSTATICBUFFER_SIZE;

	//Open file for reading
	char filename[] = "dummy.dat";
	res = PR7261_FileDialogOpen(&file_id, filename, FILE_READ, CFG_IN_FILE);
	if (res != GUI_FILE_RESULT_OK)
	{
		mprintf("ERROR - Opening file for writing\n");
		return FALSE;
	}

	//Set configuration
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0);				//Idle mode to configure
	Pe0003_CbusWriteWord(CBUSPORT, FIFO_CONTROL, 0x8080);		//Flush FIFO

	Pe0003_TimerDelayUs(10000);

	Pe0003_CbusWriteWord(CBUSPORT, INPUT_TYPE, INPUT_TYPE_UNPACKED_G723);			//G723 samples
	mprintf("INPUT_REG 0x%x\n", INPUT_TYPE_UNPACKED_G723);
	Pe0003_CbusWriteWord(CBUSPORT, OUTPUT_TYPE, OUTPUT_TYPE_16BIT_SIGNED_PCM);		//PCM 16 bit signed linear
	mprintf("OUTPUT_REG 0x%x\n", OUTPUT_TYPE_16BIT_SIGNED_PCM);

	//Configure vocoder G273.1
	Pe0003_CbusWriteWord(CBUSPORT, G723_VOCODER_OPS, G723OPT_VAD);
	mprintf("G723_VOCODER_OPS 0x%x\n", G723OPT_VAD);

	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_CONF, ANAOUT_CONF_VAL);					//Set ANAOUT driver
	mprintf("ANAOUT_CONF_REG 0x%x\n", ANAOUT_CONF_VAL);
	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_COARSE_GAIN, ANAOUT_COARSE_GAIN_VAL);		//Set Gain
	mprintf("ANAOUT_COARSE_GAIN_REG 0x%x\n", ANAOUT_COARSE_GAIN_VAL);

	mprintf("MODE_REG 0x%x\n", 0x121);
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0x121);//(MODE_AUDIO_SRC_CBUS << 8) | (MODE_AUDIO_DST_ANA << 4) | MODE_HALF_TRANSC);



	//First get some data from file
	res = PR7261_FileReadSamples(file_id, (uint16_t *)tmpBufferOut, OUTBUFFER_SIZE , &nDataRead, CFG_IN_FILE);

	i = 0;
	packetLeft = 0;
	while(nDataRead != 0)
	{
		//Copy data from a temporal buffer into final staticBuffer
		memcpy(&sBufferOut[i], tmpBufferOut, nDataRead * sizeof(uint16_t));
		i += nDataRead;

		//Read more samples
		res = PR7261_FileReadSamples(file_id, (uint16_t *)tmpBufferOut, OUTBUFFER_SIZE , &nDataRead, CFG_IN_FILE);
		lastPacketLen = GetLastPacketDataLength(&packetLeft, (uint16_t *)tmpBufferOut, nDataRead, lastPacketLen);


		if(i + nDataRead > maxSamples)
			break;
	}

	//Add data left
	if (nDataRead != 0)
	{
		if (i != maxSamples)
		{
			dataLeft = maxSamples - i;
			memcpy(&sBufferOut[i], tmpBufferOut, dataLeft * sizeof(uint16_t));

		}
	}



	//Done to keep the full packets(all the bytes)
	if (packetLeft != 0)
	{
		maxSamples -= (lastPacketLen - packetLeft);
	}
	//Close file
	res = PR7261_FileClose(file_id, CFG_IN_FILE);



	//Normal process. Infinite loop
	i = 0;
	while(1)
	{
		//Check fifo level
		level = Pe0003_CbusReadByte(CBUSPORT, FIFO_IN_LEVEL);

		if (level < MAX_FIFO_SIZE)
		{
			if ((MAX_FIFO_SIZE - level) > OUTBUFFER_SIZE)
			{
				memcpy(tmpBufferOut, &sBufferOut[i], OUTBUFFER_SIZE * sizeof(uint16_t));

				//Stream samples into CBus
				Pe0003_CbusWriteWstream(CBUSPORT,  FIFO_IN_WORD, tmpBufferOut, OUTBUFFER_SIZE);		//Streaming data
				i += OUTBUFFER_SIZE;
				count += OUTBUFFER_SIZE;


				if ((i + OUTBUFFER_SIZE) > maxSamples)
					i = 0;

			}

			//Check status for underflows
			nstatus = Pe0003_CbusReadWord(CBUSPORT, STATUS);

			if (nstatus & IRQ_UFLOW)			//Check for underflow
			{
				mprintf("ERROR - Data underflow\n");
			}

			if (DEBUG_DISP)
			{
				if (count > dispCount)
				{
					dispCount += 255;
					mprintf("s%d \n", count);
				}
			}
		}
	}

	return TRUE;
}



uint8_t G723_Encoder_Decoder_FullDuplex_Op(uint32_t nSamples)
{
	uint8_t res;
	uint32_t i = 0;
	uint8_t level = 0;
	uint16_t val = 0;
	uint8_t control = 0;
	uint8_t file_id_enc = 0;
	uint8_t file_id_dec = 0;
	uint32_t nDataWritten;
	uint16_t nstatus = 0;
	uint8_t G723_table_sizes[] = {24,20,4,1};
	uint32_t count = 0;
	uint32_t dispCount = 0;
	uint32_t nDataRead;

	uint16_t bufferIn[INBUFFER_SIZE];
	uint16_t bufferOut[OUTBUFFER_SIZE];
	uint32_t j = 0;
	uint32_t k = 0;


	//OPEN FILE FOR ENCODING
	//Open file for writting
	char filename[] = "encode.dat";
	res = PR7261_FileDialogOpen(&file_id_enc, filename, FILE_WRITE, CFG_OUT_FILE);
	if (res != GUI_FILE_RESULT_OK)
	{
		mprintf("ERROR - Opening file for writing\n");
	}

	//OPEN FILE FOR DECODING
	//Open file for reading
	char filename2[] = "decode.dat";
	res = PR7261_FileDialogOpen(&file_id_dec, filename2, FILE_READ, CFG_IN_FILE);
	if (res != GUI_FILE_RESULT_OK)
	{
		mprintf("ERROR - Opening file for writing\n");
		return FALSE;
	}


	//CONFIGURATION PART
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, 0);						//Idle mode to configure

	//Configure analogue input
	val = ADC_PWR	| ANAIN2_PWR | ANA_SW;//ANAIN_PWR | ANA_SW;
	Pe0003_CbusWriteWord(CBUSPORT, ANAIN_CONF, val);					//Power up AnlogIn
	mprintf("ANAIN_CONF_REG 0x%x\n", val);
	val = ANAIN_COARSE_GAIN_VAL;
	Pe0003_CbusWriteWord(CBUSPORT, ANAIN_COARSE_GAIN, val);				//Gain
	mprintf("ANAIN_COARSE_GAIN 0x%x\n", val);
	//Configure analogue output
	val = ANAOUT_DAC_PWR | ANAOUT_DRVPWR | SPKR2_PWR;// | SPKR1_PWR;
	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_CONF, val);					//power up ANAOUT driver
	mprintf("ANAOUT_CONF_REG 0x%x\n", val);
	val = ANAOUT_COARSE_GAIN_VAL;
	Pe0003_CbusWriteWord(CBUSPORT, ANAOUT_COARSE_GAIN, val);			//0dB attenuation
	mprintf("ANAOUT_COARSE_GAIN_REG 0x%x\n", val);

	//Configure vocoder G273.1
	val = G723OPT_VAD;
	Pe0003_CbusWriteWord(CBUSPORT, G723_VOCODER_OPS, val);
	mprintf("G723_VOCODER_OPS 0x%x\n", val);

	Pe0003_CbusWriteWord(CBUSPORT, FIFO_CONTROL, 0x8080);						//Flush FIFO

	//Configure mode register
	val = (MODE_AUDIO_SRC_CBUS << 8) | (MODE_AUDIO_DST_ANA << 4);// | MODE_FULL_TRANSC;
	mprintf("MODE_REG 0x%x\n", val);
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, val);

	//Configure input
	val = INPUT_TYPE_UNPACKED_G723;
	//val = INPUT_TYPE_16BIT_SIGNED_PCM;
	Pe0003_CbusWriteWord(CBUSPORT, INPUT_TYPE, val);			//G723 samples input
	mprintf("INPUT_REG 0x%x\n", val);

	//Configure output
	//val = OUTPUT_TYPE_UNPACKED_G723;
	val = OUTPUT_TYPE_16BIT_SIGNED_PCM;
	mprintf("OUTPUT_REG 0x%x\n", val);
	Pe0003_CbusWriteWord(CBUSPORT, OUTPUT_TYPE, val);			//G723.1 samples output

	//Configure mode register
	val = (MODE_AUDIO_SRC_CBUS << 8) | (MODE_AUDIO_DST_ANA << 4) | MODE_FULL_TRANSC;
	mprintf("MODE_REG 0x%x\n", val);
	Pe0003_CbusWriteWord(CBUSPORT, MODE_REG, val);

	//PROCESS DATA PART
	uint8_t packet_length = 0, packet_data = 0;
	uint8_t exitDecoder = FALSE;
	while((i < nSamples) || (packet_length != packet_data))// && ((count >= nSamples) || exitDecoder))
	{

		//ENCODER PART
		//Read level of the FIFO
		level = Pe0003_CbusReadByte(CBUSPORT, FIFO_OUT_LEVEL);

		if (level > 0)
		{
			if (packet_length == packet_data)
			{
				val = Pe0003_CbusReadWord(CBUSPORT, FIFO_OUT_WORD);

				packet_length = G723_table_sizes[ (control & 0x03)];

				packet_data = 1;
				bufferIn[j++] = val;
				i++;
			}
			else
			{
				//Recover sample
				val = Pe0003_CbusReadWord(CBUSPORT, FIFO_OUT_WORD);
				packet_data++;
				bufferIn[j++] = val;
				i++;
			}

			if (j == INBUFFER_SIZE)
			{
				j = 0;
				res = PR7261_FileWriteSamples(file_id_enc, (uint16_t *) &bufferIn[0], INBUFFER_SIZE, &nDataWritten, CFG_OUT_FILE);
				if (res != GUI_FILE_RESULT_OK)
				{
					mprintf("ERROR - Writing data into a file\n");
					return FALSE;
				}

			}

			if (DEBUG_DISP)
			{
				if ((i % 255) == 0)
					mprintf("Is%d \n", i);
			}
		}


		//DECODER PART
		if (!exitDecoder)
		{
			if (nDataRead != 0)
			{
				//Check fifo level
				level = Pe0003_CbusReadByte(CBUSPORT, FIFO_IN_LEVEL);

				if (level < MAX_FIFO_SIZE)
				{
					if ((MAX_FIFO_SIZE - level) > nDataRead)
					{
						//Stream some data in
						Pe0003_CbusWriteWstream(CBUSPORT,  FIFO_IN_WORD, &bufferOut[k], nDataRead);		//Stream data into the buffer
						count += nDataRead;
						k = 0;
						nDataRead = 0;
					}
				}

				if (count >= nSamples)
				{
					exitDecoder = TRUE;
					//break;
				}

				if (DEBUG_DISP)
				{
					if (count > dispCount)
					{
						dispCount += 255;
						mprintf("Os%d \n", count);
					}
				}


			}
			else
			{
				res = PR7261_FileReadSamples(file_id_dec, (uint16_t *)bufferOut, OUTBUFFER_SIZE , &nDataRead, CFG_IN_FILE);
				if (res != GUI_FILE_RESULT_OK)
				{
					mprintf("ERROR - Reading data from file\n");
					return FALSE;
				}

				if(nDataRead == 0)
				{
					exitDecoder = TRUE;
					//break;
				}
			}
		}





		//Read status
		nstatus = Pe0003_CbusReadWord(CBUSPORT, STATUS);
		if ((nstatus & IRQ_OFLOW) || (nstatus & IRQ_UFLOW))			//Underflows or overflows?
		{
			if (nstatus & IRQ_OFLOW)
				mprintf("ERROR - Buffer overflow\n");

			if (!exitDecoder)
				if (nstatus & IRQ_UFLOW)
					mprintf("ERROR - Buffer underflow\n");
		}

	}



	//Store data left
	if (j > 0)
	{
		//Save data into a file
		res = PR7261_FileWriteSamples(file_id_enc, (uint16_t *) &bufferIn[0], j, &nDataWritten, CFG_OUT_FILE);
	if (res != GUI_FILE_RESULT_OK)
		{
			mprintf("ERROR - Writing data into a file\n");
		}
	}


	//Close file
	res = PR7261_FileClose(file_id_enc, CFG_OUT_FILE);

	res = PR7261_FileClose(file_id_dec, CFG_IN_FILE);

	mprintf("G723 Full Duplex test ended\n");

	return TRUE;
}

