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

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

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

// --------------------------------------------------------------
// internal funcs
// --------------------------------------------------------------
void LFO_set_omega(lfo_t *pObj, synth_float_t omega)
{
	if (omega < 0.0 || omega > 1.0)
	{
		return;
	}
	pObj->b = 2.0 * sin(omega*pi);
	pObj->dx = omega;
}

void LFO_freq_update(lfo_t *pObj)
{
	synth_float_t omega;
	
	omega = pObj->param[LFO_PARAM2_FREQ]/ pObj->fs;
	
	LFO_set_omega(pObj, omega);

	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)
{
	if (y == 0)
	{
		return 0;
	}
	synth_float_t result = fmod(x, y);
	
	if (result < 0)
	{
		result += y;
	}
	
	return result;
}

void Sync_init(sync_t *pObj)
{
	pObj->phase = 0;
	pObj->phase_ref = 0;
	pObj->phase_adjusted = 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;
}

int Sync_process(sync_t *pObj)
{
	int do_lfo_update = 0;
	synth_float_t perr = 0;
	if (pObj->phase_update)
	{
		do_lfo_update = 1;
		pObj->phase_update = 0;
		perr = (synth_float_t)Sync_mod(pObj->phase - pObj->phase_ref, 1.0) - (synth_float_t)0.5;
//		SynthDebug("phase_ref=%f, phase=%f, perr=%f\n", pObj->phase_ref, pObj->phase_adjusted, perr);
	}
	pObj->phase_adjusted = Sync_mod(pObj->phase - 0.5, 1.0);
	pObj->omega = -(pObj->klag*pObj->accu + pObj->klead*perr);
	pObj->phase = Sync_mod(pObj->phase + pObj->omega, 1.0);
	pObj->accu += perr;
	
	return do_lfo_update;
}

// --------------------------------------------------------------
// Exported functions
// --------------------------------------------------------------
void LFO_Init(lfo_t *pObj, synth_float_t fs)
{
	pObj->fs = fs;
	pObj->pOut = NULL;
	pObj->bufsize = 0;

	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->out = 0;
	pObj->sqr = 0;
	pObj->tri = 0;
	pObj->gain = 1;
	pObj->offset = 0.5;

	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 (!size)
		return;

	pObj->pOut = (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->a = 0.5;
	pObj->y[0] = pObj->a*cos(2*pi*initial_phase);
	pObj->y[1] = pObj->a*sin(2*pi*initial_phase);
	pObj->x = initial_phase;
	pObj->out = 0;
	pObj->sqr = 0;
	pObj->tri = 0;
	pObj->delay_x = 0;
	pObj->attack_y = 0;
	LFO_freq_update(pObj);
	LFO_smmother_update(pObj);

}

void LFO_sync(lfo_t *pObj, synth_float_t phase_ref)
{
	Sync_phase_update(&pObj->sync, phase_ref);
}

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

		case LFO_PARAM2_FREQ:
			pObj->param[type] = value;
			pObj->freq_update_req = 1;
			break;

		case LFO_PARAM2_SMOOTH:
			pObj->param[type] = value;
			LFO_smmother_update(pObj);
			break;

		case LFO_PARAM2_DELAY:
			pObj->param[type] = value;
			pObj->freq_update_req = 1;
			break;

		case LFO_PARAM2_ATTACK:
			pObj->param[type] = value;
			pObj->freq_update_req = 1;
			break;

		case LFO_PARAM2_SYMMETRY:
			pObj->param[type] = value;
			pObj->gain = 1;
			pObj->offset = 0.5 *(1 + value) - 1;
			break;

		default:
			break;
	}
}

synth_float_t* LFO_ProcessDataV(lfo_t *pObj, UINT32 len)
{
	synth_float_t *pOut, out, in;
	UINT32 i, i0;
	
	pOut = pObj->pOut;

	// Process Delay
	for (i=0; i< len; i++)
	{	
		pObj->delay_x += pObj->delay_dx;

		if (pObj->delay_x >= 1.0)
			break;
		
		in = (synth_float_t)0;
		pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
		out = pObj->out;
		if (pObj->smooth_is_negative)
			out = 2*in - pObj->out;

		*(pOut++) = out;

		if (pObj->freq_update_req)
		{
			pObj->freq_update_req = 0;
			LFO_freq_update(pObj);
		}
	}
	i0 = i;

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SAW)
	{
		// Create phase
		for (i=i0; i< len; i++)
		{
			if (Sync_process(&pObj->sync))
			{
				LFO_set_omega(pObj, pObj->sync.omega);		
				SynthDebug("BPM=%f\n", pObj->sync.omega*pObj->fs*60);
			}
			pObj->x = pObj->sync.phase_adjusted;

			in = pObj->x + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			pObj->x += pObj->dx;
			if (pObj->x >= 1.f)
			{
				pObj->x -= 1.f;
			}

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_ONE_MINUS_SAW)
	{
		// Create phase
		for (i=i0; i< len; i++)
		{
			in = (1-pObj->x) + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			pObj->x += pObj->dx;
			if (pObj->x >= 1.f)
			{
				pObj->x -= 1.f;
			}

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SH_UNI)
	{
		// Create phase for S/H
		for (i=i0; i< len; i++)
		{
			if (pObj->x >= 1.f)
			{
				pObj->x -= 1.f;
				pObj->sh_sample = Noise_Uniform(&pObj->noise, 1, 0.5);
			}
			in = pObj->sh_sample + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			pObj->x += pObj->dx;

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}

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

				} while ((pObj->sh_sample < 0) || (pObj->sh_sample > 1.f));
			}
			in = pObj->sh_sample + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			pObj->x += pObj->dx;

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}

	// Create output
	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SINE)
	{
		for (i=i0; i< len; i++)
		{
			in = pObj->y[1]+pObj->a + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			pObj->y[0] = pObj->y[0] - pObj->b*pObj->y[1];
			pObj->y[1] = pObj->y[1] + pObj->b*pObj->y[0];

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_ONE_MINUS_SINE)
	{
		for (i=i0; i< len; i++)
		{
			in = (1-(pObj->y[1]+pObj->a)) + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			pObj->y[0] = pObj->y[0] - pObj->b*pObj->y[1];
			pObj->y[1] = pObj->y[1] + pObj->b*pObj->y[0];

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_COSINE)
	{
		for (i=i0; i< len; i++)
		{
			in = pObj->y[0]+pObj->a + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			pObj->y[0] = pObj->y[0] - pObj->b*pObj->y[1];
			pObj->y[1] = pObj->y[1] + pObj->b*pObj->y[0];

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_ONE_MINUS_COSINE)
	{
		for (i=i0; i< len; i++)
		{
			in = (1-(pObj->y[0]+pObj->a)) + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			pObj->y[0] = pObj->y[0] - pObj->b*pObj->y[1];
			pObj->y[1] = pObj->y[1] + pObj->b*pObj->y[0];

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_SQUARE)
	{
		// Create phase
		for (i=i0; i< len; i++)
		{
			in = (synth_float_t)pObj->sqr + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			pObj->x += 2*pObj->dx;
			if (pObj->x >= 1.f)
			{
				pObj->x -= 1.f;
				pObj->sqr = !pObj->sqr;
			}

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_ONE_MINUS_SQUARE)
	{
		// Create phase
		for (i=i0; i< len; i++)
		{
			in = (1-((synth_float_t)pObj->sqr)) + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			pObj->x += 2*pObj->dx;
			if (pObj->x >= 1.f)
			{
				pObj->x -= 1.f;
				pObj->sqr = !pObj->sqr;
			}

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}

	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_TRIANGLE)
	{
		// Create phase
		for (i=i0; i< len; i++)
		{
			in = (synth_float_t)pObj->tri + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			if (pObj->x < 0.5)
				pObj->tri = 2*pObj->x;
			else
				pObj->tri = 2*(1-pObj->x);

			pObj->x += pObj->dx;
			if (pObj->x >= 1.f)
			{
				pObj->x -= 1.f;
			}

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}
	
	if (pObj->param[LFO_PARAM2_WAVEFORM] == LFO_WAVEFORM_ONE_MINUS_TRIANGLE)
	{
		// Create phase
		for (i=i0; i< len; i++)
		{
			in = (1-((synth_float_t)pObj->tri)) + pObj->offset;
			pObj->out = pObj->smooth_a*pObj->out + pObj->smooth_b*in;
			out = pObj->out;
			if (pObj->smooth_is_negative)
				out = 2*in - pObj->out;

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

			if (pObj->x < 0.5)
				pObj->tri = 2*pObj->x;
			else
				pObj->tri = 2*(1-pObj->x);

			pObj->x += pObj->dx;
			if (pObj->x >= 1.f)
			{
				pObj->x -= 1.f;
			}

			if (pObj->freq_update_req)
			{
				pObj->freq_update_req = 0;
				LFO_freq_update(pObj);
			}
		}
	}
	
	return pObj->pOut;
}