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

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

#include "synth_defs.h"
#include "lfo.h"

// --------------------------------------------------------------
// internal funcs
// --------------------------------------------------------------
void LFO_freq_update(lfo_t *pObj)
{
	pObj->omega = pObj->param[LFO_PARAM2_FREQ]/ pObj->fs;
	
	if (pObj->param[LFO_PARAM2_DELAY] > 0)
	{
		pObj->delay_dx =  1.0/(pObj->fs*pObj->param[LFO_PARAM2_DELAY]);
	}
	else
	{
		pObj->delay_dx =  1.0;
	}

	pObj->attack_a =  1.0/(pObj->fs*pObj->param[LFO_PARAM2_ATTACK]*LFO_KE);

}

void LFO_smmother_update(lfo_t *pObj)
{
	pObj->smooth_is_negative = (pObj->param[LFO_PARAM2_SMOOTH] < 0);
	
	if (fabs(pObj->param[LFO_PARAM2_SMOOTH]) < 0.001)
	{
		pObj->smooth_b = 1;
		pObj->smooth_is_negative = 0;
	}
	else
	{
		pObj->smooth_b = fabs((synth_float_t)5/(pObj->param[LFO_PARAM2_SMOOTH]*pObj->fs));
	}

	pObj->smooth_a = 1 - pObj->smooth_b;

}

synth_float_t Sync_mod(synth_float_t x)
{
	synth_float_t y = x - (int)x;
	
	if (y < 0)
	{
		y += 1;
	}
			
	return y;
}

synth_float_t phase_det(synth_float_t lo, synth_float_t ref)
{
	lo = Sync_mod(lo - 0.5);
	synth_float_t perr = Sync_mod(lo - ref) - (synth_float_t)0.5;
	
	return perr;
}

void Sync_init(sync_t *pObj)
{
	pObj->phase = 0;
	pObj->phase_int = 0;
	pObj->phase_ref = 0;
	pObj->omega = 0.0;
	pObj->accu = 0;
	pObj->klead = 1.0;
	pObj->klag = 0.04/200;
	pObj->phase_update = 0;
}

void Sync_phase_update(sync_t *pObj, synth_float_t phase_ref)
{
	pObj->phase_ref = phase_ref;
	pObj->phase_update = 1;
}

sync_result_t Sync_process(lfo_t *pObj)
{
	sync_t *pSync = &pObj->sync;
	synth_float_t omega_base = pObj->omega;

	int sync_mode = (int)pObj->param[LFO_PARAM2_MIDISYNC_MODE];
	synth_float_t phase_ref = pSync->phase_ref;
	
	sync_result_t result;
	pSync->phase_int = Sync_mod(pSync->phase_int + pObj->omega);
	result.phase = pSync->phase;
	
	switch (sync_mode)
	{
		case LFO_SYNC_MODE_OFF:
			pSync->phase = Sync_mod(pSync->phase + omega_base);
			break;

		case LFO_SYNC_MODE_F:
			pSync->phase = Sync_mod(pSync->phase + pSync->omega);
			break;
			
		case LFO_SYNC_MODE_F_P:
			pSync->phase = pSync->phase_int;
			break;

		case LFO_SYNC_MODE_P:
			pSync->phase = Sync_mod(pSync->phase + omega_base);
			if (pSync->phase_update && phase_ref == 0)
			{
				pSync->phase = 0;
			}
			break;
	}
	result.is_cycle_start = (pSync->phase < result.phase);
	result.phase = pSync->phase;
	
	synth_float_t perr = 0;
	if (pSync->phase_update)
	{
		pSync->phase_update = 0;
		perr = phase_det(pSync->phase_int, phase_ref);
		SynthDebug("phase_ref=%f, phase_lo=%f, perr=%f\n", phase_ref, pSync->phase_int, perr);
		SynthDebug("syncOnFreqUpdate(): BPM=%f\n", pSync->omega*pObj->fs*60);
		
	}
	pSync->omega = omega_base-(pSync->klag*pSync->accu + pSync->klead*perr);
	pSync->accu += perr;
	
	return result;
}

void Sync_processV(lfo_t* pObj, int len)
{
	sync_t *pSync = &pObj->sync;
	synth_float_t perr = 0;
	synth_float_t omega_base = pObj->omega;
	
	int sync_mode = (int)pObj->param[LFO_PARAM2_MIDISYNC_MODE];
	if (pSync->phase_update)
	{
		pSync->phase_update = 0;
		perr = phase_det(pSync->phase_int, pSync->phase_ref);
		
		if (sync_mode == LFO_SYNC_MODE_P)
		{
			if (pSync->phase_ref == 0)
			{
				pSync->phase = 0;
			}
		}
		SynthDebug("phase_ref=%f, phase_lo=%f, perr=%f\n", pSync->phase_ref, pSync->phase_int, perr);
		SynthDebug("syncOnFreqUpdate(): BPM=%f\n", pSync->omega*pObj->fs*60);
	}
	
	for (int i=0; i < len; i++)
	{
		pObj->pPhase[i].phase_int = pSync->phase_int;
		pObj->pPhase[i].is_cycle_start = 0;
		pSync->phase_int = Sync_mod(pSync->phase_int + pSync->omega);
		pSync->omega = omega_base-(pSync->klag*pSync->accu + pSync->klead*perr);
		pSync->accu += perr;
		perr = 0;
	}
	
	switch (sync_mode)
	{
		case LFO_SYNC_MODE_P:
		case LFO_SYNC_MODE_OFF:
		{
			for (int i=0; i < len; i++)
			{
				pObj->pPhase[i].phase = pSync->phase;
				synth_float_t phase = pSync->phase;
				pSync->phase = Sync_mod(pSync->phase + omega_base);
				pObj->pPhase[i].is_cycle_start = (phase > pSync->phase);
			}
		}
		break;

		case LFO_SYNC_MODE_F:
		{
			for (int i=0; i < len; i++)
			{
				pObj->pPhase[i].phase = pSync->phase;
				synth_float_t phase = pSync->phase;
				pSync->phase = Sync_mod(pSync->phase + pSync->omega);
				pObj->pPhase[i].is_cycle_start = (phase > pSync->phase);
			}
		}
		break;
			
		case LFO_SYNC_MODE_F_P:
		{
			for (int i=0; i < len; i++)
			{
				synth_float_t phase = pSync->phase;
				pSync->phase = pObj->pPhase[i].phase_int;
				pObj->pPhase[i].is_cycle_start = (phase > pSync->phase);
				pObj->pPhase[i].phase = pSync->phase;
			}
		}
		break;

		default:
			break;
	}
}


// --------------------------------------------------------------
// Exported functions
// --------------------------------------------------------------
void LFO_Init(lfo_t *pObj, synth_float_t fs)
{
	pObj->fs = fs;
	pObj->pOut = NULL;
	pObj->bufsize = 0;
	pObj->pPhase = NULL;
	pObj->pWave = NULL;
	
	pObj->param[LFO_PARAM2_WAVEFORM] = LFO_WAVEFORM_SINE;
	pObj->param[LFO_PARAM2_FREQ] = 1;
	pObj->param[LFO_PARAM2_SMOOTH] = 0.001;		// seconds
	pObj->param[LFO_PARAM2_DELAY] = 0;		// seconds
	pObj->param[LFO_PARAM2_ATTACK] = 0;		// seconds
	
	pObj->sh_sample = 0;
	pObj->smooth_out = 0;
	pObj->offset = 0.0;

	LFO_Reset(pObj, 0);

	pObj->freq_update_req = 0;
	LFO_SetBufsize(pObj, SYNTH_MAX_BUFSIZE);

	Noise_Init(&pObj->noise, 1+(UINT32)clock() * (UINT32)clock());
	
	Sync_init(&pObj->sync);
}

void LFO_Free(lfo_t *pObj)
{
	LFO_SetBufsize(pObj, 0);
}

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

	pObj->bufsize = size;

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

	pObj->pOut = NULL;

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

	pObj->pPhase = NULL;

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

	pObj->pWave = NULL;

	if (!size)
		return;

	pObj->pOut = (synth_float_t*)malloc(pObj->bufsize*sizeof(synth_float_t));
	pObj->pPhase = (sync_result_t*)malloc(pObj->bufsize*sizeof(sync_result_t));
	pObj->pWave = (synth_float_t*)malloc(pObj->bufsize*sizeof(synth_float_t));
	
}

void LFO_SetFS(lfo_t *pObj, synth_float_t fs)
{
	pObj->fs = fs;
	LFO_freq_update(pObj);
}

void LFO_Reset(lfo_t *pObj, synth_float_t initial_phase)
{
	pObj->sync.phase = initial_phase;
	pObj->smooth_out = 0;
	pObj->delay_x = 0;
	pObj->attack_y = 0;
	LFO_freq_update(pObj);
	LFO_smmother_update(pObj);

}

synth_float_t LFO_sync(lfo_t *pObj, synth_float_t phase_ref)
{
	synth_float_t note_1_x = pObj->param[LFO_PARAM2_MIDISYNC_BEATDIV];
	phase_ref = Sync_mod(note_1_x*phase_ref);
	Sync_phase_update(&pObj->sync, phase_ref);
	
	// Return BPM
	return 4*60*pObj->fs*pObj->sync.omega/note_1_x;
}

void LFO_Param2Set(lfo_t *pObj, UINT32 type, synth_float_t value)
{
	int param_changed = 0;
	if (pObj->param[type] != value)
	{
		pObj->param[type] = value;
		param_changed = 1;
	}
	switch(type)
	{
		case LFO_PARAM2_WAVEFORM:
			pObj->freq_update_req = param_changed;
			break;

		case LFO_PARAM2_FREQ:
			pObj->freq_update_req = param_changed;
			break;

		case LFO_PARAM2_SMOOTH:
			if (param_changed)
			{
				LFO_smmother_update(pObj);
			}
			break;

		case LFO_PARAM2_DELAY:
			pObj->freq_update_req = param_changed;
			break;

		case LFO_PARAM2_ATTACK:
			pObj->freq_update_req = param_changed;
			break;

		case LFO_PARAM2_SYMMETRY:
			if (param_changed)
			{
				pObj->offset = 0.5 *value;
			}
			break;

		default:
			break;
	}
}

synth_float_t* LFO_ProcessDataV(lfo_t *pObj, UINT32 len)
{
	UINT32 i;
	
	sync_result_t *pPhase = pObj->pPhase;
	
	if (pObj->freq_update_req)
	{
		pObj->freq_update_req = 0;
		LFO_freq_update(pObj);
	}

	// Process Phase and Frequency
#if 0
	for (i=0; i< len; i++)
	{	
		*(pPhase++) = Sync_process(&pObj->sync);
	}
#else
	Sync_processV(pObj, len);
#endif
	
	pPhase = pObj->pPhase;
	synth_float_t *pWave = pObj->pWave;
	
	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SAW)
	{
		// Create phase
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			synth_float_t y = 2*sync_result.phase - 1.0;
			
			*(pWave++) = 0.5*y + pObj->offset;
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SAW_REV)
	{
		// Create phase
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			synth_float_t y = 2*sync_result.phase - 1.0;
			
			*(pWave++) = -0.5*y + pObj->offset;
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SH_UNI)
	{
		// Create phase for S/H
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			if (sync_result.is_cycle_start)
			{
				pObj->sh_sample = Noise_Uniform(&pObj->noise, 1, 0.5);
			}
			*(pWave++) = pObj->sh_sample + pObj->offset;
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SH_GAUSS)
	{
		// Create phase for S/H
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			if (sync_result.is_cycle_start)
			{
				do
				{
					pObj->sh_sample = Noise_Gaussian(&pObj->noise, sqrt(1.f/36), 0.5);

				} while ((pObj->sh_sample < 0) || (pObj->sh_sample > 1.f));
			}
			
			*(pWave++) = pObj->sh_sample + pObj->offset;
		}
	}

	// Create output
	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SINE)
	{
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			synth_float_t y = sin(2*M_PI*sync_result.phase);

			*(pWave++) = 0.5*y + pObj->offset;
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SINE_REV)
	{
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			synth_float_t y = sin(2*M_PI*sync_result.phase);

			*(pWave++) = -0.5*y + pObj->offset;
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_COSINE)
	{
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			synth_float_t y = cos(2*M_PI*sync_result.phase);

			*(pWave++) = 0.5*y + pObj->offset;
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_COSINE_REV)
	{
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			synth_float_t y = cos(2*M_PI*sync_result.phase);

			*(pWave++) = -0.5*y + pObj->offset;
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SQUARE)
	{
		// Create phase
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			synth_float_t y = 2*(synth_float_t)(sync_result.phase < 0.5) - 1.0;

			*(pWave++) = 0.5*y + pObj->offset;
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SQUARE_REV)
	{
		// Create phase
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			synth_float_t y = 2*(synth_float_t)(sync_result.phase < 0.5) - 1.0;

			*(pWave++) = -0.5*y + pObj->offset;
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_TRIANGLE)
	{
		// Create phase
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			synth_float_t y = 2*(1-sync_result.phase) - 1.0;
			if (sync_result.phase < 0.5)
			{
				y = 2*sync_result.phase - 1.0;
			}
			
			*(pWave++) = 0.5*y + pObj->offset;
		}
	}
	
	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_TRIANGLE_REV)
	{
		// Create phase
		for (i=0; i< len; i++)
		{
			sync_result_t sync_result = *(pPhase++);
			synth_float_t y = 2*(1-sync_result.phase) - 0.5;
			if (sync_result.phase < 0.5)
			{
				y = 2*sync_result.phase - 0.5;
			}
			
			*(pWave++) = -0.5*y + pObj->offset;
			
		}
	}
	
	// Output smoothing and delay
	pWave = pObj->pWave;
	synth_float_t *pOut = pObj->pOut;
	for (i=0; i< len; i++)
	{
		synth_float_t in = *(pWave++);
		pObj->delay_x += pObj->delay_dx;
		if (pObj->delay_x < 1.0)
		{
			in = 0;
		}
		pObj->smooth_out = pObj->smooth_a*pObj->smooth_out + pObj->smooth_b*in;
		synth_float_t out = pObj->smooth_out;
		if (pObj->smooth_is_negative)
			out = 2*in - pObj->smooth_out;

		*(pOut++) = out*pObj->attack_y;
		pObj->attack_y += pObj->attack_a*(1-pObj->attack_y);
		
	}
	
	return pObj->pOut;
}