#include <string.h>
#include "config.h"
#include "f2f.h"
#include "../drv/sport.h"
#include <math.h>

extern u16 mCodecNum ;
extern struct AudioCodec mAudioCodec[16];

static u16    mInputNum = 0;
static u16    mOutputNum = 0;
#if 1
static ufloat* mRxChannel[MAX_INPUT_NUM];
#else
static ufloat mRxChannel[MAX_INPUT_NUM][64];
#endif
static ufloat* mTxChannel[MAX_OUTPUT_NUM];

extern int local_input_channels_num;
extern int local_output_channels_num;
extern int dante_input_channels_num;
extern int dante_output_channels_num;


uvoid SetNumOfChannels(u16 rxNum, u16 txNum)
{
	mInputNum = rxNum;
	mOutputNum = txNum;
}


inline uvoid floatData(ufloat *output, const s32 *input, u32 instep, u32 length)
{
	u32 i;
	
	for(i = 0; i < length; i++)
	{
		output[i] = __builtin_conv_RtoF(input[instep*i]);
	}
}

// Unoptimized function to convert the outgoing floating-point data to 1.31 fixed-point format.
inline uvoid fixData(s32 *output, const ufloat *input, u32 outstep, u32 length)
{
	u32 i ;
	
	for(i = 0; i < length; i++)
	{
		output[outstep*i] = __builtin_conv_FtoR(input[i]);
	}
}



uvoid SetRxChannelPtr(u32 channel ,ufloat* rxBuffer)
{
	if (channel >= MAX_INPUT_NUM) {
		return ;
    }
	mRxChannel[channel] = rxBuffer;
}

uvoid SetTxChannelPtr(u32 channel, ufloat* txBuffer)
{
	if (channel >= MAX_OUTPUT_NUM) {
		return ;
	}
	mTxChannel[channel] = txBuffer;
}

uvoid MuteOutput()
{
	u32 i ;
	
	for(i =0 ;i< mOutputNum;i++) {
		memset(mTxChannel[i], 0 , SAMPLE_NUM*sizeof(ufloat));
	}
}

#define CHANNEL_OF(c) (2*((c)&(codec->slot_num-1))+ (c)/codec->slot_num)

static int mixer_process(const float* old_data ,float* data_in, float* data_out
		,float gain,int smpl_num)
{
	int i;
#if 1
	for(i=0;i<smpl_num;i++){
		data_out[i]=data_in[i]+old_data[i]-(data_in[i]*old_data[i]*(1.0f/0x7fffffff));
		data_out[i] = fclipf(data_out[i],1.0);
	}
#else
	float factor;

	factor = 1;//powf(10.0,gain/20.0);

	for(i=0;i<smpl_num;i++){
		data_out[i]=factor*data_in[i]+old_data[i];//-(data_in[i]*old_data[i]*(1.0f/0x7fffffff));
		//data_out[i] = fclipf(data_out[i],1.0);
	}
#endif
	return 0;
}

//extern int    sport0a_buf[2][NUM_SAMPLES*2];
//extern int    sport0b_buf[2][NUM_SAMPLES*2];
//extern int    sport1a_buf[2][NUM_SAMPLES*2];
//extern int    sport1b_buf[2][NUM_SAMPLES*2];
//
//extern int    sport2a_buf[2][NUM_SAMPLES*2];
//extern int    sport2b_buf[2][NUM_SAMPLES*2];
//extern int    sport3a_buf[2][NUM_SAMPLES*2];
//extern int    sport3b_buf[2][NUM_SAMPLES*2];
//
//extern int    sport4a_buf[2][NUM_SAMPLES*2];
//extern int    sport4b_buf[2][NUM_SAMPLES*2];
//extern int    sport5a_buf[2][NUM_SAMPLES*2];
//extern int    sport5b_buf[2][NUM_SAMPLES*2];
//
//extern int    sport6a_buf[2][NUM_SAMPLES*2];
//extern int    sport6b_buf[2][NUM_SAMPLES*2];
//extern int    sport7a_buf[2][NUM_SAMPLES*2];
//extern int    sport7b_buf[2][NUM_SAMPLES*2];

#if 1
uvoid UpdateInput(u32 blockIndex)
{
	u32 channel = 0;
	u32 i ,j ;
//	float pcm[SAMPLE_NUM];
//
//	static int t=0;
//
//	for(i=0;i<SAMPLE_NUM;i++){
//		pcm[i] = 0.5*sinf(2*3.1415926*1000*t/48000);
//		t++;
//	}
//	if(t>=48000){
//		t =0;
//	}
#if 1
	if(mInputNum == 0)
		return;

	for( i =0 ;i < mCodecNum ;i++) {
		struct AudioCodec* codec = &mAudioCodec[i];

		if(codec->rx) {
			s32* dataPtr = codec->dataPtr[blockIndex] ;

			for( j = 0; channel < mInputNum - 2 && j < codec->channel_num ;j ++,channel++) {
//				if(channel == (mInputNum - 2))
//					channel++;
				if(mRxChannel[channel] != NULL)
					floatData(mRxChannel[channel], dataPtr + ((j+1)&0x1),codec->slot_num, SAMPLE_NUM);
			}
		}
	}

	if(mRxChannel[mInputNum - 1] != NULL){
		floatData(mRxChannel[mInputNum - 1], mAudioCodec[9].dataPtr[blockIndex] + 0, mAudioCodec[9].slot_num, SAMPLE_NUM);
		//memcpy(mRxChannel[mInputNum - 1], pcm,  SAMPLE_NUM);
	}
#else
//	float pcm[SAMPLE_NUM];
//
//	//int i;
//	static int t=0;
//
//	for(i=0;i<SAMPLE_NUM;i++){
//		pcm[i] = 0.5*sinf(2*3.1415926*1000*t/96000);
//		t++;
//	}
//	if(t>=96000){
//		t =0;
//	}
	for( i =0 ;i < mCodecNum ;i++) {
		struct AudioCodec* codec = &mAudioCodec[i];

		if(codec->rx) {
			s32* dataPtr = codec->dataPtr[blockIndex] ;

			for( j = 0; channel < mInputNum && j < codec->channel_num ;j ++,channel++) {
				if(mRxChannel[channel] != NULL){
					if(channel >= 8/* || channel < 6*/) {
						floatData(mRxChannel[channel], dataPtr + ((j+1)&0x1),codec->slot_num, SAMPLE_NUM);
						//floatData(mRxChannel[channel], dataPtr + j,codec->slot_num, SAMPLE_NUM);
					}
//					else{
//					if(channel == 0){
//						memcpy(mRxChannel[channel], pcm, SAMPLE_NUM*sizeof(float));
//					}
					else if(channel == 6){ //iis input mix left and right
						ufloat iis_audio[2][SAMPLE_NUM], iis_out[SAMPLE_NUM];
						memset(iis_audio[0], 0, sizeof(ufloat)*SAMPLE_NUM);
						memset(iis_audio[1], 0, sizeof(ufloat)*SAMPLE_NUM);
						memset(iis_out, 0, sizeof(ufloat)*SAMPLE_NUM);
						floatData(iis_audio[0], dataPtr + 0,codec->slot_num, SAMPLE_NUM);
						floatData(iis_audio[1], dataPtr + 1,codec->slot_num, SAMPLE_NUM);
						mixer_process(iis_out,iis_audio[0],iis_out, 0,SAMPLE_NUM);
						mixer_process(iis_out,iis_audio[1],iis_out, 0,SAMPLE_NUM);
						memcpy(mRxChannel[channel], iis_out, sizeof(ufloat)*SAMPLE_NUM);
					}
					else{
						floatData(mRxChannel[channel], dataPtr + j,codec->slot_num, SAMPLE_NUM);
					}
				}
			}
		}
	}
#endif

}
#else
uvoid UpdateInput(u32 blockIndex)
{
	ufloat iis_audio[2][SAMPLE_NUM], iis_out[SAMPLE_NUM];

	memset(iis_audio[0], 0, sizeof(ufloat)*SAMPLE_NUM);
	memset(iis_audio[1], 0, sizeof(ufloat)*SAMPLE_NUM);
	memset(iis_out, 0, sizeof(ufloat)*SAMPLE_NUM);
	floatData(iis_audio[0], sport1b_buf[blockIndex] + 0,SLOTS, SAMPLE_NUM);
	floatData(iis_audio[1], sport1b_buf[blockIndex] + 1,SLOTS, SAMPLE_NUM);
	mixer_process(iis_out,iis_audio[0],iis_out, 0,SAMPLE_NUM);
	mixer_process(iis_out,iis_audio[1],iis_out, 0,SAMPLE_NUM);


	floatData(mRxChannel[0], sport2a_buf[blockIndex] + 0, SLOTS, SAMPLE_NUM);
	floatData(mRxChannel[1], sport2a_buf[blockIndex] + 1, SLOTS, SAMPLE_NUM);
	floatData(mRxChannel[2], sport2b_buf[blockIndex] + 0, SLOTS, SAMPLE_NUM);
	floatData(mRxChannel[3], sport2b_buf[blockIndex] + 1, SLOTS, SAMPLE_NUM);
	floatData(mRxChannel[4], sport1a_buf[blockIndex] + 0, SLOTS, SAMPLE_NUM);
	floatData(mRxChannel[5], sport1a_buf[blockIndex] + 1, SLOTS, SAMPLE_NUM);

	memcpy(mRxChannel[6], iis_out, sizeof(ufloat)*SAMPLE_NUM);
	floatData(mRxChannel[7], sport5a_buf[blockIndex] + 0, SLOTS, SAMPLE_NUM);

	if(dante_input_channels_num > 0){
		floatData(mRxChannel[8],  sport6a_buf[blockIndex] + 1, SLOTS, SAMPLE_NUM);
		floatData(mRxChannel[9],  sport6a_buf[blockIndex] + 0, SLOTS, SAMPLE_NUM);
		floatData(mRxChannel[10], sport6b_buf[blockIndex] + 1, SLOTS, SAMPLE_NUM);
		floatData(mRxChannel[11], sport6b_buf[blockIndex] + 0, SLOTS, SAMPLE_NUM);
	}
}
#endif

#if 1
uvoid UpdateOutput(u32 blockIndex)
{
	u32 channel = 0;
	u32 i ,j ;
#if 0
	//mOutputNum = 7;
	for( i =0 ;i < mCodecNum ;i++) {
		struct AudioCodec* codec = &mAudioCodec[i];

		if(codec->rx == ufalse) {
			s32* dataPtr = codec->dataPtr[blockIndex] ;

			for( j = 0; channel < mOutputNum && j < codec->channel_num ;j ++,channel++) {
				if(mTxChannel[channel] != NULL){
					if(channel >= 7/* || channel < 6*/) {
						fixData(dataPtr + ((j+1)&0x1), mTxChannel[channel], codec->slot_num, SAMPLE_NUM);
						//fixData(dataPtr + j, mTxChannel[channel], codec->slot_num, SAMPLE_NUM);
					}
					else if(channel == 6){
						fixData(dataPtr + 0, mTxChannel[channel], codec->slot_num, SAMPLE_NUM);
						fixData(dataPtr + 1, mTxChannel[channel], codec->slot_num, SAMPLE_NUM);
					}
					else{
						fixData(dataPtr + j, mTxChannel[channel], codec->slot_num, SAMPLE_NUM);
					}
					//fixData(dataPtr + (codec->channel_num > 1 ? ((j+1)&0x1) : j), mTxChannel[channel], codec->slot_num, SAMPLE_NUM);
					//fixData(dataPtr + ((j+1)&0x1), mTxChannel[channel], codec->slot_num, SAMPLE_NUM);
				}
			}
		}
	}
#else
//	float pcm[SAMPLE_NUM];
//
//	int i;
//	static int t=0;
//
//	for(i=0;i<SAMPLE_NUM;i++){
//		pcm[i] = 0.5*sinf(2*3.1415926*1000*t/48000);
//		t++;
//	}
//	if(t>=48000){
//		t =0;
//	}
	if(mOutputNum == 0)
		return;

	for( i =0 ;i < mCodecNum ;i++) {
		struct AudioCodec* codec = &mAudioCodec[i];

		if(codec->rx == ufalse) {
			s32* dataPtr = codec->dataPtr[blockIndex] ;

			for( j = 0; channel < mOutputNum - 1 && j < codec->channel_num ;j ++,channel++) {
				if(mTxChannel[channel] != NULL){
//					if(channel == 2 || channel == 3){
//						fixData(dataPtr + ((j+1)&0x1), mTxChannel[channel], codec->slot_num, SAMPLE_NUM);
//					}
//					else{
						fixData(dataPtr + ((j+1)&0x1), mTxChannel[channel], codec->slot_num, SAMPLE_NUM);
//					}
				}
			}
		}
	}

	if(mTxChannel[mOutputNum - 1] != NULL){
		fixData(mAudioCodec[8].dataPtr[blockIndex] + 0, mTxChannel[mOutputNum - 1], mAudioCodec[8].slot_num, SAMPLE_NUM);
		fixData(mAudioCodec[8].dataPtr[blockIndex] + 1, mTxChannel[mOutputNum - 1], mAudioCodec[8].slot_num, SAMPLE_NUM);
	}
#endif
}
#else
uvoid UpdateOutput(u32 blockIndex)
{
	fixData(sport0a_buf[blockIndex] + 0, mTxChannel[0], SLOTS, SAMPLE_NUM);
	fixData(sport0a_buf[blockIndex] + 1, mTxChannel[1], SLOTS, SAMPLE_NUM);
	fixData(sport3a_buf[blockIndex] + 0, mTxChannel[2], SLOTS, SAMPLE_NUM);
	fixData(sport3a_buf[blockIndex] + 1, mTxChannel[3], SLOTS, SAMPLE_NUM);
	fixData(sport3b_buf[blockIndex] + 0, mTxChannel[4], SLOTS, SAMPLE_NUM);
	fixData(sport3b_buf[blockIndex] + 1, mTxChannel[5], SLOTS, SAMPLE_NUM);

	fixData(sport4a_buf[blockIndex] + 0, mTxChannel[6], SLOTS, SAMPLE_NUM);
	fixData(sport4a_buf[blockIndex] + 1, mTxChannel[6], SLOTS, SAMPLE_NUM);

	if(dante_output_channels_num > 0){
		fixData(sport7a_buf[blockIndex] + 0, mTxChannel[7],  SLOTS, SAMPLE_NUM);
		fixData(sport7a_buf[blockIndex] + 1, mTxChannel[8],  SLOTS, SAMPLE_NUM);
		fixData(sport7b_buf[blockIndex] + 0, mTxChannel[9],  SLOTS, SAMPLE_NUM);
		fixData(sport7b_buf[blockIndex] + 1, mTxChannel[10], SLOTS, SAMPLE_NUM);
	}
}
#endif