// --------------------------------------------------------------
// --------------------------------------------------------------


#include <string.h>
#include <stdlib.h>
#include <math.h>

#include "synth_defs.h"
#include "blit.h"
#include "fir/fir2.h"
#include "vector_utils.h"

// --------------------------------------------------------------
// internal funcs
// --------------------------------------------------------------
INT32 smin(INT32 x, INT32 y)
{
	if (x < y)
		return x;

	return y;
}

INT32 smax(INT32 x, INT32 y)
{
	if (x > y)
		return x;

	return y;
}

UINT32 umin(UINT32 x, UINT32 y)
{
	if (x < y)
		return x;

	return y;
}

UINT32 umax(UINT32 x, UINT32 y)
{
	if (x > y)
		return x;

	return y;
}

void IP_ModInit(blit_common_t *pCom)
{
	INT32 n, k;
	synth_float_t *pN_K = pCom->N_K;
	synth_float_t *pK = pCom->K;

	for (n=0; n <= BLIT_INTEROLATION_ORDER; n++)
	{
		for (k=0; k <= BLIT_INTEROLATION_ORDER; k++)
		{
			if (k == n)
				continue;

			*(pN_K++) =  (synth_float_t)1 / (n-k);
			*(pK++) =  (synth_float_t)k;
		}
	}
}

synth_float_t IP_Process(blit_t *pObj, synth_float_t *pLUT, synth_float_t m)
{
	INT32 mi, j;
	INT32 n, k;
	synth_float_t hn, y = 0, delay;
	synth_float_t *pN_K = pObj->pCom->N_K;
	synth_float_t *pK = pObj->pCom->K;

	mi = (INT32)m;
	delay = m - (synth_float_t)mi;

	j = (INT32)BLIT_INTEROLATION_ORDER;
	for (n=0; n <= BLIT_INTEROLATION_ORDER; n++)
	{
		hn = (synth_float_t)1;
		for (k=0; k < BLIT_INTEROLATION_ORDER; k++)
		{
			hn *= (*(pN_K++) * (delay - *(pK++)));
		}
		y += hn*pLUT[smax(0, mi-j)];
		j--;
	}
	return y;
}
	
void BLIT_freq_update(blit_t *pObj, synth_float_t scale)
{
	synth_float_t p;

	pObj->freq_update_req = 0;

//	p = pObj->fs/(pObj->f*scale*pObj->fm);
	p = pObj->fs/(scale*pObj->fm);

	pObj->dx  = (synth_float_t)1.0/p;
	pObj->m = umin(BLIT_NUM_HARM_MAX-1, (UINT32)((synth_float_t)SYNTH_BANDWIDTH*2.0*(int)((p/2)) + 0));
}

// --------------------------------------------------------------
// Exported functions
// --------------------------------------------------------------
void BLIT_ModInit(blit_common_t *pCom)
{
	int m, n;
	synth_float_t **ppKaiser;
	synth_float_t x, dx, a, b, blit, blep;

	// Calculate table sizes
	pCom->pTableSizes = (UINT32*)malloc(BLIT_NUM_HARM_MAX*sizeof(UINT32));
	for (m=0; m < BLIT_NUM_HARM_MAX; m++)
	{
		pCom->pTableSizes[m] = m*BLIT_TABLE_OVERSAMPLING;
		if (pCom->pTableSizes[m] < BLIT_TABLE_SIZE_MIN)
			pCom->pTableSizes[m] = BLIT_TABLE_SIZE_MIN;
	}

	// Generate Kaiser window
	ppKaiser = (synth_float_t**)malloc(BLIT_NUM_HARM_MAX*sizeof(synth_float_t*));
	for (m=0; m < BLIT_NUM_HARM_MAX; m++)
	{
		ppKaiser[m] = (synth_float_t*)malloc(pCom->pTableSizes[m]*sizeof(synth_float_t));
		CalcKaiser(NULL, ppKaiser[m], 8.f, pCom->pTableSizes[m]);
	}

	// Allocate BLIT table
	pCom->ppBLEP = (synth_float_t**)malloc(BLIT_NUM_HARM_MAX*sizeof(synth_float_t*));
	for (m=0; m < BLIT_NUM_HARM_MAX; m++)
	{
		pCom->ppBLEP[m] = (synth_float_t*)malloc(pCom->pTableSizes[m]*sizeof(synth_float_t));
	}

	// Generate BLIT
	for (m=0; m < BLIT_NUM_HARM_MAX; m++)
	{
		dx = (synth_float_t)1/(pCom->pTableSizes[m]);
		x = -0.5;
		blep = 0;
		for (n=0; n < pCom->pTableSizes[m]; n++)
		{
			a = sin(x*pi);
			b = sin(m*x*pi);
			if (a == 0)
				blit = (synth_float_t)m;
			else
				blit = b/a * ppKaiser[m][n];

			blep += blit;
			pCom->ppBLEP[m][n] = blep/(pCom->pTableSizes[m]);
			x += dx;
		}
	}

/*
	m = BLIT_NUM_HARM_MAX/2;
	for (n=0; n < BLIT_TABLE_SIZE; n++)
	{
		SynthDebug("BLEP[%d][%d] = %.14f\n", m, n, pCom->ppBLEP[m][n]);
	}

	for (n=0; n < BLIT_TABLE_SIZE; n++)
	{
		SynthDebug("Kaiser[%d] = %.14f\n", n, pKaiser[n]);
	}
*/
	for (m=0; m < BLIT_NUM_HARM_MAX; m++)
	{
		free(ppKaiser[m]);
	}
	free(ppKaiser);

	// Precalculate Interpolation constants
	IP_ModInit(pCom);
}

void BLIT_ModFree(blit_common_t *pCom)
{
	int m;

	if (pCom->ppBLEP)
	{
		for (m=0; m < BLIT_NUM_HARM_MAX; m++)
		{
			free(pCom->ppBLEP[m]);
		}
		free(pCom->ppBLEP);
	}
	free(pCom->pTableSizes);
}

void BLIT_Init(blit_t *pObj, blit_common_t *pCom, synth_float_t fs)
{
	pObj->pCom = pCom;

	pObj->fs = fs;
	pObj->dutycycle = 0.5;
	BLIT_Reset(pObj, 0);
	pObj->bufsize = 0;
	BLIT_SetBufsize(pObj, SYNTH_MAX_BUFSIZE);
	BLIT_Prepare(pObj, SYNTH_MAX_BUFSIZE);
}

void BLIT_Free(blit_t *pObj)
{
	BLIT_SetBufsize(pObj, 0);
}

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

	pObj->bufsize = size;

	if (!size)
		return;

}

void BLIT_SetFS(blit_t *pObj, synth_float_t fs)
{
	pObj->fs = fs;
	pObj->freq_update_req = 1;
}

void BLIT_Prepare(blit_t *pObj, UINT32 len)
{
	synth_float_t pitch_buf[SYNTH_MAX_BUFSIZE];
	synth_float_t out_buf[SYNTH_MAX_BUFSIZE];

	len = min(len, SYNTH_MAX_BUFSIZE);

	// Let Oscillators run with different pitches to randomize phase
	Add_inplace_VS(pitch_buf, 0, 8*440.0*(1+0.01*(synth_float_t)rand()/RAND_MAX), len);
	BLIT_Process_SAW_Vector(pObj, pitch_buf, NULL, NULL, NULL, out_buf, len);
	BLIT_Process_SQR_Vector(pObj, pitch_buf, NULL, NULL, NULL, NULL, out_buf, len);
	BLIT_Process_TRI_Vector(pObj, pitch_buf, NULL, NULL, NULL, out_buf, len);
}

void BLIT_Reset(blit_t *pObj, synth_float_t phase)
{
	pObj->saw_x = fmod(0.5f + phase, 1);
	pObj->sqr_x1 = fmod(0.5f + phase, 1);
	pObj->sqr_x2 = fmod(0.5f + phase, 1);
	pObj->sqr_pol1 = 1;				// ToDo: Adjust according to phase
	pObj->sqr_pol2 = 1;				// ToDo: Adjust according to phase
	pObj->tri_x = fmod(0.5f + phase, 1);
	pObj->tri_sum = 0;				// ToDo: Adjust according to phase
	pObj->tri_pol = 1;				// ToDo: Adjust according to phase
	pObj->fm = 1;
	pObj->pwm = 0;
	pObj->pulse_offset = 0;		// ToDo: Adjust according to phase
}

void BLIT_Start(blit_t *pObj)
{
	pObj->freq_update_req = 1;
}

void BLIT_SetDutyCycle(blit_t *pObj, synth_float_t duty_cycle)
{
	pObj->dutycycle = duty_cycle;
}

void BLIT_Process_SAW_Vector(blit_t *pObj, synth_float_t *pPitch, synth_float_t *pCV_fm, UINT32 *pSyncIn, UINT32 *pSyncOut, synth_float_t *pOut, UINT32 len)
{
	UINT32 i, is_slave, m;
	synth_float_t out;
	synth_float_t blep, n;
	blit_common_t *pCom = pObj->pCom;

	is_slave = (pSyncIn != NULL) && (pSyncOut == NULL);

	if (pSyncOut)
		memset(pSyncOut, 0, len*sizeof(synth_float_t));

	// Create output
	for (i=0; i< len; i++)
	{
		if (pObj->freq_update_req)
		{
			BLIT_freq_update(pObj, pPitch[i]);
		}

		if (pObj->saw_x >= (synth_float_t)0.5)
		{
			pObj->saw_x -= 1;
			if (pCV_fm)
			{
				pObj->fm = (synth_float_t)pow((synth_float_t)2, pCV_fm[i]);
			}
			BLIT_freq_update(pObj, pPitch[i]);
		}

		m = pObj->m;

		// Lagrange interpolation
		n = (pObj->saw_x+0.5)*((pCom->pTableSizes[m])-1);
		blep = IP_Process(pObj, &pCom->ppBLEP[m][0], n);

		out = pObj->saw_x - blep;
		pObj->saw_x += pObj->dx;

		*(pOut++) = 2*(out+0.5);
	}
}

void BLIT_Process_SQR_Vector(blit_t *pObj, synth_float_t *pPitch, synth_float_t *pCV_fm, synth_float_t *pCV_pwm, UINT32 *pSyncIn, UINT32 *pSyncOut, synth_float_t *pOut, UINT32 len)
{
	UINT32 i, is_slave, m;
	synth_float_t blep1, blep2, n;
	synth_float_t dutycycle;
	blit_common_t *pCom = pObj->pCom;

	is_slave = (pSyncIn != NULL) && (pSyncOut == NULL);

	if (pSyncOut)
		memset(pSyncOut, 0, len*sizeof(synth_float_t));

	// Create output
	for (i=0; i< len; i++)
	{
		if (pObj->freq_update_req)
		{
			BLIT_freq_update(pObj, pPitch[i]);
		}

		if (pObj->sqr_x1 >= (synth_float_t)0.5)
		{
			pObj->sqr_x1 -= 1;
			pObj->sqr_pol1 = !pObj->sqr_pol1;
			if (pCV_fm)
			{
				pObj->fm = (synth_float_t)pow((synth_float_t)2, pCV_fm[i]);
			}
			if (pCV_pwm)
			{
				pObj->pwm = (synth_float_t)pCV_pwm[i];
			}
			dutycycle = min(1, max(0, pObj->dutycycle + pObj->pwm));
			pObj->pulse_offset = 2*(dutycycle-0.5);
			pObj->sqr_x2 = pObj->sqr_x1 + dutycycle - 1;
			pObj->sqr_pol2 = pObj->sqr_pol1;
			BLIT_freq_update(pObj, pPitch[i]);
		}
		while (pObj->sqr_x2 >= (synth_float_t)0.5)
		{
			pObj->sqr_pol2 = !pObj->sqr_pol2;
			pObj->sqr_x2 -= 1;
		}
		while (pObj->sqr_x2 < (synth_float_t)-0.5)
		{
			pObj->sqr_pol2 = !pObj->sqr_pol2;
			pObj->sqr_x2 += 1;
		}
		m = pObj->m;

		// Lagrange interpolation
		n = (pObj->sqr_x1+0.5)*((pCom->pTableSizes[m])-1);
		blep1 = IP_Process(pObj, &pCom->ppBLEP[m][0], n);
		
		if (pObj->sqr_pol1)
			blep1 = 1 - blep1;

		n = (pObj->sqr_x2+0.5)*((pCom->pTableSizes[m])-1);
		blep2 = IP_Process(pObj, &pCom->ppBLEP[m][0], n);
			
		if (pObj->sqr_pol2)
			blep2 = 1 - blep2;

		*(pOut++) = 4*(blep1-0.5)*(blep2-0.5) - pObj->pulse_offset;

		pObj->sqr_x1 += pObj->dx;
		pObj->sqr_x2 += pObj->dx;
	}
}

void BLIT_Process_TRI_Vector(blit_t *pObj, synth_float_t *pPitch, synth_float_t *pCV_fm, UINT32 *pSyncIn, UINT32 *pSyncOut, synth_float_t *pOut, UINT32 len)
{
	UINT32 i, is_slave, m;
	synth_float_t out;
	synth_float_t blep, n;
	blit_common_t *pCom = pObj->pCom;

	is_slave = (pSyncIn != NULL) && (pSyncOut == NULL);

	if (pSyncOut)
		memset(pSyncOut, 0, len*sizeof(synth_float_t));

	for (i=0; i< len; i++)
	{
		if (pObj->freq_update_req)
		{
			BLIT_freq_update(pObj, pPitch[i]);
		}

		if (pObj->tri_x >= (synth_float_t)0.5)
		{
			pObj->tri_x -= 1;
			pObj->tri_pol = !pObj->tri_pol;
			if (pCV_fm)
			{
				pObj->fm = (synth_float_t)pow((synth_float_t)2, pCV_fm[i]);
			}
			BLIT_freq_update(pObj, 2*pPitch[i]);
		}
		m = pObj->m;

		// Lagrange interpolation
		n = (pObj->tri_x+0.5)*((pCom->pTableSizes[m])-1);
//		blep = IP_Process(&_sg_ppBLEP[m][0], n);

		// No interpolation
		blep = pCom->ppBLEP[m][(UINT32)n];

		if (pObj->tri_pol)
			out = 1 - blep;
		else
			out = blep;

	    pObj->tri_sum += 4*(out-0.5)*pObj->dx;
		*(pOut++) = pObj->tri_sum;

		pObj->tri_x += pObj->dx;

	}
}