/*
 * eq_soft.c
 *
 *  Created on: 2024年3月8日
 *      Author: haobox
 */
#include "platform.h"
#include "hal_headfile.h"
#include "eq_soft.h"

//int status_int[EQ_NUM][EQ_FILTER_COUNT] = {0};
//int coef_array_441_int[EQ_FILTER_COUNT][5] = {0};
//eq_soft_param_shift_by_fre_int(coef_array_441_int[level], eq_config_data->EQ_filter_gain[eq_index][level], eq_config_data->EQ_fc[eq_index][level], eq_config_data->EQ_Q[eq_index][level], eq_config_data->EQ_filter_type[eq_index][level], 44100);


void eq_soft_update_data_int(int* data, uint32_t len_in_frame, int* coef_array, int* status)
{
	spv40_iir_bqd_df1_stereo_q31_ofp(len_in_frame, data, coef_array, status);
}

void eq_soft_update_data_q15_int(int16_t* data, uint32_t len_in_frame, int* coef_array, int* status)
{
	spv40_iir_bqd_df1_stereo_q15_ofp(len_in_frame, data, coef_array, status);
}


__ILM double eq_soft_gain_cal(uint16_t data_raw)
{
	return (double)((short)data_raw/256.0);
}

__ILM double eq_soft_q_cal(uint16_t data_raw)
{
	return ((double)(data_raw >> 8) + (1.0/256.0)*(data_raw & 0xff));
}

void eq_soft_param_shift_by_fre_int(int* coef_array, uint16_t gain_raw, uint16_t fc, uint16_t q, uint8_t type, uint32_t samplerate)
{
	double gain = eq_soft_gain_cal(gain_raw);
	double Q = eq_soft_q_cal(q);

	double b0 = 1, b1 = 0, b2 = 0, a0 = 1, a1 = 0, a2 = 0;
	double A = pow(10, gain / 40);
	double W = 2 * PI * fc / samplerate;
	double sinw = sin(W);
	double cosw = cos(W);
	double alpha = sinw / (2 * Q);
	double b = sqrt(2 * A) / Q;

	switch(type)
	{
	case TYPE_PEAK:
		b0 = 1 + alpha * A;
		b1 = -2 * cosw;
		b2 = 1 - alpha * A;
		a0 = 1 + alpha / A;
		a1 = b1;
		a2 = 1 - alpha / A;
		break;
	case TYPE_LOWSHELF:
		b0 = A * ((A + 1.0) - (A - 1.0) * cosw + b * sinw);
		b1 = 2 * A * ((A - 1.0) - (A + 1.0) * cosw);
		b2 = A * ((A + 1.0) - (A - 1.0) * cosw - b * sinw);
		a0 = (A + 1.0) + (A - 1.0) * cosw + b * sinw;
		a1 = (-2) * ((A - 1.0) + (A + 1.0) * cosw);
		a2 = (A + 1.0) + (A - 1.0) * cosw - b * sinw;
		break;
	case TYPE_HIGHSHELF:
		b0 = A * ((A + 1.0) + (A - 1.0) * cosw + b * sinw);
		b1 = (-2) * A * ((A - 1.0) + (A + 1) * cosw);
		b2 = A * ((A + 1.0) + (A - 1.0) * cosw - b * sinw);
		a0 = (A + 1.0) - (A - 1.0) * cosw + b * sinw;
		a1 = 2 * ((A - 1.0) - (A + 1.0) * cosw);
		a2 = (A + 1.0) - (A - 1.0) * cosw - b * sinw;
		break;
	case TYPE_LOWPASS:
		b0 = ((1 - cosw) / 2);
		b1 = (1 - cosw);
		b2 = ((1 - cosw) / 2);
		a0 = 1 + alpha;
		a1 = -2 * cosw;
		a2 = 1 - alpha;
		break;
	case TYPE_HIGHPASS:
		b0 = ((1 + cosw) / 2);
		b1 = (-(1 + cosw));
		b2 = ((1 + cosw) / 2);
		a0 = 1 + alpha;
		a1 = -2 * cosw;
		a2 = 1 - alpha;
		break;
	case TYPE_BANDPASS:
		b0 = alpha;
		b1 = 0;
		b2 = -alpha;
		a0 = 1 + alpha;
		a1 = -2*cosw;
		a2 = 1 - alpha;
		break;
	case TYPE_ALLPASS:
		b0 = 1 - alpha;
		b1 = -2*cosw;
		b2 = 1 + alpha;
		a0 = b2;
		a1 = b1;
		a2 = b0;
		break;
	default:
		b0 = 1;
		b1 = 0;
		b2 = 0;
		a0 = 1;
		a1 = 0;
		a2 = 0;
		break;
	}

	coef_array[0] = (int)(b0 / a0 * 268435456);		//1073741824
	coef_array[1] = (int)(b1 / a0 * 268435456);
	coef_array[2] = (int)(b2 / a0 * 268435456);
	coef_array[3] = (int)(-a1 / a0 * 268435456);
	coef_array[4] = (int)(-a2 / a0 * 268435456);
}
