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

static u16 gCodecNum[COUNT] ;
static const AudioCodec* gAudioCodec[COUNT];

static u16   mInputNum[COUNT] = {0,0,0};
static u16   mOutputNum[COUNT] = {0,0,0};
static ufloat* mRxChannel[COUNT][PHY_INPUT_NUM];
static ufloat* mTxChannel[COUNT][PHY_OUTPUT_NUM];

uvoid SetAudioCodec(eIntDataType type, u16 codec_num , const AudioCodec* pcodec)
{
	gCodecNum[type] = codec_num;
	gAudioCodec[type] = pcodec;
}

uvoid SetNumOfChannels(eIntDataType type ,u16 rxNum, u16 txNum)
{
	if(rxNum > PHY_INPUT_NUM) rxNum = PHY_INPUT_NUM;
	if(txNum > PHY_OUTPUT_NUM) txNum = PHY_OUTPUT_NUM;

	mInputNum[type] = rxNum;
	mOutputNum[type] = txNum;
}


inline uvoid floatData(ufloat *output, const s32 *input, u32 instep, u32 length)
{
	for(u32 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)
{
	for(u32 i = 0; i < length; i++){
		output[outstep*i] = __builtin_conv_FtoR(input[i]);
	}
}



uvoid SetRxChannelPtr(eIntDataType type ,u32 channel ,ufloat* rxBuffer)
{
	if (type < COUNT && channel < PHY_INPUT_NUM) {
		mRxChannel[type][channel] = rxBuffer;
    }
}

uvoid SetTxChannelPtr(eIntDataType type ,u32 channel, ufloat* txBuffer)
{
	if (type < COUNT && channel < PHY_OUTPUT_NUM) {
		mTxChannel[type][channel] = txBuffer;
	}
}


uvoid SetTxRxNullBufferPtr()
{
	memset(mRxChannel, 0, sizeof(ufloat*)*COUNT*PHY_INPUT_NUM);
	memset(mTxChannel, 0, sizeof(ufloat*)*COUNT*PHY_OUTPUT_NUM);
}

uvoid MuteOutput()
{
	for(s32 type =0 ; type < COUNT ;type ++) {
		u16 channel = 0;
		u16 output_num = mOutputNum[type];
		ufloat** pTxChannel = mTxChannel[type];
		const AudioCodec* codec_arr = gAudioCodec[type];

		for(u32 i =0 ;i < gCodecNum[type] ;i++) {
			const AudioCodec* codec = &codec_arr[i];
			s32* dataPtr0 = codec->dataPtr[0] ;
			s32* dataPtr1 = codec->dataPtr[1] ;

			if(codec->rx == utrue) continue;
			if(codec->enable_sec){
				memset(dataPtr0, 0 ,sizeof(s32)*codec->slot_num*2*SAMPLE_NUM);
				memset(dataPtr1, 0 ,sizeof(s32)*codec->slot_num*2*SAMPLE_NUM);
			}
			else{
				memset(dataPtr0, 0 ,sizeof(s32)*codec->slot_num*SAMPLE_NUM);
				memset(dataPtr1, 0 ,sizeof(s32)*codec->slot_num*SAMPLE_NUM);
			}
		}
	}
}

#define CHANNEL_OF(c) (2*((c)&(codec->slot_num-1))+ (c)/codec->slot_num)
#define OFFSET(j) (j<codec->channel_num?CHANNEL_OF(j): CHANNEL_OF(j-codec->channel_num+codec->slot_num))
/*
 * sec ÅÅÁз½Ê½: 0,16,1,17,2,18,...,15,31
 */
uvoid UpdateInput(u32 iid ,u32 blockIndex)
{
	for(s32 type = 0; type < COUNT ;type ++) {
		u16 channel = 0;
		u16 input_num = mInputNum[type];
		ufloat** pRxChannel = mRxChannel[type];
		const AudioCodec* codec_arr = gAudioCodec[type];

		for(u32 i =0 ;i < gCodecNum[type] ;i++) {
			const AudioCodec* codec = &codec_arr[i];
			if(codec->rx == ufalse) continue;

			ubool enable = (iid == codec->follow_intr_no);
			s32* dataPtr = codec->dataPtr[blockIndex];

			if(codec->enable_sec){
				for(u32 j = 0; channel < input_num && j < codec->channel_num*2 ;j ++,channel++) {
					if(enable && pRxChannel[channel] != NULL)
						floatData(pRxChannel[channel], dataPtr + OFFSET(j),codec->slot_num*2, SAMPLE_NUM);
				}
			}
			else{
				for(u32 j = 0; channel < input_num && j < codec->channel_num ;j ++,channel++) {
					if(enable && pRxChannel[channel] != NULL)
						floatData(pRxChannel[channel], dataPtr + j,codec->slot_num, SAMPLE_NUM);
				}
			}
		}
	}
}


void in2out_bypass(ufloat* out[], ufloat* in[], int ch, int len)
{
	int i, j;
	for (i = 0; i < ch; i++)
		for (j = 0; j < len; j++) {
			out[i][j] = in[i][j];
		}
}
void sin_gen(ufloat* dat, int len)
{
	static int sta = 0;
	for (u32 i = 0; i < len; i++) {
		dat[i] = 0.4f * sinf(2 * 3.1415926f * 675 * sta++ / SAMPLE_RATE);
	}
	if (sta == SAMPLE_RATE)
		sta = 0;
}
uvoid UpdateOutput(u32 iid ,u32 blockIndex)
{
//	in2out_bypass(mTxChannel[0], mRxChannel[0], 1, SAMPLE_NUM);
//	static ufloat data[64] = {0};
//	static int odd = 0;
//	if (0 == odd++ % 3) {
//		sin_gen(data, SAMPLE_NUM);
//		for (u32 i = 0; i < SAMPLE_NUM; i++) {
//			for (u32 j = 0; j < 8; j++) {
//				mTxChannel[0][j][i] = data[i];
//			}
//		}
//	}

	for(s32 type =0 ; type < COUNT ;type ++) {
		u16 channel = 0;
		u16 output_num = mOutputNum[type];
		ufloat** pTxChannel = mTxChannel[type];
		const AudioCodec* codec_arr = gAudioCodec[type];

		for(u32 i =0 ;i < gCodecNum[type] ;i++) {
			const AudioCodec* codec = &codec_arr[i];

			if(codec->rx == utrue) continue;

			ubool enable = (iid == codec->follow_intr_no);
			s32* dataPtr = codec->dataPtr[blockIndex];

			if(codec->enable_sec){
				for(u32 j = 0; channel < output_num && j < codec->channel_num*2 ;j ++,channel++) {
					if(enable && pTxChannel[channel] != NULL) {
						s32 shift = OFFSET(j);
						fixData(dataPtr + shift, pTxChannel[channel], codec->slot_num*2, SAMPLE_NUM);
					}
				}
			}
			else{
				for(u32 j = 0; channel < output_num && j < codec->channel_num ;j ++,channel++) {
					if(enable && pTxChannel[channel] != NULL)
						fixData(dataPtr + j, pTxChannel[channel], codec->slot_num, SAMPLE_NUM);
				}
			}
		}
	}
}