/*
 * Copyright 2018-2025 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <xtensa/config/core.h>
#include <xtensa/xos.h>

#include <stdio.h>

#include "fsl_device_registers.h"
#include "fsl_debug_console.h"

#include "fsl_dma.h"
#include "fsl_mu.h"
#include "fsl_sema42.h"

#include "board_hifi4.h"
#include "fsl_common.h"
#include "fsl_gpio.h"
#include "fsl_inputmux.h"

#include "GlobalDef.h"
#include "AudioProcess.h"
#include "Sweep.h"
#include "CadenceAsrcProc.h"

#define EnableKd							0

#define Def_KpValueInPNoIControl			(1/3000.0f)			//use this value when AsrcBufLengthInSamples is 360
//#define Def_KpValueInPNoIControl			(1/500.0f)			//use this value when AsrcBufLengthInSamples is 60


//default starting values
#define Def_KpValueAtFsIn48KHz				(1.0f/  39500.0f)
#define Def_KiValueAtFsIn48KHz				(1.0f/1750000.0f)
#define Def_KdValueAtFsIn48KHz				(-1.0f/17000.0f)
#define Def_PI_ErrAccMaxValueAtFsIn48KHz	(100000.0f)

#define Def_KpValueAtFsIn32KHz				(1.0f/  39500.0f)
#define Def_KiValueAtFsIn32KHz				(1.0f/1750000.0f)
#define Def_KdValueAtFsIn32KHz				(-1.0f/17000.0f)
#define Def_PI_ErrAccMaxValueAtFsIn32KHz	(100000.0f)

int PI_ErrAccMaxValue;

extern int NonCacheable_start, NonCacheable_end;
extern int NonCacheable_init_start, NonCacheable_init_end;

extern volatile float AsrcDriftingValueCurrent;
extern volatile float AsrcDriftingValueTarget;
extern volatile int AsrcFsInCurrent;
extern volatile int AsrcFsInTarget;

extern float AudioSrcFltBuf1[AudioFrameSizeInSamplePerCh];
extern float AudioSrcFltBuf2[AudioFrameSizeInSamplePerCh];


extern void AudioProcess_ForTraining(void);
extern void AudioProcess_Test(void);

uint32_t CycCntA,CycCntB;
unsigned int AudioFrameCnt=0;

void APP_MU_IRQHandler(void);

void XOS_INIT(void)
{
    xos_set_clock_freq(CLOCK_GetFreq(kCLOCK_DspCpuClk));
	xos_start_system_timer(-1, 0);

    /* DSP interrupt only can be enable after XOS is started. */
    xos_register_interrupt_handler(APP_MU_IRQn, (XosIntFunc *)APP_MU_IRQHandler, NULL);
    xos_interrupt_enable(APP_MU_IRQn);
}

uint8_t domainId;

int AsrcOutputL[AudioFrameSizeInSamplePerCh*2];
int AsrcOutputR[AudioFrameSizeInSamplePerCh*2];

//#define AOD_AverageLength	16
#define AOD_AverageLength	8
//#define AOD_AverageLength	4
int AsrcBufAmountOfData[AOD_AverageLength];

float Kp;
float Ki;
float Kd;
int PI_Control_SampleAOD_ErrAccumulated;
int PreErr;

//set PID coefficients to default starting values
float KpValueInPNoIControl=Def_KpValueInPNoIControl;

float KpValueAtFsIn48KHz=Def_KpValueAtFsIn48KHz;
float KiValueAtFsIn48KHz=Def_KiValueAtFsIn48KHz;
float KdValueAtFsIn48KHz=Def_KdValueAtFsIn48KHz;
float PI_ErrAccMaxValueAtFsIn48KHz=Def_PI_ErrAccMaxValueAtFsIn48KHz;

float KpValueAtFsIn32KHz=Def_KpValueAtFsIn32KHz;
float KiValueAtFsIn32KHz=Def_KiValueAtFsIn32KHz;
float KdValueAtFsIn32KHz=Def_KdValueAtFsIn32KHz;
float PI_ErrAccMaxValueAtFsIn32KHz=Def_PI_ErrAccMaxValueAtFsIn32KHz;

int AudioProcFrameCnt=0;

#if EnableAsrcLatencyTest==1
int AsrcLatencyTestCnt=0;
#endif

void APP_MU_IRQHandler(void)
{
    uint32_t flag = 0;
    uint32_t MU_U32InfoFromMcu;

    flag = MU_GetStatusFlags(APP_MU);

    if ((flag & kMU_Rx0FullFlag) == kMU_Rx0FullFlag)
    {
    	MU_U32InfoFromMcu = MU_ReceiveMsgNonBlocking(APP_MU, CHN_MU_REG_NUM);

		if(MU_U32InfoFromMcu==0x1000)
		{
			//audio processing for every local Fs synchronized audio frame --- 0x1000 stands for audio processing for each audio frame that is synchronized with local I2S Fs
			DbgPin3Up;
			AudioProcFrameCnt++;

			CycCntA=read_ccount();
				AudioProcess_Test();
			CycCntB=read_ccount();

			VarBlockSharedByDspAndMcu.U32CycCntHistory[AudioProcFrameCnt%100]= CycCntB-CycCntA;

			DbgPin3Dn;
		}else
		{
			//ASRC converting processing triggered by external I2S or USB audio downstreaming --- MU_U32InfoFromMcu is the value of ASRC input samples of each ch
			DbgPin4Up;

			//step 1: calculate the averaged Amound Of Data of VarBlockSharedByDspAndMcu.CirBufUsbAudioDnStreamingL --- to get AmountOfDataAcc
			int AsrcOutputSampleNum;

		    SEMA42_Lock(APP_SEMA42, SEMA42_GATE, domainId);
				int BufAmountOfData=CirAudioBuf_SpaceOccupiedInSamples_S32(&VarBlockSharedByDspAndMcu.CirBufUsbAudioDnStreamingL);
		    SEMA42_Unlock(APP_SEMA42, SEMA42_GATE);

			//calculate AsrcDriftingValueTarget
			int AmountOfDataAcc=0;

			for(int i=0;i<(AOD_AverageLength-1);i++)
			{
				AsrcBufAmountOfData[i]=AsrcBufAmountOfData[i+1];
				AmountOfDataAcc+=AsrcBufAmountOfData[i+1];
			}
			AsrcBufAmountOfData[AOD_AverageLength-1]=BufAmountOfData;
			AmountOfDataAcc+=BufAmountOfData;

			//step 2: PID calculation (P control or PID control) --- to get AsrcDriftingValueTarget
			int Err;

			#if AOD_AverageLength==16
				Err=(float)(AsrcBufLengthInSamples/2)-(float)(AmountOfDataAcc>>4);
			#endif
			#if AOD_AverageLength==8
				Err=(float)(AsrcBufLengthInSamples/2)-(float)(AmountOfDataAcc>>3);
			#endif
			#if AOD_AverageLength==4
				Err=(float)(AsrcBufLengthInSamples/2)-(float)(AmountOfDataAcc>>2);
			#endif

			//update PID coefficients if needed
			if(VarBlockSharedByDspAndMcu.ControlPara[3])
			{
				//need to update Kp Ki Kd values from CM33 side VCOM
				KpValueInPNoIControl=1.0f/VarBlockSharedByDspAndMcu.ControlPara[4];

				KpValueAtFsIn48KHz=1.0f/(int)VarBlockSharedByDspAndMcu.ControlPara[4];
				KiValueAtFsIn48KHz=1.0f/(int)VarBlockSharedByDspAndMcu.ControlPara[5];
				KdValueAtFsIn48KHz=1.0f/(int)VarBlockSharedByDspAndMcu.ControlPara[6];
				PI_ErrAccMaxValueAtFsIn48KHz=(int)VarBlockSharedByDspAndMcu.ControlPara[7];

				KpValueAtFsIn32KHz=1.0f/(int)VarBlockSharedByDspAndMcu.ControlPara[4];
				KiValueAtFsIn32KHz=1.0f/(int)VarBlockSharedByDspAndMcu.ControlPara[5];
				KdValueAtFsIn32KHz=1.0f/(int)VarBlockSharedByDspAndMcu.ControlPara[6];
				PI_ErrAccMaxValueAtFsIn32KHz=(int)VarBlockSharedByDspAndMcu.ControlPara[7];

				VarBlockSharedByDspAndMcu.ControlPara[3]=0;
			}

			if(VarBlockSharedByDspAndMcu.ControlPara[2]==2)
			{
				//now in USB audio downstreaming playing, PNoI control --- and decrease Kp by *0.05
				Kp=KpValueInPNoIControl*0.05;
				AsrcDriftingValueTarget=Kp*Err;
				PI_Control_SampleAOD_ErrAccumulated=0;
				PreErr=0;
			}else if(VarBlockSharedByDspAndMcu.ControlPara[2]==1)
			{
				//not in USB audio downstreaming playing, PNoI control --- no need to decrease Kp by *0.05
				Kp=KpValueInPNoIControl;
				AsrcDriftingValueTarget=Kp*Err;
				PI_Control_SampleAOD_ErrAccumulated=0;
				PreErr=0;
			}else
			{
				//PID control
				if(AsrcFsInTarget==48000)
				{
					Kp=KpValueAtFsIn48KHz;
					Ki=KiValueAtFsIn48KHz;
					Kd=KdValueAtFsIn48KHz;
					PI_ErrAccMaxValue=PI_ErrAccMaxValueAtFsIn48KHz;
				}else
				{
					Kp=KpValueAtFsIn32KHz;
					Ki=KiValueAtFsIn32KHz;
					Kd=KdValueAtFsIn32KHz;
					PI_ErrAccMaxValue=PI_ErrAccMaxValueAtFsIn32KHz;
				}


				PI_Control_SampleAOD_ErrAccumulated+=Err;
				if(PI_Control_SampleAOD_ErrAccumulated>PI_ErrAccMaxValue)
					PI_Control_SampleAOD_ErrAccumulated=PI_ErrAccMaxValue;
				if(PI_Control_SampleAOD_ErrAccumulated<-PI_ErrAccMaxValue)
					PI_Control_SampleAOD_ErrAccumulated=-PI_ErrAccMaxValue;

				#if EnableKd==1
					AsrcDriftingValueTarget=Kp*Err+Ki*PI_Control_SampleAOD_ErrAccumulated+Kd*(Err-PreErr);
				#else
					AsrcDriftingValueTarget=Kp*Err+Ki*PI_Control_SampleAOD_ErrAccumulated;
				#endif

				PreErr=Err;
			}

			#if 0
				//can be closed --- only to watch PID coefficients
				float *f1=(float *)&VarBlockSharedByDspAndMcu.ControlPara[8];
				float *f2=(float *)&VarBlockSharedByDspAndMcu.ControlPara[9];
				float *f3=(float *)&VarBlockSharedByDspAndMcu.ControlPara[10];
				float *f4=(float *)&VarBlockSharedByDspAndMcu.ControlPara[11];

				*f1=Kp;
				*f2=Ki;
				*f3=Kd;
				*f4=(float)PI_ErrAccMaxValue;
			#endif


			if(AsrcDriftingValueTarget> 0.03999f) AsrcDriftingValueTarget= 0.03999f;
			if(AsrcDriftingValueTarget<-0.03999f) AsrcDriftingValueTarget=-0.03999f;


			DbgPin4Dn;

			//step 3: check if need to use internal signal generator to over-write the received external I2S audio data
			if(VarBlockSharedByDspAndMcu.ControlPara[1])
			{
				//to use internal signal generator as filter testing signal source
				if(VarBlockSharedByDspAndMcu.ControlPara[1]==1)
					//sweeping is selected
					GenerateSineTone(&SineToneGenerator1, AudioSrcFltBuf1, AudioFrameSizeInSamplePerCh,1);
				else
					//1KHz is selected
					GenerateSineToneSingleFreq(&SineToneGenerator2, AudioSrcFltBuf1, AudioFrameSizeInSamplePerCh,1);

				//copy the tone of the first generator to the next buffer with *-1
				#if EnableAsrcLatencyTest==1
					if((AsrcLatencyTestCnt%20000)<1000)
					{
						for(int i=0;i<AudioFrameSizeInSamplePerCh;i++)
							AudioSrcFltBuf2[i]=AudioSrcFltBuf1[i]*-0.5f;
					}else
					{
						for(int i=0;i<AudioFrameSizeInSamplePerCh;i++)
							AudioSrcFltBuf2[i]=AudioSrcFltBuf1[i]*-1.0f;
					}
				#else
					for(int i=0;i<AudioFrameSizeInSamplePerCh;i++)
						AudioSrcFltBuf2[i]=AudioSrcFltBuf1[i]*-1.0f;
				#endif


				//convert and mix generated signal data to AsrcInBuf --- Int, LR mixed
				for(int i=0;i<AudioFrameSizeInSamplePerCh;i++)
				{
					VarBlockSharedByDspAndMcu.AsrcInBuf[2*i+0]=AudioSrcFltBuf1[i]*(float)_Pow2_31_M1_;
					VarBlockSharedByDspAndMcu.AsrcInBuf[2*i+1]=AudioSrcFltBuf2[i]*(float)_Pow2_31_M1_;
				}
			}

			//step 4: call the main ASRC process
			CycCntA=read_ccount();
				//ASRC process, input sample number is in MU_U32InfoFromMcu, AsrcOutputSampleNum will be written by the ASRC main process
				DbgPin4Up;
				ProcCadenceAsrc((int *)VarBlockSharedByDspAndMcu.AsrcOutBuf, (int *)VarBlockSharedByDspAndMcu.AsrcInBuf, MU_U32InfoFromMcu, &AsrcOutputSampleNum);
				DbgPin4Dn;
			CycCntB=read_ccount();
			VarBlockSharedByDspAndMcu.CycCnt[0]= PI_Control_SampleAOD_ErrAccumulated;


			//step 5: put the generated audio to circular buffer
			//fill AsrcOutputSampleNum samples from VarBlockSharedByDspAndMcu.AsrcOutBuf to circular buffer
			for(int i=0;i<AsrcOutputSampleNum;i++)
			{
				AsrcOutputL[i]=VarBlockSharedByDspAndMcu.AsrcOutBuf[2*i+0];
				AsrcOutputR[i]=VarBlockSharedByDspAndMcu.AsrcOutBuf[2*i+1];
			}

			DbgPin4Up;

		    SEMA42_Lock(APP_SEMA42, SEMA42_GATE, domainId);
				if(CirAudioBuf_SpaceAvailableInSamples_S32(&VarBlockSharedByDspAndMcu.CirBufUsbAudioDnStreamingL)>=AsrcOutputSampleNum)
				{
					CirAudioBuf_WriteSamples_S32(&VarBlockSharedByDspAndMcu.CirBufUsbAudioDnStreamingL, AsrcOutputSampleNum, AsrcOutputL);
					CirAudioBuf_WriteSamples_S32(&VarBlockSharedByDspAndMcu.CirBufUsbAudioDnStreamingR, AsrcOutputSampleNum, AsrcOutputR);
				}else
				{
					PRINTF("F");
				}
		    SEMA42_Unlock(APP_SEMA42, SEMA42_GATE);

			//step 6: put some debug values to CycCnt, and will be printed by MCU side to the USB COM host
			VarBlockSharedByDspAndMcu.CycCnt[1]= AsrcDriftingValueTarget*10000;
			VarBlockSharedByDspAndMcu.CycCnt[2]=BufAmountOfData;

			#if AOD_AverageLength==16
				VarBlockSharedByDspAndMcu.CycCnt[3]=(AmountOfDataAcc>>4);
			#endif
			#if AOD_AverageLength==8
				VarBlockSharedByDspAndMcu.CycCnt[3]=(AmountOfDataAcc>>3);
			#endif
			#if AOD_AverageLength==4
				VarBlockSharedByDspAndMcu.CycCnt[3]=(AmountOfDataAcc>>2);
			#endif

			//VarBlockSharedByDspAndMcu.CycCnt[3]=AsrcOutputSampleNum;
			VarBlockSharedByDspAndMcu.CycCnt[3]=CycCntB-CycCntA;

			//other watch values to be printed
			#if 0
				VarBlockSharedByDspAndMcu.CycCnt[4]=BufAmountOfData;
				VarBlockSharedByDspAndMcu.CycCnt[5]=AsrcOutputSampleNum;
			#endif

			#if 0
				if(BufAmountOfData<=6)
					VarBlockSharedByDspAndMcu.CycCnt[6]++;
				if(BufAmountOfData>=(AsrcBufLengthInSamples-6))
					VarBlockSharedByDspAndMcu.CycCnt[7]++;
			#endif

			#if 0
				if(AsrcOutputSampleNum==6)
				{
					VarBlockSharedByDspAndMcu.CycCnt[8]++;
				}else
				{
					if(AsrcOutputSampleNum==5)
					{
						VarBlockSharedByDspAndMcu.CycCnt[9]++;
					}else
					if(AsrcOutputSampleNum==7)
					{
						VarBlockSharedByDspAndMcu.CycCnt[10]++;
					}else
					{
						VarBlockSharedByDspAndMcu.CycCnt[11]++;
					}
				}
			#endif

			#if 0
				VarBlockSharedByDspAndMcu.CycCnt[12]=AsrcDriftingValueCurrent*10000;
				VarBlockSharedByDspAndMcu.CycCnt[13]=AsrcDriftingValueTarget*10000;

				#if AOD_AverageLength==16
					VarBlockSharedByDspAndMcu.CycCnt[14]=(AmountOfDataAcc>>4);
				#endif
				#if AOD_AverageLength==8
					VarBlockSharedByDspAndMcu.CycCnt[14]=(AmountOfDataAcc>>3);
				#endif
				#if AOD_AverageLength==4
					VarBlockSharedByDspAndMcu.CycCnt[14]=(AmountOfDataAcc>>2);
				#endif

				VarBlockSharedByDspAndMcu.CycCnt[15]=AsrcFsInTarget;
			#endif

			DbgPin4Dn;
		}



        MU_SendMsgNonBlocking(APP_MU, CHN_MU_REG_NUM, 0x00000001);
    }
}

void BOARD_InitClock(void)
{
	/* DSP_INT0_SEL18 = DMA1 */
	INPUTMUX_AttachSignal(INPUTMUX, 18U, kINPUTMUX_Dmac1ToDspInterrupt);
}

uint8_t APP_GetDSPCoreDomainID(void)
{
    return 3U;
}

#ifdef SoftSimu
	extern int TestCadenceAsrcmain(int argc, char **argv);
#endif
int main(void)
{
	#ifdef SoftSimu
		TestCadenceAsrcmain(1, NULL);
	#endif

	xos_start_main_ex("main", 7, 1024*8);		//xos_start_main_ex() Init XOS and convert main() into a thread, stack size is set to 8*1024 byte

	XOS_INIT();

	/* Disable DSP cache for non-cacheable sections. */
	xthal_set_region_attribute((uint32_t *)0x20070000,0xb0000, XCHAL_CA_BYPASS, 0);

	BOARD_InitDebugConsole();
	BOARD_InitClock();

	/* MUB init */
	MU_Init(APP_MU);

	/* Send flag to CM33 core to indicate DSP Core has started */
	MU_SetFlags(APP_MU, BOOT_FLAG);

    /* SEMA42 init */
    SEMA42_Init(APP_SEMA42);
    domainId = APP_GetDSPCoreDomainID();

    SEMA42_Lock(APP_SEMA42, SEMA42_GATE, domainId);
		PRINTF("RT685 DSP: started \r\n");
    SEMA42_Unlock(APP_SEMA42, SEMA42_GATE);


	//set to 48KHz FsIn as starting --- only 48KHz local I2S Fs is considered in this demo
	//                                          Amplitude(),      FreqBeg,   FreqEnd,  Sweeping Duration,  Fs
	//                                        in between +/- 1      Hz          Hz            Sec          Hz
	InitSineToneGenerator(&SineToneGenerator1,     0.99f,          0.1f,      24000.1f,      20.0f,        48000);
	InitSineToneGenerator(&SineToneGenerator2,     0.99f,          1000.0f,    1000.0f,      20.0f,        48000);
	//                 (int InputBlockSizeInSamples, int inFs, int outFs, float AsrcInitialDrifting, int ChNum,  int NeedToDisplay)
	InitCadenceAsrc (6,                               48000,       48000,          0.0001,                 2  ,                1  );
	PI_Control_SampleAOD_ErrAccumulated=0;
	PreErr=0;

	Kp=KpValueAtFsIn48KHz;
	Ki=KiValueAtFsIn48KHz;
	PI_ErrAccMaxValue=PI_ErrAccMaxValueAtFsIn48KHz;

	/* Enable transmit and receive interrupt */
	MU_EnableInterrupts(APP_MU, kMU_Rx0FullInterruptEnable);

    SEMA42_Lock(APP_SEMA42, SEMA42_GATE, domainId);
		PRINTF("RT685 DSP: MU interrupt is now enabled\r\n");
    SEMA42_Unlock(APP_SEMA42, SEMA42_GATE);

	while(1)
	{
		//check if need to switch between 48KHz or 32KHz --- must ensure selected Fs equal to the real external I2S source Fs
		//if external I2S source is (around)48KHz and you select 32KHz FsIn, signal will be broken
		//if external I2S source is (around)32KHz and you select 48KHz FsIn, signal will be broken
		AsrcFsInTarget=VarBlockSharedByDspAndMcu.ControlPara[0];

		if(AsrcFsInTarget!=48000)
			AsrcFsInTarget=32000;	//only 48000 and 32000 are considered in this demo prj

		if(AsrcFsInTarget!=AsrcFsInCurrent)
		{
			//need to switch --- first deinit and then init again with the target Fs value
			AsrcDriftingValueTarget=0.0f;
			PI_Control_SampleAOD_ErrAccumulated=0;
			PreErr=0;

			//DbgPin3Up;
			DeinitCadenceAsrc();
			//DbgPin3Dn;

			//DbgPin4Up;
			//              (int InputBlockSizeInSamples, int inFs,    int outFs, float AsrcInitialDrifting, int ChNum,  int NeedToDisplay)
			InitCadenceAsrc (6,                      AsrcFsInTarget,     48000,           0.0001,                2,                    0  );

			if(AsrcFsInTarget==48000)
			{
				//                                          Amplitude(),      FreqBeg,   FreqEnd,  Sweeping Duration,  Fs
				//                                        in between +/- 1      Hz          Hz            Sec          Hz
				InitSineToneGenerator(&SineToneGenerator1,     0.99f,          0.1f,      24000.1f,      20.0f,        48000);
				InitSineToneGenerator(&SineToneGenerator2,     0.99f,          1000.0f,    1000.0f,      20.0f,        48000);
				Kp=KpValueAtFsIn48KHz;
				Ki=KiValueAtFsIn48KHz;
				PI_ErrAccMaxValue=PI_ErrAccMaxValueAtFsIn48KHz;
			}else
			{
				//                                          Amplitude(),      FreqBeg,   FreqEnd,  Sweeping Duration,  Fs
				//                                        in between +/- 1      Hz          Hz            Sec          Hz
				InitSineToneGenerator(&SineToneGenerator1,     0.99f,          0.1f,      16000.1f,      20.0f,        32000);
				InitSineToneGenerator(&SineToneGenerator2,     0.99f,          1000.0f,    1000.0f,      20.0f,        32000);
				Kp=KpValueAtFsIn32KHz;
				Ki=KpValueAtFsIn32KHz;
				PI_ErrAccMaxValue=PI_ErrAccMaxValueAtFsIn32KHz;
			}

			AsrcFsInCurrent=AsrcFsInTarget;
			//DbgPin4Dn;
		}
	};

}


inline uint32_t read_ccount(void)
{
	unsigned int ccount;
	__asm__(
		"rsr %0, ccount"
		: "=a" (ccount) : : "memory"
	);
	return ccount;
}
