
#include "sound_effect.h"
#include <math.h>

static void IIR_resonator(float fc, float Q, float a[], float b[])
{
  fc = tanf((float)M_PI * fc) / (2.0f * (float)M_PI);

  a[0] = 1.0f + 2.0f * (float)M_PI * fc / Q + 4.0f * (float)M_PI * (float)M_PI * fc * fc;
  a[1] = (8.0f * (float)M_PI * (float)M_PI * fc * fc - 2.0f) / a[0];
  a[2] = (1.0f - 2.0f * (float)M_PI * fc / Q + 4.0f * (float)M_PI * (float)M_PI * fc * fc) / a[0];
  b[0] = 2.0f * (float)M_PI * fc / Q / a[0];
  b[1] = 0.0f;
  b[2] = -2.0f * (float)M_PI * fc / Q / a[0];

  a[0] = 1.0f;
}


void radio_voice(int offset, int data_handling_size)
{

	int n;
	short volume1 = (short)g_ad_val[0], volume2 = (short)g_ad_val[1], volume3 = (short)g_ad_val[2];
	int m;
	float fc, Q, a[3], b[3], s0[128 + 2], s1[128 + 2];	//data_handling_size = 128 の場合

	for (n = 0; n < data_handling_size + 2; n++)
	{
		m = offset - 2 + n;
		if (m < 0)
		{
			m += DATA_BUFFER_SIZE;
		}
		s0[n] = g_pcm0[m].ch0;
	}

	for (n = 0; n < 2; n++)
	{
		m = offset - 2 + n;
		if (m < 0)
		{
			m += DATA_BUFFER_SIZE;
		}
		s1[n] = g_pcm1[m].ch0;
	}

	for (n = 2; n < data_handling_size + 2; n++)
	{
		s1[n] = 0.0f;
	}
	
	fc = 1000.0f;
	Q = 10.0f;

	IIR_resonator(fc / FS, Q, a, b);

	for (n = 0; n < data_handling_size; n++)
	{
		 for (m = 0; m <= 2; m++)
		{
			s1[2 + n] += b[m] * s0[2 + n - m];
		}
		for (m = 1; m <= 2; m++)
		{
			s1[2 + n] += -a[m] * s1[2 + n - m];
		}
	}

	for (n = 0; n < data_handling_size; n++)
	{
		g_pcm1[offset + n].ch0 = s1[2 + n];
	}
}
