/*************************************************************************/
/* vcf.c */
/*************************************************************************/
#include <string.h>
#include <stdlib.h>
#include <math.h>

#include "synth_defs.h"
#include "vcf.h"
#include "fast_trig/fast_trig.h"

/*************************************************************************/
/* Global Variables */
/*************************************************************************/

/******************************************************************************/
void VCF_Init(vcf_t *pObj, UINT32 id, vcf_common_t *pCom, synth_float_t fs)
{
	pObj->pCom = pCom;
	pObj->id = id;
	pObj->fs = fs;

	if (id == 0)
	{
		VCF_ModInit(pObj);
	}

	pObj->pOut = NULL;
	pObj->pCoef = NULL;
	pObj->bufsize = 0;
	VCF_SetType(pObj, VCF_FILTERTYPE_LOWPASS);
	VCF_SetOrder(pObj, 4);
	pObj->req_filter_update = 0;


	VCF_clear_state(pObj);
	VCF_SetBufsize(pObj, SYNTH_MAX_BUFSIZE);
	VCF_FilterParamUpdate(pObj);
	VCF_SetF(pObj, 20000);
	VCF_SetQ(pObj, 1);
}

void VCF_Free(vcf_t *pObj)
{
	VCF_SetBufsize(pObj, 0);

	if (pObj->id == 0)
	{
		VCF_ModFree(pObj);
	}

}

void VCF_ModInit(vcf_t *pObj)
{
	vcf_common_t *pCom = pObj->pCom;

	pCom->pLUT_cos = (synth_float_t*)malloc(FILTER_TABLE_SIZE*sizeof(synth_float_t));
	pCom->pLUT_sin = (synth_float_t*)malloc(FILTER_TABLE_SIZE*sizeof(synth_float_t));
}

void VCF_ModFree(vcf_t *pObj)
{
	vcf_common_t *pCom = pObj->pCom;
	
	free(pCom->pLUT_cos);
	free(pCom->pLUT_sin);
}

void VCF_LUT_gen(vcf_t *pObj)
{
	int i;
	synth_float_t cv, omega;
	vcf_common_t *pCom = pObj->pCom;
	
	for (i=0; i < FILTER_TABLE_SIZE; i++)
	{
		cv = (synth_float_t)i/(FILTER_TABLE_SIZE-1);
		omega = VCF_cv2omega(pObj, cv);
		pCom->pLUT_cos[i] = cos(pi*omega);
		pCom->pLUT_sin[i] = sin(pi*omega);
	}
}

void VCF_FilterParamUpdate(vcf_t *pObj)
{
	pObj->omega_max = 2*FILTER_F_MAX/pObj->fs;
	pObj->omega_min = 2*FILTER_F_MIN/pObj->fs;
	pObj->num_octaves = log(FILTER_F_MAX/FILTER_F_MIN)/log(FILTER_SWEEP_BASE);

	if (pObj->id == 0)
	{
		VCF_LUT_gen(pObj);
	}
}

synth_float_t VCF_LUT_get_cos(vcf_t *pObj, synth_float_t cv)
{
	int ni, ni_next;
	synth_float_t n, nf;
	vcf_common_t *pCom = pObj->pCom;

	n = min(1, max(0, cv))*(FILTER_TABLE_SIZE-1);
	ni = (int)n;
	nf = n - (synth_float_t)ni;
	ni_next = min(FILTER_TABLE_SIZE-1, ni+1);

	return (pCom->pLUT_cos[ni_next] - pCom->pLUT_cos[ni])*nf + pCom->pLUT_cos[ni];
}

synth_float_t VCF_LUT_get_sin(vcf_t *pObj, synth_float_t cv)
{
	int ni, ni_next;
	synth_float_t n, nf;
	vcf_common_t *pCom = pObj->pCom;

	n = min(1, max(0, cv))*(FILTER_TABLE_SIZE-1);
	ni = (int)n;
	nf = n - (synth_float_t)ni;
	ni_next = min(FILTER_TABLE_SIZE-1, ni+1);

	return (pCom->pLUT_sin[ni_next] - pCom->pLUT_sin[ni])*nf + pCom->pLUT_sin[ni];
}

void VCF_SetBufsize(vcf_t *pObj, UINT32 size)
{
	if (pObj->bufsize == size)
		return;

	pObj->bufsize = size;

	if (pObj->pOut)
		free(pObj->pOut);

	if (pObj->pCoef)
		free(pObj->pCoef);

	pObj->pOut = NULL;
	pObj->pCoef = NULL;

	if (!size)
		return;

	pObj->pOut = (synth_float_t*)malloc(pObj->bufsize*sizeof(synth_float_t));
	pObj->pCoef = (vcf_coef_t*)malloc(pObj->bufsize*sizeof(vcf_coef_t));
	pObj->pCoeff_last = pObj->pCoef;
	VCF_Coeff_update(pObj);

}

void VCF_SetFS(vcf_t *pObj, synth_float_t fs)
{
	pObj->fs = fs;
	VCF_FilterParamUpdate(pObj);
	pObj->req_filter_update = 1;
}

void VCF_SetF(vcf_t *pObj, synth_float_t f)
{
	if (f > pObj->fs/2)
		f = FILTER_F_MAX;

	if (f < FILTER_F_MIN)
		f = FILTER_F_MIN;

	if (f < 0)
		f = FILTER_F_MIN;

	pObj->cv_f_bias = VCF_omega2cv(pObj, 2*f/pObj->fs);
	pObj->req_filter_update = 1;
}

void VCF_SetQ(vcf_t *pObj, synth_float_t q)
{
	pObj->cv_q_bias = max(FILTER_Q_MIN, q);
	pObj->req_filter_update = 1;
}

void VCF_SetType(vcf_t *pObj, UINT32 type)
{
	pObj->type = type;
	VCF_SetQ(pObj, pObj->cv_q_bias);

	VCF_Coeff_update(pObj);
}

synth_float_t IIRCalcQp(unsigned p, unsigned N)
{
	return 1.0f/(synth_float_t)(2*sin(pi*(2*p-1)/(2*N)));
}

void VCF_SetOrder(vcf_t *pObj, UINT32 order)
{
	UINT32 p;
	
	if (pObj->order == order)
		return;

	if (order % 2)
		return;

	if (order > FILTER_MAX_ORDER)
		return;

	pObj->order = order;
	pObj->num_sections = pObj->order/2;

	for(p=0; p < pObj->num_sections; p++)
	{
		pObj->pQP_inv[p] = (synth_float_t)1.0/IIRCalcQp(p+1, pObj->order);
	}
	VCF_clear_state(pObj);
	VCF_SetQ(pObj, pObj->cv_q_bias);
	VCF_Coeff_update(pObj);
}

void VCF_Coeff_update(vcf_t *pObj)
{
	pObj->req_filter_update = 1;
}

void VCF_clear_state(vcf_t *pObj)
{
	UINT32 p;

	for(p=0; p < pObj->num_sections; p++)
	{
		pObj->pState[p].xn1 = 0;
		pObj->pState[p].xn2 = 0;
		pObj->pState[p].yn1 = 0;
		pObj->pState[p].yn2 = 0;
	}
	VCF_Coeff_update(pObj);
}

synth_float_t VCF_omega2cv(vcf_t *pObj, synth_float_t omega)
{
	synth_float_t cv;

	cv = log(omega/pObj->omega_min)/(pObj->num_octaves * log(FILTER_SWEEP_BASE));
	return min(1, max(0, cv));
}

synth_float_t VCF_cv2omega(vcf_t *pObj, synth_float_t cv)
{
	synth_float_t omega;

	omega = pObj->omega_min*pow(FILTER_SWEEP_BASE, pObj->num_octaves*min(1, max(0, cv))); 
	return omega;
}

void VCF_CalcCoeff_LPF(vcf_t *pObj, vcf_coef_t *pCoeff, synth_float_t *pCV_f, synth_float_t *pCV_q, UINT32 len)
{
	synth_float_t a0_inv;
	synth_float_t alpha, ks, kc, Q_inv, q, cv;
	unsigned p, n, filter_changed;

	ks = 0;
	kc = 0;
	Q_inv = 0;
	for(n=0; n < len; n++)
	{
		filter_changed = pObj->req_filter_update;

		if (fabs(pObj->cv_f_last - pCV_f[n]) > FILTER_RECALC_F_THRESH)
			filter_changed = 1;

		if (fabs(pObj->cv_q_last - pCV_q[n]) > FILTER_RECALC_Q_THRESH)
			filter_changed = 1;

		if (filter_changed)
		{
			pObj->req_filter_update = 0;
			pObj->cv_f_last = pCV_f[n];
			cv = pObj->cv_f_bias + pCV_f[n];
			ks = VCF_LUT_get_sin(pObj, cv);
			kc = VCF_LUT_get_cos(pObj, cv);
			q = min(FILTER_Q_MAX, max(FILTER_Q_MIN, pObj->cv_q_bias + FILTER_Q_MAX*pCV_q[n]));
			pObj->cv_q_last = q;
			if (pObj->order == 2)
				q *= q;

			Q_inv = (synth_float_t)1.0/q;
			pObj->pCoeff_last = pCoeff;
		}

		for(p=0; p < pObj->num_sections; p++)
		{
			if (filter_changed)
			{
				alpha = 0.5f*ks * Q_inv * pObj->pQP_inv[p];
				a0_inv = (synth_float_t)1.0/(1 + alpha);

				(*pCoeff).ak1 = -2.0f*kc *a0_inv;
				(*pCoeff).ak2 = (1 - alpha) *a0_inv;

				(*pCoeff).bk1 = (1 - kc) *a0_inv;
				(*pCoeff).bk0 = 0.5f*(1 - kc) *a0_inv;
				(*pCoeff).bk2 = (*pCoeff).bk0;
			}
			else
			{
				*pCoeff = pObj->pCoeff_last[p];
			}
			pCoeff++;
		}
	}
}


void VCF_CalcCoeff_HPF(vcf_t *pObj, vcf_coef_t *pCoeff, synth_float_t *pCV_f, synth_float_t *pCV_q, UINT32 len)
{
	synth_float_t a0_inv;
	synth_float_t alpha, ks, kc, Q_inv, q, cv;
	unsigned p, n, filter_changed;

	ks = 0;
	kc = 0;
	Q_inv = 0;
	for(n=0; n < len; n++)
	{
		filter_changed = pObj->req_filter_update;

		if (fabs(pObj->cv_f_last - pCV_f[n]) > FILTER_RECALC_F_THRESH)
			filter_changed = 1;

		if (fabs(pObj->cv_q_last - pCV_q[n]) > FILTER_RECALC_Q_THRESH)
			filter_changed = 1;

		if (filter_changed)
		{
			pObj->req_filter_update = 0;
			pObj->cv_f_last = pCV_f[n];
			cv = pObj->cv_f_bias + pCV_f[n];
			ks = VCF_LUT_get_sin(pObj, cv);
			kc = VCF_LUT_get_cos(pObj, cv);
			q = min(FILTER_Q_MAX, max(FILTER_Q_MIN, pObj->cv_q_bias + FILTER_Q_MAX*pCV_q[n]));
			pObj->cv_q_last = q;
			if (pObj->order == 2)
				q *= q;

			Q_inv = (synth_float_t)1.0/q;
			pObj->pCoeff_last = pCoeff;
		}

		for(p=0; p < pObj->num_sections; p++)
		{
			if (filter_changed)
			{
				alpha = 0.5f*ks * Q_inv * pObj->pQP_inv[p];
				a0_inv = (synth_float_t)1.0/(1 + alpha);

				(*pCoeff).ak1 = -2.0f*kc *a0_inv;
				(*pCoeff).ak2 = (1 - alpha) *a0_inv;

				(*pCoeff).bk1 = - (1 + kc) *a0_inv;
				(*pCoeff).bk0 = 0.5f*(1 + kc) *a0_inv;
				(*pCoeff).bk2 = (*pCoeff).bk0;
			}
			else
			{
				*pCoeff = pObj->pCoeff_last[p];
			}
			pCoeff++;
		}
	}
}


void VCF_CalcCoeff_BPF(vcf_t *pObj, vcf_coef_t *pCoeff, synth_float_t *pCV_f, synth_float_t *pCV_q, UINT32 len)
{
	synth_float_t a0_inv;
	synth_float_t alpha, ks, kc, Q_inv, q, cv;
	unsigned p, n, filter_changed;

	ks = 0;
	kc = 0;
	Q_inv = 0;
	for(n=0; n < len; n++)
	{
		filter_changed = pObj->req_filter_update;

		if (fabs(pObj->cv_f_last - pCV_f[n]) > FILTER_RECALC_F_THRESH)
			filter_changed = 1;

		if (fabs(pObj->cv_q_last - pCV_q[n]) > FILTER_RECALC_Q_THRESH)
			filter_changed = 1;

		if (filter_changed)
		{
			pObj->req_filter_update = 0;
			pObj->cv_f_last = pCV_f[n];
			cv = pObj->cv_f_bias + pCV_f[n];
			ks = VCF_LUT_get_sin(pObj, cv);
			kc = VCF_LUT_get_cos(pObj, cv);
			q = min(FILTER_Q_MAX, max(FILTER_Q_MIN, pObj->cv_q_bias + FILTER_Q_MAX*pCV_q[n]));
			pObj->cv_q_last = q;
			if (pObj->order == 2)
				q *= q;

			Q_inv = (synth_float_t)1.0/q;
			pObj->pCoeff_last = pCoeff;
		}

		for(p=0; p < pObj->num_sections; p++)
		{
			if (filter_changed)
			{
				alpha = 0.5f*ks * Q_inv * pObj->pQP_inv[p];
				a0_inv = (synth_float_t)1.0/(1 + alpha);

				(*pCoeff).ak1 = -2.0f*kc *a0_inv;
				(*pCoeff).ak2 = (1 - alpha) *a0_inv;

				(*pCoeff).bk1 = 0;
				(*pCoeff).bk0 = 0.5*ks *a0_inv;
				(*pCoeff).bk2 = -(*pCoeff).bk0;
			}
			else
			{
				*pCoeff = pObj->pCoeff_last[p];
			}
			pCoeff++;
		}
	}
}

void VCF_CalcCoeff(vcf_t *pObj, vcf_coef_t *pCoeff, synth_float_t *pFG, synth_float_t *pQ, UINT32 len)
{
	switch(pObj->type)
	{
		case VCF_FILTERTYPE_LOWPASS:
		VCF_CalcCoeff_LPF(pObj, pCoeff, pFG, pQ, len);
			break;

		case VCF_FILTERTYPE_HIGHPASS:
		VCF_CalcCoeff_HPF(pObj, pCoeff, pFG, pQ, len);
			break;

		case VCF_FILTERTYPE_BANDPASS:
		VCF_CalcCoeff_BPF(pObj, pCoeff, pFG, pQ, len);
			break;

		default:
		VCF_CalcCoeff_LPF(pObj, pCoeff, pFG, pQ, len);
			break;
	}

}

synth_float_t* VCF_ProcessDataV(vcf_t *pObj, synth_float_t *pCV_f, synth_float_t *pCV_q, synth_float_t *pX, UINT32 len)
{
	synth_float_t xp, yp, *pY;
	unsigned i, p;
	vcf_state_t *pState = pObj->pState;
	vcf_coef_t *pCoeff;

	VCF_CalcCoeff(pObj, pObj->pCoef, pCV_f, pCV_q, len);

	pCoeff = pObj->pCoef;

	yp = 0;
	pY = pObj->pOut;
	for (i=0; i<len; i++)
	{
		xp = pX[i];
		for (p=0; p < pObj->num_sections; p++)
		{
			yp = (*pCoeff).bk0*xp 
			   + (*pCoeff).bk1*pState[p].xn1
			   + (*pCoeff).bk2*pState[p].xn2
			   - (*pCoeff).ak1*pState[p].yn1
			   - (*pCoeff).ak2*pState[p].yn2;
				pCoeff++;
			
			pState[p].yn2 = pState[p].yn1;
			pState[p].yn1 = yp;
			pState[p].xn2 = pState[p].xn1;
			pState[p].xn1 = xp;
			xp	  = yp;
		}
		pY[i] = yp;	
	}
	return pObj->pOut;
}