#include <stdint.h>
#include <math.h>

/* modulator */
typedef struct _lfo_t
{
    //float   freq;      /* Osc. Frequency (in Hertz) */
    float   a1;          /* Coefficient: a1 = cos(w) for sine mod, inc for triangle mod. */
    float   buffer1;     /* sin(n-1), val for triangle mod. */
    float   buffer2;     /* sin(n-2) */
} lfo_t;

/* Phase is in range [0,2], basically take the phase you'd feed lsx_generate_wave_table * 
 * take out the factor of M_PI from it.
 */
void lfo_sine_init(lfo_t *mod, float freq, float samplerate, float phase)
{
    float w = freq / samplerate; /* intial angle */

    mod->a1 = cosf((float)M_PI * w);

    phase *= 0.5f;
    mod->buffer2 = sinf((float)M_PI * (phase - 2*w)); /* y(n-2) = sin(-intial angle) */
    mod->buffer1 = sinf((float)M_PI * (phase -   w)); /* y(n-1) = sin(initial phase) */
}

/*-----------------------------------------------------------------------------
 Output next value from sine LFO:
   out  = a1 * buffer1 -  buffer2

 This is based on the Chebyshev recurrence:
   sin(n*x) = 2*a*sin((n-1)*x) - sin((n-2)*x).
 Note that we also have for cos:
   cos(n*x) = 2*a*cos((n-1)*x) - cos((n-2)*x).

 In both cases a = cos(pi * freq / samplerate).

 *-----------------------------------------------------------------------------*/
float lfo_sine_get(lfo_t *mod)
{
    float out;
    out = 2*mod->a1 * mod->buffer1 - mod->buffer2;
    mod->buffer2 = mod->buffer1;

#if 0
    if(out >= 1.0f) /* reset in case of instability near PI/2 */
    {
        out = 1.0f; /* forces output to the right value */
        mod->buffer2 = mod->a1;
    }

    if(out <= -1.0f) /* reset in case of instability near -PI/2 */
    {
        out = -1.0f; /* forces output to the right value */
        mod->buffer2 = -mod->a1;
    }
#endif

    mod->buffer1 = out;
    /* It's an LFO, so shift wave up to make it always +ve.
     * Obviously if you just want the raw oscillator value
     * then just return out directly
     */
    return (0.5f * (1.0f + out));
}