/* blepvco - minBLEP-based, hard-sync-capable LADSPA VCOs.
 *
 * Copyright (C) 2004-2005 Sean Bolton.
 *
 * Much of the LADSPA framework used here comes from VCO-plugins
 * 0.3.0, copyright (c) 2003-2004 Fons Adriaensen.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public
 * License along with this program; if not, write to the Free
 * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 * MA 02111-1307, USA.
 */

#define _BSD_SOURCE    1
#define _SVID_SOURCE   1
#define _ISOC99_SOURCE 1

#include <stdio.h>
#include <string.h>
#include <math.h>
#include "minblep_tables.h"
#include "blepvco.h"


extern float exp2ap (float x);


void
Ladspa_minBLEP_VCO::place_step_dd(float *buffer, int index, float phase, float w, float scale)
{
    float r;
    int i;

    r = MINBLEP_PHASES * phase / w;
    i = lrintf(r - 0.5f);
    r -= (float)i;
    i &= MINBLEP_PHASE_MASK;  /* extreme modulation can cause i to be out-of-range */
    /* this would be better than the above, but more expensive:
     *  while (i < 0) {
     *    i += MINBLEP_PHASES;
     *    index++;
     *  }
     */

    while (i < MINBLEP_PHASES * STEP_DD_PULSE_LENGTH) {
        buffer[index] += scale * (step_dd_table[i].value + r * step_dd_table[i].delta);
        i += MINBLEP_PHASES;
        index++;
    }
}

void
Ladspa_minBLEP_VCO::place_slope_dd(float *buffer, int index, float phase, float w, float slope_delta)
{
    float r;
    int i;

    r = MINBLEP_PHASES * phase / w;
    i = lrintf(r - 0.5f);
    r -= (float)i;
    i &= MINBLEP_PHASE_MASK;  /* extreme modulation can cause i to be out-of-range */

    slope_delta *= w;

    while (i < MINBLEP_PHASES * SLOPE_DD_PULSE_LENGTH) {
        buffer[index] += slope_delta * (slope_dd_table[i] + r * (slope_dd_table[i + 1] - slope_dd_table[i]));
        i += MINBLEP_PHASES;
        index++;
    }
}

/* ==== hard-sync-capable sawtooth oscillator ==== */

void Ladspa_VCO_blepsaw::setport (unsigned long port, LADSPA_Data *data)
{
    _port [port] = data;
}


void Ladspa_VCO_blepsaw::active (bool act)
{
    _init = 1;
    _z = 0.0f;
    _j = 0;
    memset (_f, 0, (FILLEN + STEP_DD_PULSE_LENGTH) * sizeof (float));
}


void Ladspa_VCO_blepsaw::runproc (unsigned long len, bool add)
{
    int    j, n;
    float  *outp, *freq, *expm, *linm, *syncin, *syncout;
    float  a, p, t, w, dw, z;

    outp    = _port[OUTP];
    syncout = _port[SYNCOUT];
    syncin  = _port[SYNCIN];
    freq = _port[FREQ] - 1;
    expm = _port[EXPM] - 1;
    linm = _port[LINM] - 1;

    p = _p;  /* phase [0, 1) */
    w = _w;  /* phase increment */
    z = _z;  /* low pass filter state */
    j = _j;  /* index into buffer _f */

    if (_init) {
        p = 0.5f;
        w = (exp2ap (freq[1] + _port[OCTN][0] + _port[TUNE][0] + expm[1] * _port[EXPG][0] + 8.03136)
	     + 1e3 * linm[1] * _port[LING][0]) / _fsam; 
        if (w < 1e-5) w = 1e-5;
        if (w > 0.5) w = 0.5;
        /* if we valued alias-free startup over low startup time, we could do:
         *   p -= w;
         *   place_slope_dd(_f, j, 0.0f, w, -1.0f); */
        _init = 0;
    }

    a = 0.2 + 0.8 * _port [FILT][0];
    do
    {
        n = (len > 24) ? 16 : len;
        freq += n;
        expm += n;
        linm += n;
        len -= n;
 
        t = (exp2ap (*freq + _port[OCTN][0] + _port[TUNE][0] + *expm * _port[EXPG][0] + 8.03136)
	     + 1e3 * *linm * _port[LING][0]) / _fsam; 
        if (t < 1e-5) t = 1e-5;
        if (t > 0.5) t = 0.5;
        dw = (t - w) / n;

        while (n--)
	{
	    w += dw; 
            p += w;

            if (*syncin >= 1e-20f) {  /* sync to master */

                float eof_offset = (*syncin - 1e-20f) * w;
                float p_at_reset = p - eof_offset;
                p = eof_offset;

                /* place any DD that may have occurred in subsample before reset */
                if (p_at_reset >= 1.0f) {
                    p_at_reset -= 1.0f;
                    place_step_dd(_f, j, p_at_reset + eof_offset, w, 1.0f);
                }

                /* now place reset DD */
                place_step_dd(_f, j, p, w, p_at_reset);

                *syncout = *syncin;  /* best we can do is pass on upstream sync */

            } else if (p >= 1.0f) {  /* normal phase reset */

                p -= 1.0f;
                *syncout = p / w + 1e-20f;
                place_step_dd(_f, j, p, w, 1.0f);

            } else {

                *syncout = 0.0f;
            }
            _f[j + DD_SAMPLE_DELAY] += 0.5f - p;

            z += a * (_f[j] - z);
            *outp++ = z;
            syncin++;
            syncout++;
            if (++j == FILLEN)
            { 
                j = 0;
		memcpy (_f, _f + FILLEN, STEP_DD_PULSE_LENGTH * sizeof (float));
                memset (_f + STEP_DD_PULSE_LENGTH, 0,  FILLEN * sizeof (float));
	    }
	}
    }
    while (len);

    _p = p;
    _w = w;
    _z = z;
    _j = j;
}

/* ==== variable-width, hard-sync-capable rectangular oscillator ==== */

void Ladspa_VCO_bleprect::setport (unsigned long port, LADSPA_Data *data)
{
    _port [port] = data;
}


void Ladspa_VCO_bleprect::active (bool act)
{
    _init = 1;
    _z = 0.0f;
    _j = 0;
    memset (_f, 0, (FILLEN + STEP_DD_PULSE_LENGTH) * sizeof (float));
}


void Ladspa_VCO_bleprect::runproc (unsigned long len, bool add)
{
    int    j, k, n;
    float  *outp, *freq, *expm, *linm, *wavm, *syncin, *syncout;
    float  a, b, db, p, t, w, dw, x, z;

    outp    = _port[OUTP];
    syncout = _port[SYNCOUT];
    syncin  = _port[SYNCIN];
    freq = _port[FREQ] - 1;
    expm = _port[EXPM] - 1;
    linm = _port[LINM] - 1;
    wavm = _port[WAVM] - 1;

    p = _p;  /* phase [0, 1) */
    w = _w;  /* phase increment */
    b = _b;  /* duty cycle (0, 1) */
    x = _x;  /* temporary output variable */
    z = _z;  /* low pass filter state */
    j = _j;  /* index into buffer _f */
    k = _k;  /* output state, 0 = high (0.5f), 1 = low (-0.5f) */

    if (_init) {
        p = 0.0f;
        w = (exp2ap (freq[1] + _port[OCTN][0] + _port[TUNE][0] + expm[1] * _port[EXPG][0] + 8.03136)
	     + 1e3 * linm[1] * _port[LING][0]) / _fsam; 
        if (w < 1e-5) w = 1e-5;
        if (w > 0.5) w = 0.5;
        b = 0.5 * (1.0 + _port [WAVE][0] + wavm[1] * _port [WMDG][0]);
        if (b < w) b = w;
        if (b > 1.0f - w) b = 1.0f - w;
        /* for variable-width rectangular wave, we could do DC compensation with:
         *     x = 1.0f - b;
         * but that doesn't work well with highly modulated hard sync.  Instead,
         * we keep things in the range [-0.5f, 0.5f]. */
        x = 0.5f;
        /* if we valued alias-free startup over low startup time, we could do:
         *   p -= w;
         *   place_step_dd(_f, j, 0.0f, w, 0.5f); */
        k = 0;
        _init = 0;
    }

    a = 0.2 + 0.8 * _port [FILT][0];
    do
    {
        n = (len > 24) ? 16 : len;
        freq += n;
        expm += n;
        linm += n;
        wavm += n;
        len -= n;
 
        t = (exp2ap (*freq + _port[OCTN][0] + _port[TUNE][0] + *expm * _port[EXPG][0] + 8.03136)
	     + 1e3 * *linm * _port[LING][0]) / _fsam; 
        if (t < 1e-5) t = 1e-5;
        if (t > 0.5) t = 0.5;
        dw = (t - w) / n;
        t = 0.5 * (1.0 + _port [WAVE][0] + *wavm * _port [WMDG][0]);
        if (t < w) t = w;
        if (t > 1.0f - w) t = 1.0f - w;
        db = (t - b) / n;

        while (n--)
	{
	    w += dw; 
            b += db;
            p += w;

            if (*syncin >= 1e-20f) {  /* sync to master */

                float eof_offset = (*syncin - 1e-20f) * w;
                float p_at_reset = p - eof_offset;
                p = eof_offset;

                /* place any DDs that may have occurred in subsample before reset */
                if (!k) {
                    if (p_at_reset >= b) {
                        place_step_dd(_f, j, p_at_reset - b + eof_offset, w, -1.0f);
                        k = 1;
                        x = -0.5f;
                    }
                    if (p_at_reset >= 1.0f) {
                        p_at_reset -= 1.0f;
                        place_step_dd(_f, j, p_at_reset + eof_offset, w, 1.0f);
                        k = 0;
                        x = 0.5f;
                    }
                } else {
                    if (p_at_reset >= 1.0f) {
                        p_at_reset -= 1.0f;
                        place_step_dd(_f, j, p_at_reset + eof_offset, w, 1.0f);
                        k = 0;
                        x = 0.5f;
                    }
                    if (!k && p_at_reset >= b) {
                        place_step_dd(_f, j, p_at_reset - b + eof_offset, w, -1.0f);
                        k = 1;
                        x = -0.5f;
                    }
                }

                /* now place reset DD */
                if (k) {
                    place_step_dd(_f, j, p, w, 1.0f);
                    k = 0;
                    x = 0.5f;
                }
                if (p >= b) {
                    place_step_dd(_f, j, p - b, w, -1.0f);
                    k = 1;
                    x = -0.5f;
                }

                *syncout = *syncin;  /* best we can do is pass on upstream sync */

            } else if (!k) {  /* normal operation, signal currently high */

                if (p >= b) {
                    place_step_dd(_f, j, p - b, w, -1.0f);
                    k = 1;
                    x = -0.5f;
                }
                if (p >= 1.0f) {
                    p -= 1.0f;
                    *syncout = p / w + 1e-20f;
                    place_step_dd(_f, j, p, w, 1.0f);
                    k = 0;
                    x = 0.5f;
                } else {
                    *syncout = 0.0f;
                }

            } else {  /* normal operation, signal currently low */

                if (p >= 1.0f) {
                    p -= 1.0f;
                    *syncout = p / w + 1e-20f;
                    place_step_dd(_f, j, p, w, 1.0f);
                    k = 0;
                    x = 0.5f;
                } else {
                    *syncout = 0.0f;
                }
                if (!k && p >= b) {
                    place_step_dd(_f, j, p - b, w, -1.0f);
                    k = 1;
                    x = -0.5f;
                }
            }
            _f[j + DD_SAMPLE_DELAY] += x;

            z += a * (_f[j] - z);
            *outp++ = z;
            syncin++;
            syncout++;
            if (++j == FILLEN)
            { 
                j = 0;
		memcpy (_f, _f + FILLEN, STEP_DD_PULSE_LENGTH * sizeof (float));
                memset (_f + STEP_DD_PULSE_LENGTH, 0,  FILLEN * sizeof (float));
	    }
	}
    }
    while (len);

    _p = p;
    _w = w;
    _b = b;
    _x = x;
    _z = z;
    _j = j;
    _k = k;
}

/* ==== variable-slope, hard-sync-capable triangle oscillator ==== */

void Ladspa_VCO_bleptri::setport (unsigned long port, LADSPA_Data *data)
{
    _port [port] = data;
}


void Ladspa_VCO_bleptri::active (bool act)
{
    _init = 1;
    _z = 0.0f;
    _j = 0;
    memset (_f, 0, (FILLEN + STEP_DD_PULSE_LENGTH) * sizeof (float));
}


void Ladspa_VCO_bleptri::runproc (unsigned long len, bool add)
{
    int    j, k, n;
    float  *outp, *freq, *expm, *linm, *wavm, *syncin, *syncout;
    float  a, b, b1, db, p, t, w, dw, x, z;

    outp    = _port[OUTP];
    syncout = _port[SYNCOUT];
    syncin  = _port[SYNCIN];
    freq = _port[FREQ] - 1;
    expm = _port[EXPM] - 1;
    linm = _port[LINM] - 1;
    wavm = _port[WAVM] - 1;

    p = _p;  /* phase [0, 1) */
    w = _w;  /* phase increment */
    b = _b;  /* duty cycle (0, 1) */
    z = _z;  /* low pass filter state */
    j = _j;  /* index into buffer _f */
    k = _k;  /* output state, 0 = positive slope, 1 = negative slope */

    if (_init) {
        w = (exp2ap (freq[1] + _port[OCTN][0] + _port[TUNE][0] + expm[1] * _port[EXPG][0] + 8.03136)
	     + 1e3 * linm[1] * _port[LING][0]) / _fsam; 
        if (w < 1e-5) w = 1e-5;
        if (w > 0.5) w = 0.5;
        b = 0.5 * (1.0 + _port [WAVE][0] + wavm[1] * _port [WMDG][0]);
        if (b < w) b = w;
        if (b > 1.0f - w) b = 1.0f - w;
        p = 0.5f * b;
        /* if we valued alias-free startup over low startup time, we could do:
         *   p -= w;
         *   place_slope_dd(_f, j, 0.0f, w, 1.0f / b); */
        k = 0;
        _init = 0;
    }

    a = 0.2 + 0.8 * _port [FILT][0];
    do
    {
        n = (len > 24) ? 16 : len;
        freq += n;
        expm += n;
        linm += n;
        wavm += n;
        len -= n;
 
        t = (exp2ap (*freq + _port[OCTN][0] + _port[TUNE][0] + *expm * _port[EXPG][0] + 8.03136)
	     + 1e3 * *linm * _port[LING][0]) / _fsam; 
        if (t < 1e-5) t = 1e-5;
        if (t > 0.5) t = 0.5;
        dw = (t - w) / n;
        t = 0.5 * (1.0 + _port [WAVE][0] + *wavm * _port [WMDG][0]);
        if (t < w) t = w;
        if (t > 1.0f - w) t = 1.0f - w;
        db = (t - b) / n;

        while (n--)
	{
	    w += dw; 
            b += db;
            b1 = 1.0f - b;
            p += w;

            if (*syncin >= 1e-20f) {  /* sync to master */

                float eof_offset = (*syncin - 1e-20f) * w;
                float p_at_reset = p - eof_offset;
                p = eof_offset;

                /* place any DDs that may have occurred in subsample before reset */
                if (!k) {
                    x = -0.5f + p_at_reset / b;
                    if (p_at_reset >= b) {
                        x = 0.5f - (p_at_reset - b) / b1;
                        place_slope_dd(_f, j, p_at_reset - b + eof_offset, w, -1.0f / b1 - 1.0f / b);
                        k = 1;
                    }
                    if (p_at_reset >= 1.0f) {
                        p_at_reset -= 1.0f;
                        x = -0.5f + p_at_reset / b;
                        place_slope_dd(_f, j, p_at_reset + eof_offset, w, 1.0f / b + 1.0f / b1);
                        k = 0;
                    }
                } else {
                    x = 0.5f - (p_at_reset - b) / b1;
                    if (p_at_reset >= 1.0f) {
                        p_at_reset -= 1.0f;
                        x = -0.5f + p_at_reset / b;
                        place_slope_dd(_f, j, p_at_reset + eof_offset, w, 1.0f / b + 1.0f / b1);
                        k = 0;
                    }
                    if (!k && p_at_reset >= b) {
                        x = 0.5f - (p_at_reset - b) / b1;
                        place_slope_dd(_f, j, p_at_reset - b + eof_offset, w, -1.0f / b1 - 1.0f / b);
                        k = 1;
                    }
                }

                /* now place reset DDs */
                if (k)
                    place_slope_dd(_f, j, p, w, 1.0f / b + 1.0f / b1);
                place_step_dd(_f, j, p, w, -0.5f - x);
                x = -0.5f + p / b;
                k = 0;
                if (p >= b) {
                    x = 0.5f - (p - b) / b1;
                    place_slope_dd(_f, j, p - b, w, -1.0f / b1 - 1.0f / b);
                    k = 1;
                }
                *syncout = *syncin;  /* best we can do is pass on upstream sync */

            } else if (!k) {  /* normal operation, slope currently up */

                x = -0.5f + p / b;
                if (p >= b) {
                    x = 0.5f - (p - b) / b1;
                    place_slope_dd(_f, j, p - b, w, -1.0f / b1 - 1.0f / b);
                    k = 1;
                }
                if (p >= 1.0f) {
                    p -= 1.0f;
                    *syncout = p / w + 1e-20f;
                    x = -0.5f + p / b;
                    place_slope_dd(_f, j, p, w, 1.0f / b + 1.0f / b1);
                    k = 0;
                } else {
                    *syncout = 0.0f;
                }

            } else {  /* normal operation, slope currently down */

                x = 0.5f - (p - b) / b1;
                if (p >= 1.0f) {
                    p -= 1.0f;
                    *syncout = p / w + 1e-20f;
                    x = -0.5f + p / b;
                    place_slope_dd(_f, j, p, w, 1.0f / b + 1.0f / b1);
                    k = 0;
                } else {
                    *syncout = 0.0f;
                }
                if (!k && p >= b) {
                    x = 0.5f - (p - b) / b1;
                    place_slope_dd(_f, j, p - b, w, -1.0f / b1 - 1.0f / b);
                    k = 1;
                }
            }
            _f[j + DD_SAMPLE_DELAY] += x;

            z += a * (_f[j] - z);
            *outp++ = z;
            syncin++;
            syncout++;
            if (++j == FILLEN)
            { 
                j = 0;
		memcpy (_f, _f + FILLEN, STEP_DD_PULSE_LENGTH * sizeof (float));
                memset (_f + STEP_DD_PULSE_LENGTH, 0,  FILLEN * sizeof (float));
	    }
	}
    }
    while (len);

    _p = p;
    _w = w;
    _b = b;
    _z = z;
    _j = j;
    _k = k;
}

