view devuvuzelator-ladspa.cpp @ 3:8b79175c9f02

* fix off-by-one error

/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */

#include <ladspa.h>
#include <alloca.h>
#include <iostream>
#include <cmath>

#define FFTSIZE 1024

class Devuvuzelator
{
public:
    static const LADSPA_Descriptor *getDescriptor(unsigned long index);

private:
    Devuvuzelator(int sampleRate);
    ~Devuvuzelator();

    enum {
        InputPort     = 0,
	OutputPort    = 1,
        LowPort       = 2,
        HighPort      = 3,
        FundamentalPort = 4,
        BandwidthPort = 5,
        HarmonicsPort = 6,
        ReductionPort = 7,
	PortCount     = 8,
    };

    static const char *const portNames[PortCount];
    static const LADSPA_PortDescriptor ports[PortCount];
    static const LADSPA_PortRangeHint hints[PortCount];
    static const LADSPA_Properties properties;
    static const LADSPA_Descriptor ladspaDescriptor;

    static LADSPA_Handle instantiate(const LADSPA_Descriptor *, unsigned long);
    static void connectPort(LADSPA_Handle, unsigned long, LADSPA_Data *);
    static void activate(LADSPA_Handle);
    static void run(LADSPA_Handle, unsigned long);
    static void deactivate(LADSPA_Handle);
    static void cleanup(LADSPA_Handle);

    void reset();
    void window(float *);
    void runImpl(unsigned long);
    void processFrame();
    void processSpectralFrame();

    static void fft(unsigned int n, bool inverse,
                    double *ri, double *ii, double *ro, double *io);

    int m_sampleRate;
    float *m_input;
    float *m_output;
    float *m_low;
    float *m_high;
    float *m_fundamental;
    float *m_bandwidth;
    float *m_harmonics;
    float *m_reduction;

    const int m_fftsize;
    const int m_increment;
    int m_fill;
    int m_read;
    float *m_buffer;
    float *m_outacc;
    double *m_real;
    double *m_imag;
    double *m_window;
};

const char *const
Devuvuzelator::portNames[PortCount] =
{
    "Input",
    "Output",
    "Low threshold (dB)",
    "High threshold (dB)",
    "Fundamental frequency (Hz)",
    "Bandwidth of fundamental (Hz)",
    "Number of partials",
    "Reduction (dB)",
};

const LADSPA_PortDescriptor 
Devuvuzelator::ports[PortCount] =
{
    LADSPA_PORT_INPUT  | LADSPA_PORT_AUDIO,
    LADSPA_PORT_OUTPUT | LADSPA_PORT_AUDIO,
    LADSPA_PORT_INPUT  | LADSPA_PORT_CONTROL,
    LADSPA_PORT_INPUT  | LADSPA_PORT_CONTROL,
    LADSPA_PORT_INPUT  | LADSPA_PORT_CONTROL,
    LADSPA_PORT_INPUT  | LADSPA_PORT_CONTROL,
    LADSPA_PORT_INPUT  | LADSPA_PORT_CONTROL,
    LADSPA_PORT_INPUT  | LADSPA_PORT_CONTROL,
};

const LADSPA_PortRangeHint 
Devuvuzelator::hints[PortCount] =
{
    { 0, 0, 0 },
    { 0, 0, 0 },
    { LADSPA_HINT_DEFAULT_MIDDLE |
      LADSPA_HINT_BOUNDED_BELOW | LADSPA_HINT_BOUNDED_ABOVE, -80, 0 },
    { LADSPA_HINT_DEFAULT_HIGH |
      LADSPA_HINT_BOUNDED_BELOW | LADSPA_HINT_BOUNDED_ABOVE, -80, 0 },
    { LADSPA_HINT_DEFAULT_LOW |
      LADSPA_HINT_BOUNDED_BELOW | LADSPA_HINT_BOUNDED_ABOVE, 110, 550 },
    { LADSPA_HINT_DEFAULT_MIDDLE |
      LADSPA_HINT_BOUNDED_BELOW | LADSPA_HINT_BOUNDED_ABOVE, 20, 100 },
    { LADSPA_HINT_DEFAULT_MIDDLE | LADSPA_HINT_INTEGER |
      LADSPA_HINT_BOUNDED_BELOW | LADSPA_HINT_BOUNDED_ABOVE, 0, 6 },
    { LADSPA_HINT_DEFAULT_MIDDLE |
      LADSPA_HINT_BOUNDED_BELOW | LADSPA_HINT_BOUNDED_ABOVE, 0, 20 },
};

const LADSPA_Properties
Devuvuzelator::properties = LADSPA_PROPERTY_HARD_RT_CAPABLE;

const LADSPA_Descriptor 
Devuvuzelator::ladspaDescriptor =
{
    0, // "Unique" ID
    "devuvuzelator", // Label
    properties,
    "Devuvuzelator", // Name
    "Queen Mary, University of London", // Maker
    "All Rights Reserved", // Copyright
    PortCount,
    ports,
    portNames,
    hints,
    0, // Implementation data
    instantiate,
    connectPort,
    activate,
    run,
    0, // Run adding
    0, // Set run adding gain
    deactivate,
    cleanup
};

const LADSPA_Descriptor *
Devuvuzelator::getDescriptor(unsigned long index)
{
    if (index == 0) return &ladspaDescriptor;
    return 0;
}

Devuvuzelator::Devuvuzelator(int sampleRate) :
    m_sampleRate(sampleRate),
    m_input(0),
    m_output(0),
    m_low(0),
    m_high(0),
    m_fftsize(FFTSIZE),
    m_increment(m_fftsize/4),
    m_fill(0),
    m_read(0)
{
    m_buffer = new float[m_fftsize];
    m_outacc = new float[m_fftsize * 2];
    m_real = new double[m_fftsize];
    m_imag = new double[m_fftsize];
    m_window = new double[m_fftsize];

    for (int i = 0; i < m_fftsize; ++i) {
        m_window[i] = 0.5 - 0.5 * cos(2 * M_PI * i / m_fftsize);
    }

    reset();
}

Devuvuzelator::~Devuvuzelator()
{
    delete[] m_buffer;
    delete[] m_outacc;
    delete[] m_real;
    delete[] m_imag;
    delete[] m_window;
}
    
LADSPA_Handle
Devuvuzelator::instantiate(const LADSPA_Descriptor *, unsigned long rate)
{
    Devuvuzelator *devuvu = new Devuvuzelator(rate);
    return devuvu;
}

void
Devuvuzelator::connectPort(LADSPA_Handle handle,
                           unsigned long port, LADSPA_Data *location)
{
    Devuvuzelator *devuvu = (Devuvuzelator *)handle;

    float **ports[PortCount] = {
	&devuvu->m_input,
	&devuvu->m_output,
        &devuvu->m_low,
        &devuvu->m_high,
        &devuvu->m_fundamental,
        &devuvu->m_bandwidth,
        &devuvu->m_harmonics,
        &devuvu->m_reduction,
    };

    *ports[port] = (float *)location;
}

void
Devuvuzelator::activate(LADSPA_Handle handle)
{
    Devuvuzelator *devuvu = (Devuvuzelator *)handle;
    devuvu->reset();
}

void
Devuvuzelator::run(LADSPA_Handle handle, unsigned long samples)
{
    Devuvuzelator *devuvu = (Devuvuzelator *)handle;
    devuvu->runImpl(samples);
}

void
Devuvuzelator::deactivate(LADSPA_Handle handle)
{
    activate(handle); // both functions just reset the plugin
}

void
Devuvuzelator::cleanup(LADSPA_Handle handle)
{
    delete (Devuvuzelator *)handle;
}

void
Devuvuzelator::reset()
{
    for (int i = 0; i < m_fftsize; ++i) {
        m_buffer[i] = 0.f;
    }
    for (int i = 0; i < m_fftsize*2; ++i) {
        m_outacc[i] = 0.f;
    }
    m_fill = 0;
    m_read = 0;
}

void
Devuvuzelator::runImpl(unsigned long sampleCount)
{
    if (!m_input || !m_output) return;

    int ii = 0;
    int oi = 0;
    const int sc = sampleCount;

    while (ii < sc) {

        m_output[oi++] = m_outacc[m_read++] / 1.5f;

        if (m_fill == m_fftsize) {

            processFrame();

            for (int j = m_increment; j < m_fftsize; ++j) {
                m_buffer[j - m_increment] = m_buffer[j];
            }

            for (int j = m_increment; j < m_fftsize*2; ++j) {
                m_outacc[j - m_increment] = m_outacc[j];
            }

            for (int j = m_fftsize*2 - m_increment; j < m_fftsize*2; ++j) {
                m_outacc[j] = 0.f;
            }

            m_fill -= m_increment;
            m_read -= m_increment;
        }

        m_buffer[m_fill++] = m_input[ii++];
    }
}

void
Devuvuzelator::processFrame()
{
    double *frame = (double *)alloca(m_fftsize * sizeof(double));
    int ix = m_fftsize/2;
    for (int i = 0; i < m_fftsize; ++i) {
        frame[ix++] = m_buffer[i] * m_window[i];
        if (ix == m_fftsize) ix = 0;
    }

    fft(m_fftsize, false, frame, 0, m_real, m_imag);

    processSpectralFrame();

    for (int i = 0; i < m_fftsize/2-1; ++i) {
        m_real[m_fftsize-i-1] =  m_real[i+1];
        m_imag[m_fftsize-i-1] = -m_imag[i+1];
    }

    double *spare = (double *)alloca(m_fftsize * sizeof(double));
    fft(m_fftsize, true, m_real, m_imag, frame, spare);

    ix = m_fftsize/2;
    for (int i = 0; i < m_fftsize; ++i) {
        m_outacc[m_fftsize + i] += frame[ix++] * m_window[i];
        if (ix == m_fftsize) ix = 0;
    }
}

// FFT implementation by Don Cross, public domain.
// This version scales the forward transform.

void Devuvuzelator::fft(unsigned int n, bool inverse,
                        double *ri, double *ii, double *ro, double *io)
{
    if (!ri || !ro || !io) return;

    unsigned int bits;
    unsigned int i, j, k, m;
    unsigned int blockSize, blockEnd;

    double tr, ti;

    if (n < 2) return;
    if (n & (n-1)) return;

    double angle = 2.0 * M_PI;
    if (inverse) angle = -angle;

    for (i = 0; ; ++i) {
	if (n & (1 << i)) {
	    bits = i;
	    break;
	}
    }

    static unsigned int tableSize = 0;
    static int *table = 0;

    if (tableSize != n) {

	delete[] table;

	table = new int[n];

	for (i = 0; i < n; ++i) {
	
	    m = i;

	    for (j = k = 0; j < bits; ++j) {
		k = (k << 1) | (m & 1);
		m >>= 1;
	    }

	    table[i] = k;
	}

	tableSize = n;
    }

    if (ii) {
	for (i = 0; i < n; ++i) {
	    ro[table[i]] = ri[i];
	    io[table[i]] = ii[i];
	}
    } else {
	for (i = 0; i < n; ++i) {
	    ro[table[i]] = ri[i];
	    io[table[i]] = 0.0;
	}
    }

    blockEnd = 1;

    for (blockSize = 2; blockSize <= n; blockSize <<= 1) {

	double delta = angle / (double)blockSize;
	double sm2 = -sin(-2 * delta);
	double sm1 = -sin(-delta);
	double cm2 = cos(-2 * delta);
	double cm1 = cos(-delta);
	double w = 2 * cm1;
	double ar[3], ai[3];

	for (i = 0; i < n; i += blockSize) {

	    ar[2] = cm2;
	    ar[1] = cm1;

	    ai[2] = sm2;
	    ai[1] = sm1;

	    for (j = i, m = 0; m < blockEnd; j++, m++) {

		ar[0] = w * ar[1] - ar[2];
		ar[2] = ar[1];
		ar[1] = ar[0];

		ai[0] = w * ai[1] - ai[2];
		ai[2] = ai[1];
		ai[1] = ai[0];

		k = j + blockEnd;
		tr = ar[0] * ro[k] - ai[0] * io[k];
		ti = ar[0] * io[k] + ai[0] * ro[k];

		ro[k] = ro[j] - tr;
		io[k] = io[j] - ti;

		ro[j] += tr;
		io[j] += ti;
	    }
	}

	blockEnd = blockSize;
    }

    if (!inverse) {

	double denom = (double)n;

	for (i = 0; i < n; i++) {
	    ro[i] /= denom;
	    io[i] /= denom;
	}
    }
}

const LADSPA_Descriptor *
ladspa_descriptor(unsigned long ix)
{
    return Devuvuzelator::getDescriptor(ix);
}

#include "devuvuzelator.cpp"