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

#include "synth_defs.h"
#include "smooth.h"

// --------------------------------------------------------------
// Exported functions
// --------------------------------------------------------------
void Smooth_Update(smooth_t *pObj)
{
	pObj->a_r = (synth_float_t)1/(pObj->fs*pObj->T_r*SMOOTH_KE);
	pObj->a_f = (synth_float_t)1/(pObj->fs*pObj->T_f);
}

void Smooth_Init(smooth_t *pObj, synth_float_t fs, synth_float_t T_r, synth_float_t T_f, synth_float_t inital_value)
{
	pObj->fs = fs;
	pObj->T_r = T_r;
	pObj->T_f = T_f;
	pObj->y = inital_value;
	Smooth_Update(pObj);
}

void Smooth_Free(smooth_t *pObj)
{
}

void Smooth_SetValue(smooth_t *pObj, synth_float_t value)
{
	pObj->y = value;
}

void Smooth_SetFS(smooth_t *pObj, synth_float_t fs)
{
	pObj->fs = fs;
	Smooth_Update(pObj);
}

void Smooth_SetT(smooth_t *pObj, synth_float_t T_r, synth_float_t T_f)
{
	pObj->T_r = T_r;
	pObj->T_f = T_f;
	Smooth_Update(pObj);
}

synth_float_t Smooth_ProcessScalar(smooth_t *pObj, synth_float_t x_hard, synth_float_t in)
{
	if (pObj->y < x_hard)
		pObj->y += pObj->a_r*(x_hard - pObj->y);
	else
		pObj->y += pObj->a_f*(x_hard - pObj->y);

	return pObj->y*in;
}

void Smooth_Process_VV(smooth_t *pObj, synth_float_t x_hard, synth_float_t *pIn, synth_float_t *pOut, UINT32 len)
{
	UINT32 i;

	if (Smooth_is_settled(pObj, x_hard) && (fabs(x_hard) < 1E-18))
	{
		for (i = 0; i < len; i++)
		{
			pOut[i] = 0;
		}
		return;
	}

	if (Smooth_is_settled(pObj, x_hard))
	{
		for (i = 0; i < len; i++)
		{
			pOut[i] = x_hard*pIn[i];
		}
		return;
	}

	for (i = 0; i < len; i++)
	{
	if (pObj->y < x_hard)
		pObj->y += pObj->a_r*(x_hard - pObj->y);
	else
		pObj->y += pObj->a_f*(x_hard - pObj->y);

		pOut[i] = pObj->y*pIn[i];
	}
}

void Smooth_Process_V(smooth_t *pObj, synth_float_t x_hard, synth_float_t *pOut, UINT32 len)
{
	UINT32 i;

	if (Smooth_is_settled(pObj, x_hard) && (fabs(x_hard) < 1E-18))
	{
		for (i = 0; i < len; i++)
		{
			pOut[i] = 0;
		}
		return;
	}

	if (Smooth_is_settled(pObj, x_hard))
	{
		for (i = 0; i < len; i++)
		{
			pOut[i] = x_hard;
		}
		return;
	}

	for (i = 0; i < len; i++)
	{
		if (pObj->y < x_hard)
			pObj->y += pObj->a_r*(x_hard - pObj->y);
		else
			pObj->y += pObj->a_f*(x_hard - pObj->y);

		pOut[i] = pObj->y;
	}
}

UINT32 Smooth_is_settled(smooth_t *pObj, synth_float_t x_hard)
{
	return (fabs(pObj->y - x_hard) < SMOOTH_TOL);
}