
#include "ucps2.h"
#include "ucpm2.h"
#include <channelEquImpl.h>


void ChannelEquImpl(
		v16u32 * SVRReg,
		int *CfgFft4096,
		int *CfgEQ21Part1,
		int *ConfigBaseAddr3,
		int *CfgIFFT4096,
		int *ConfigDataTurn,
		int *ConfigAddCp,
		int *available_ptr_dm0,
		int *available_ptr_dm1,
		int *available_ptr_dm2,
		int *available_ptr_dm3,
		int *InChannelEst_ddr_ptr,
		int *InData_ddr_ptr,
		int *signal0,
		int *InputNoise,
		int *W4096 ,
		int *CalAddr0,
		int *CalAddr1,
		int *CalAddr2,
		int *Lut_phase,
		int res_ptr

		){

	volatile int a = 1;
	int numSym = 7;
	int Scale = 13;
	int NRE=4096;
	int ShiftFactor[] = {7,1,0,0,0,0,0,0,0,0,0,0};
	int NumCB = 2;

	for(int i=0;i<NumCB;i++){

		int *Scalep = available_ptr_dm0;
		int *Scale0 = available_ptr_dm0 + 0x0040;
		int *Scales = available_ptr_dm0 + 0x0080;

		int *tmp0 = (int*)(((int)available_ptr_dm1 + 0x4000 - 1) & ~(0x4000 - 1));             // LENGTH: 0x2000
		int *Fft_est_dm3_ptr = (int*)(((int)available_ptr_dm3 + 0x4000 - 1) & ~(0x4000 - 1));   // LENGTH: 0x1000
		int *channelEst_dm0_ptr = (int*)(((int)Scales + 0x0100 + 0x4000 - 1) & ~(0x4000 - 1));

		 /*****************************************FFT of channelEst*****************************************/

    	ape_csu_task_lookup(DMA_TAG_G2L, 1);
    	ape_csu_dma_1D_G2L_ch0ch1_transfer((uint64_t)(InChannelEst_ddr_ptr + 4096*i) ,
                             (uint64_t)DM_TO_CSU_ADDR(channelEst_dm0_ptr),
                             4096*4,
                             DMA_TAG_G2L,
                             0);

		Fft4096Int32(
			(int)CfgFft4096,
			1,
			Scale,
			ShiftFactor,
			MPU_ADDR(W4096),                            //DM2
			MPU_ADDR(channelEst_dm0_ptr),               //DM0
			MPU_ADDR(available_ptr_dm2),                //DM2
			MPU_ADDR(Fft_est_dm3_ptr),                  //DM3
			MPU_ADDR(tmp0),                             //DM1
			MPU_ADDR(Scalep),
			MPU_ADDR(Scale0)
			);
	    SVRReg[0][0] = MPU_ADDR(CfgFft4096);
		Fft4096Int32Asm(*SVRReg);
		__ucps2_getStatB();
		__ucps2_delay();


		 /*****************************************channelEqu*****************************************/

//		for(int m=0;m<4096;m++){
//			*(Fft_est_dm3_ptr + 4096 + m) = Fft_est_dm3_ptr[m];
//		}

		for(int j=0;j<numSym;j++){

			SVRReg[0] = (v16u32){0, 0, 0, 0,
								0x40, 0, 0, 0,
								0xff00ff, 0, 0, 0x0000,
								0xffff, 0x6, 0, 0};

			int *Fft_outputdata_dm3_ptr = Fft_est_dm3_ptr + 0x2000;
			int *InData_dm0_ptr = channelEst_dm0_ptr + 0x2000;

	    	ape_csu_task_lookup(DMA_TAG_G2L, 1);
	    	ape_csu_dma_1D_G2L_ch0ch1_transfer((uint64_t)(InData_ddr_ptr + (68+4096)*j + 68) ,
	                             (uint64_t)DM_TO_CSU_ADDR(InData_dm0_ptr),
	                             4096*4,
	                             DMA_TAG_G2L,
	                             0);
	    	// FFT of Data
			Fft4096Int32(
				(int)CfgFft4096,
				1,
				Scale,
				ShiftFactor,
				MPU_ADDR(W4096),
				MPU_ADDR(InData_dm0_ptr),          //DM0
				MPU_ADDR(available_ptr_dm2),       //DM2
				MPU_ADDR(Fft_outputdata_dm3_ptr),  //DM3
				MPU_ADDR(tmp0),                    //DM1
				MPU_ADDR(Scalep),
				MPU_ADDR(Scale0)
				);
			SVRReg[0][0] = MPU_ADDR(CfgFft4096);
			Fft4096Int32Asm(*SVRReg);
			__ucps2_getStatB();
			__ucps2_delay();



			// 构造第二根天线数据，与第一根相同
			for(int m=0;m<4096;m++){
				*(Fft_outputdata_dm3_ptr + 4096 + m) = Fft_outputdata_dm3_ptr[m];
			}

			SVRReg[0] = (v16u32){0, 0, 0, 0,
								0x40, 0, 0, 0,
								0xff00ff, 0, 0, 0x0000,
								0xffff, 0x6, 0, 0};

			int *Equ_Output = InData_dm0_ptr + 0x1000;			 	        //LENGTH 0x2000 (the second ant occupied additional space 0x1000)
			int *InOut2_dm1_ptr = tmp0;				                        //LENGTH 0x1000
			int *InOut3_dm0_ptr = InData_dm0_ptr + 0x3000;  				//LENGTH 0x2000
			int *InOut1_dm1_ptr = tmp0 + 0x1000;          	                //LENGTH 0x1000


			EQ21Part1(
				CfgEQ21Part1,
				NRE,
				1,
				MPU_ADDR(Fft_est_dm3_ptr),       	//dm3  信道估计fft
				MPU_ADDR(InputNoise),            	//dm3  噪声全0
				MPU_ADDR(Fft_outputdata_dm3_ptr), 	//dm3  数据fft
				MPU_ADDR(Equ_Output),           	//dm0
				MPU_ADDR(InOut1_dm1_ptr),       	//dm1
				MPU_ADDR(InOut2_dm1_ptr),    		//dm1
				MPU_ADDR(InOut3_dm0_ptr)     		//dm0
				);

			SVRReg[0][0] = MPU_ADDR(CfgEQ21Part1);
			EQ21Part1Asm(*SVRReg);
			__ucps2_getStatB();
			__ucps2_delay();



			int *Equ_Output_2 = available_ptr_dm2;                  //第一根天线均衡输出
			int *Equ_Output_21 = available_ptr_dm2 + 0x2000;        //第二根天线均衡输出

			EQ1Part2(
					ConfigBaseAddr3,
					NRE,
					1,
					MPU_ADDR(Equ_Output),
					MPU_ADDR(InOut1_dm1_ptr),
					MPU_ADDR(InOut2_dm1_ptr),
					MPU_ADDR(InOut3_dm0_ptr),
					MPU_ADDR(Equ_Output_2),
					MPU_ADDR(Equ_Output_21)
					);

			SVRReg[0][0] = MPU_ADDR(ConfigBaseAddr3);
			EQ1Part2Asm(*SVRReg);
			a = __ucps2_getStatB();
			__ucps2_delay();


//			for (size_t i = 0; i < 4096; i++) {
//				int32_t num = Equ_Output_2[i];
//
//				int16_t real = (int16_t)(num & 0xFFFF);
//				int16_t imag = (int16_t)(num >> 16);
//				real /= 8;
//				imag /= 8;
//
//				*(Equ_Output + i) = ((int32_t)imag << 16) | ((uint16_t)real);
//			}


			int *Temp1 = Fft_outputdata_dm3_ptr;           // 缓存

			IFFT4096DataTurn(
					(int)ConfigDataTurn,
					1,
					1,
					MPU_ADDR(Equ_Output_2), //dm2
					MPU_ADDR(tmp0),         //dm1  无效
					MPU_ADDR(Equ_Output),   //dm0
					MPU_ADDR(Temp1)         //dm3  无效
					);

			SVRReg[0] = (v16u32){0, 0, 0, 0,
								0x40, 0, 0, 0,
								0xff00ff, 0, 0, 0x0000,
								0xffff, 0x6, 0, 0};

			SVRReg[0][0] = MPU_ADDR(ConfigDataTurn);
			IFFT4096DataTurnAsm(*SVRReg);
			__ucps2_getStatB();
			__ucps2_delay();



			int *Temp0 = available_ptr_dm2;                // buff, Equ_Output_2

			IFFT4096(
				(int)CfgIFFT4096,
				1,
				MPU_ADDR(Temp0),       //DM2
				MPU_ADDR(Temp1),       //DM3
				MPU_ADDR(Equ_Output),  //DM0
				MPU_ADDR(tmp0),        //DM1
				MPU_ADDR(CalAddr0),    //DM2
				MPU_ADDR(CalAddr1),    //DM3
				MPU_ADDR(CalAddr2));   //DM1
			SVRReg[0][0] = MPU_ADDR(CfgIFFT4096);
			IFFT4096Asm(*SVRReg);
			__ucps2_getStatB();
			__ucps2_delay();


			int CPLength0 = 352;
			int CPLength1 = 288;

			AddCP(
				(int) ConfigAddCp,
				2,
				1,
				CPLength0,
				CPLength1,
				MPU_ADDR(Equ_Output),      //DM0
				MPU_ADDR(tmp0),     		//DM1  invalid
				MPU_ADDR(Equ_Output_2),    //DM2
				MPU_ADDR(Temp1),           //DM3   invalid
				MPU_ADDR(Lut_phase)        //
				);

				SVRReg[0][0] = MPU_ADDR(ConfigAddCp);
				AddCPAsm(*SVRReg);
				a = __ucps2_getStatB();
				__ucps2_delay();


	    	ape_csu_dma_1D_L2G_ch0ch1_transfer(
	    								(uint64_t)DM_TO_CSU_ADDR((Equ_Output_2+CPLength0)),
	    	                            (uint64_t)res_ptr,
										4096*4 ,
	    	                            DMA_TAG_L2G,
	    	                            1);

	    	res_ptr = res_ptr + 16384;
		}

	}

	return;
}


