#include "IIR.h"
#include "arm_math.h"
#include "arm_const_structs.h"

//Log需要引用的头文件
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"

#define bp_numStages  1                /* 2阶IIR滤波的个数 */
#define bs_numStages  2                /* 2阶IIR滤波的个数 */

#define BLOCK_SIZE           1    	 /* 调用一次arm_biquad_cascade_df1_f32处理的采样点个数 */

uint32_t blockSize = BLOCK_SIZE;
static float32_t IIRStateF32_bp[4*bp_numStages];                      /* 带通滤波器状态缓存 */
static float32_t IIRStateF32_bs[4*bs_numStages];                      /* 带阻滤波器状态缓存 */

arm_biquad_casd_df1_inst_f32 S_BS;
arm_biquad_casd_df1_inst_f32 S_BP;
float32_t bpScaleValue;
float32_t bsScaleValue;

/* Elliptic带通滤波器系数20Hz 500Hz*/                                                                                                                                         
const float32_t IIRCoeffs32BP[5*bp_numStages] = {
    1.0f,  0,  -1.0f,  0.638776097661321373699649939226219430566f,  0.339488565819422882796629892254713922739f
};  
/* 巴特沃斯带阻滤波器系数48Hz 52Hz*/                                                                                                                                           
const float32_t IIRCoeffs32BS[5*bs_numStages] = {
    1.0f,  -1.975854434025588135526163569011259824038f,  1.0f,  1.937393708057424390744927222840487957001f,  -0.966392678521233783328625577269122004509f,                    
    1.0f,  -1.975854434025588135526163569011259824038f,  1.0f,  1.952884644070242314839447317353915423155f,  -0.972373462682551759073135144717525690794f                    

}; 

void arm_iir_f32_bs(void)
{
	/* 初始化 */
	arm_biquad_cascade_df1_init_f32(&S_BS, bs_numStages, (float32_t *)&IIRCoeffs32BS[0], (float32_t *)&IIRStateF32_bs[0]);      
	/*放缩系数 */
	bsScaleValue = 0.984570131944842996674083224206697195768f*0.984570131944842996674083224206697195768f; 
}

void bs_bp(float32_t *bpinputF32_IN,float32_t *bsoutputF32_IN)
{
	float32_t bpoutputF32_IN __attribute__((aligned(4)));

	arm_biquad_cascade_df1_f32(&S_BP, bpinputF32_IN,  &bpoutputF32_IN,  blockSize);
	bpoutputF32_IN = (bpoutputF32_IN)*bpScaleValue;
	
	arm_biquad_cascade_df1_f32(&S_BS, &bpoutputF32_IN,  bsoutputF32_IN,  blockSize);
	*bsoutputF32_IN = (*bsoutputF32_IN)*bsScaleValue;	
}


/*
*********************************************************************************************************
*	函 数 名: arm_iir_f32_bp
*	功能说明: 调用函数arm_iir_f32_hp实现带通滤波器
*	形    参：无
*	返 回 值: 无
*********************************************************************************************************
*/
void arm_iir_f32_bp(void)
{
	/* 初始化 */
	arm_biquad_cascade_df1_init_f32(&S_BP, bp_numStages, (float32_t *)&IIRCoeffs32BP[0], (float32_t *)&IIRStateF32_bp[0]);
	        
	/*放缩系数 */
	bpScaleValue = 0.66974428290971144139831494612735696137f;
}

#define TEST_LENGTH_SAMPLES  400    /* 采样点数 */
//static float32_t testInput_f32_50Hz_200Hz[TEST_LENGTH_SAMPLES]; /* 采样点 */

//static float32_t testOutput_f32_50Hz_200Hz[TEST_LENGTH_SAMPLES];

float math_caculate_signal(uint16_t sampleHz) 
{
    static uint16_t i = 0;
    float singalOutput;
    
    /* 50Hz正弦波 + 200Hz正弦波 */
    singalOutput = arm_sin_f32(2 * 3.1415926f * 50 * i / sampleHz) + 
                   arm_sin_f32(2 * 3.1415926f * 200 * i / sampleHz);
    
    /* 归零逻辑：当i达到一个完整周期的采样点数时 */
    if (++i >= (sampleHz / 50)) {
        i = 0;
    }
    
    return singalOutput;
}

void FilterInit()
{
	arm_iir_f32_bs();   // 带阻滤波器
	arm_iir_f32_bp();   // 带通滤波器
	
	////test code
	
//	CreateData();
//	
//	for (int i = 0; i < TEST_LENGTH_SAMPLES; i++)
//	{
//		arm_biquad_cascade_df1_f32(&S_BS, &testInput_f32_50Hz_200Hz[i],  &testOutput_f32_50Hz_200Hz[i],  1);
//	}
	/*
	NRF_LOG_INFO("raw\r\n");
	char stf[10];
	for (int i = 0; i < TEST_LENGTH_SAMPLES; i++)
	{
		sprintf(stf,"%0.3f", testInput_f32_50Hz_200Hz[i]);
		
		NRF_LOG_INFO("%s",stf);
		if (NRF_LOG_PROCESS() == false) 	//处理挂起的log
		{
		
		}
	}
	

	NRF_LOG_INFO("proccess\r\n");
	NRF_LOG_PROCESS();
	arm_biquad_cascade_df1_f32(&S_BS, testInput_f32_50Hz_200Hz,  testOutput_f32_50Hz_200Hz,  TEST_LENGTH_SAMPLES);
	for (int i = 0; i < TEST_LENGTH_SAMPLES; i++)
	{
		sprintf(stf,"%0.3f", testOutput_f32_50Hz_200Hz[i]);
		
		NRF_LOG_INFO("%s",stf);
		if (NRF_LOG_PROCESS() == false) 	//处理挂起的log
		{
		
		}
	}
	while(1)
	{
		NRF_LOG_PROCESS();
	}
	*/
	
	//处理挂起的log

	//////end
}