view host/Processor.cpp @ 15:df33703ace3b

Get building with newer libraries &c

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

#include "Processor.h"

#include "plugin/FeatureExtractionPluginFactory.h"

//#define DEBUG_RUN_PROCESSOR 1

using std::cout;
using std::cerr;
using std::endl;

void printFeatures(int, int, int, int, Vamp::Plugin::FeatureSet &);
void transformInput(float *, size_t);
void fft(unsigned int, bool, double *, double *, double *, double *);

Processor::Processor(BufferingAudioCallbackRecordTarget *audioRecordTarget) :
    m_exiting(false),
    m_audioRecordTarget(audioRecordTarget),
    m_nextNumber(1),
    m_havePlugins(false),
    m_minBlockSize(0),
    m_maxBlockSize(0)
{
    for (int i = 0; i < MAX_DISTINCT_STEP_SIZES; ++i) {
        m_unusedReaders.insert(i);
    }
}

Processor::~Processor()
{
    m_exiting = true;
    wait();

    for (RuleSet::const_iterator i = m_rules.begin(); i != m_rules.end(); ++i) {
        delete *i;
    }
}

void
Processor::run()
{
    while (!m_exiting) {

        if (!m_havePlugins) {
            msleep(100);
            continue;
        }

        size_t nframes = 0;

        for (StepSizeReaderMap::iterator i = m_stepSizeReaderMap.begin();
             i != m_stepSizeReaderMap.end(); ++i) {
            size_t here = m_audioRecordTarget->samplesReady(i->second);
            if (here > nframes) {
                nframes = here;
            }
        }

        if (nframes > m_minBlockSize) {
            if (!runPlugins()) {
                msleep(10);
            }
        } else {
            msleep(50);
        }

        m_mutex.lock();
        for (ReaderSet::iterator i = m_unusedReaders.begin();
             i != m_unusedReaders.end(); ++i) {
            size_t ch = m_audioRecordTarget->getChannelCount();
            for (size_t c = 0; c < ch; ++c) {
                m_audioRecordTarget->skipAllSamples(c, *i);
            }
        }
        m_mutex.unlock();
    }
}

bool
Processor::runPlugins()
{
    QMutexLocker locker(&m_mutex);

    static std::map<int, size_t> frame; // reader -> frame
    if (frame.empty()) {
        for (int i = 0; i < MAX_DISTINCT_STEP_SIZES; ++i) {
            frame[i] = 0;
        }
    }

    size_t sr = m_audioRecordTarget->getSourceSampleRate();

//    cerr << "max block size " << m_maxBlockSize << endl;

    //!!!
    size_t ch = m_audioRecordTarget->getChannelCount();
    float **buffers = new float *[ch];
    float **transformed = new float *[ch];
    for (size_t c = 0; c < ch; ++c) {
        buffers[c] = new float[m_maxBlockSize];
        transformed[c] = new float[m_maxBlockSize + 2];
    }

    bool doneWork = false;

    for (StepSizePluginMap::iterator i = m_processingMap.begin();
         i != m_processingMap.end(); ++i) {

        size_t step = i->first;
        if (m_stepSizeReaderMap.find(step) == m_stepSizeReaderMap.end()) {
            cerr << "ERROR: Unrecorded step size " << step << endl;
            continue;
        }

        int reader = m_stepSizeReaderMap[step];
        size_t maxBlock = 0;

        for (BlockSizePluginMap::iterator j = i->second.begin();
             j != i->second.end(); ++j) {

            if (j == i->second.begin() || j->first > maxBlock) {
                maxBlock = j->first;
            }
        }

        if (maxBlock == 0) {
            cerr << "ERROR: maxBlock == 0 for step " << step << endl;
            continue;
        }

//        int counter = 0;

        while (m_audioRecordTarget->samplesReady(reader) >= maxBlock) {
//              && counter < 10) {

//            ++counter;

#ifdef DEBUG_RUN_PROCESSOR
            cerr << "enough samples for max block " << maxBlock
                 << " for step " << step << endl;
#endif

            for (size_t c = 0; c < ch; ++c) {
                size_t got = m_audioRecordTarget->peekSamples(c, maxBlock, buffers[c], reader);
                if (got < maxBlock) {
                    cerr << "ERROR: Requested " << maxBlock << " samples on channel " << c << " reader " << reader << ", only got " << got << endl;
                }
                size_t skipped = m_audioRecordTarget->skipSamples(c, step, reader);
                if (skipped < step) {
                    cerr << "ERROR: Requested skip of " << step << " samples on channel " << c << " reader " << reader << ", only skipped " << skipped << endl;
                }
            }

            for (BlockSizePluginMap::iterator j = i->second.begin();
                 j != i->second.end(); ++j) {

                size_t block = j->first;
                size_t toUse = maxBlock;
                size_t off = 0;

                while (toUse >= block) {

                    bool silentInput = true;
                    for (size_t s = 0; s < block; ++s) {
                        for (size_t c = 0; c < ch; ++c) {
                            if (fabsf(buffers[c][s + off]) > 0.00001) {
                                silentInput = false;
                                break;
                            }
                        }
                        if (!silentInput) break;
                    }

                    bool haveTransformed = false;
                
                    for (PluginSet::iterator k = j->second.begin();
                         k != j->second.end(); ++k) {

#ifdef DEBUG_RUN_PROCESSOR
                        cerr << "block inner loop for block " << block
                             << " (toUse = " << toUse << ")" << endl;
#endif

                        Vamp::Plugin *plugin = *k;
                        Vamp::Plugin::FeatureSet fs;
                        Vamp::RealTime timestamp =
                            Vamp::RealTime::frame2RealTime(frame[reader], sr);
                        
                        if (plugin->getInputDomain() == Vamp::Plugin::FrequencyDomain) {
                            if (!haveTransformed) {
                                for (size_t c = 0; c < ch; ++c) {
                                    for (size_t s = 0; s < block; ++s) {
                                        transformed[c][s] = buffers[c][s + off];
                                    }
                                    transformInput(transformed[c], block);
                                }
                                haveTransformed = true;
                            }
#ifdef DEBUG_RUN_PROCESSOR
                            cerr << "running " << plugin->getName() << " (transformed) frame = " << frame[reader] << endl;
#endif
                            fs = plugin->process(transformed, timestamp);
                        } else {
#ifdef DEBUG_RUN_PROCESSOR
                            cerr << "running " << plugin->getName() << " frame = " << frame[reader] << endl;
#endif
                            float *tmp[10];
                            for (size_t c = 0; c < ch; ++c) {
                                tmp[c] = buffers[c] + off;
                            }
                            fs = plugin->process(tmp, timestamp);
                        }
            
                        int pluginIndex = m_pluginRMap[plugin];

                        if (fs.empty()) {
                            for (OutputStateMap::iterator oi = 
                                     m_pluginStates[pluginIndex].begin();
                                 oi != m_pluginStates[pluginIndex].end(); ++oi) {
                                int output = oi->first;
                                OutputState currentState = oi->second;
                            
                                Vamp::RealTime rt =
                                    Vamp::RealTime::frame2RealTime(frame[reader], sr);

//                                bool changed = currentState.present;
                                bool changed = false;
                                if (currentState.silentInput && !silentInput) {
                                    changed = true;
                                }
                                Vamp::RealTime gap = rt - currentState.laststamp;
                                m_pluginStates[pluginIndex][output] =
                                    OutputState(false, silentInput, changed, currentState.value, currentState.laststamp, currentState.gap);
                            }
                        }

                        for (Vamp::Plugin::FeatureSet::iterator fi = fs.begin();
                             fi != fs.end(); ++fi) {

                            int output = fi->first;

                            //!!! For each output, we need to step
                            //through the returned features one by one
                            //and update the plugin state for each,
                            //then run the rules each time a timestamp
                            //changes.  But it has to do this with the
                            //features in the order of their
                            //timestamps, not in order of
                            //plugin/output enumeration.
                            //
                            // But we don't do that yet, we just use
                            // the first feature in the block (if any)

                            Vamp::RealTime rt =
                                Vamp::RealTime::frame2RealTime(frame[reader], sr);

                            Vamp::Plugin::FeatureList fl(fi->second);
                            
                            OutputState currentState = 
                                m_pluginStates[pluginIndex][output];
                            
                            if (fl.empty()) {
//                                bool changed = currentState.present;
                                bool changed = false;
                                if (currentState.silentInput && !silentInput) {
                                    changed = true;
                                }
                                Vamp::RealTime gap = rt - currentState.laststamp;
                                m_pluginStates[pluginIndex][output] =
                                    OutputState(false, silentInput, changed, currentState.value, currentState.laststamp, currentState.gap);
                            } else {

                                if (fl[0].hasTimestamp) {
                                    rt = fl[0].timestamp;
                                }
                                float value = 0.f;
                                if (!fl[0].values.empty()) {
                                    value = fl[0].values[0];
                                }
                                bool changed =
//                                    (!currentState.present ||
                                     (fabsf(currentState.value - value) >
                                      0.000001)
//                                    )
                                    ;
                                if (currentState.silentInput && !silentInput) {
                                    changed = true;
                                }
                                if (changed) {
                                    std::cerr << "changed: " <<currentState.value << " -> " << value << std::endl;
                                }
                                Vamp::RealTime gap = rt - currentState.laststamp;
//                                std::cerr << "gap = " << gap << std::endl;
                                m_pluginStates[pluginIndex][output] =
                                    OutputState(true, silentInput, changed, value, rt, gap);
                            }

                            printFeatures(pluginIndex, frame[reader], sr, output, fs);
                        }

                        processRules();

                        doneWork = true;
                    }

                    toUse -= block;
                    off += block;
                }
            }

            frame[reader] += step;
        }

    }

    for (size_t c = 0; c < ch; ++c) {
        delete[] buffers[c];
        delete[] transformed[c];
    }
    delete[] buffers;
    delete[] transformed;

    return doneWork;
}
    
int
Processor::addPlugin(QString pluginId)
{
    QMutexLocker locker(&m_mutex);

    size_t sr = m_audioRecordTarget->getSourceSampleRate();

    if (!sr) {
        cerr << "ERROR: Processor::addPlugin: Source sample rate is not defined" << endl;
        return 0;
    }

    FeatureExtractionPluginFactory *factory = 
        FeatureExtractionPluginFactory::instanceFor(pluginId);

    if (!factory) {
        cerr << "ERROR: Processor::addPlugin: No factory for plugin \"" << pluginId.toStdString() << "\"" << endl;
        return 0;
    }

    Vamp::Plugin *plugin = factory->instantiatePlugin(pluginId, sr);

    if (!plugin) {
        cerr << "ERROR: Processor::addPlugin: Failed to instantiate plugin \"" << pluginId.toStdString() << "\"" << endl;
        return 0;
    }

    size_t block = plugin->getPreferredBlockSize();
    if (block == 0) block = m_audioRecordTarget->getSourceBlockSize();

    size_t step = plugin->getPreferredStepSize();
    if (step == 0) {
        if (plugin->getInputDomain() == Vamp::Plugin::TimeDomain) {
            step = block;
        } else {
            step = block/2;
        }
    }

    if (m_stepSizeReaderMap.find(step) == m_stepSizeReaderMap.end()) {
        if (m_unusedReaders.empty()) {
            cerr << "ERROR: Processor::addPlugin: Run out of distinct step size slots: increase MAX_DISTINCT_STEP_SIZES and recompile" << endl;
            delete plugin;
            return 0;
        }
        int reader = *m_unusedReaders.begin();
        m_unusedReaders.erase(reader);
        m_stepSizeReaderMap[step] = reader;
    }

    size_t ch = m_audioRecordTarget->getChannelCount();
    if (ch > plugin->getMaxChannelCount()) ch = plugin->getMaxChannelCount();

    if (!plugin->initialise(ch, step, block)) {
        cerr << "ERROR: Processor::addPlugin: Initialisation failed with step size " << step << ", block size " << block << ", channels " << ch << endl;
        delete plugin;
        return 0;
    }

    if (plugin->getInputDomain() == Vamp::Plugin::FrequencyDomain) {
        block = block + 2; // but we should just use plugin adapter
    }
    if (!m_havePlugins || block < m_minBlockSize) m_minBlockSize = block;
    if (!m_havePlugins || block > m_maxBlockSize) m_maxBlockSize = block;

    std::cerr << "initialised plugin \"" << pluginId.toStdString() 
              << "\" with step \"" << step << "\" block \"" << block << "\""
              << std::endl;

    int number = m_nextNumber++;
    m_plugins[number] = plugin;
    m_pluginRMap[plugin] = number;
    m_processingMap[step][block].insert(plugin);

    m_havePlugins = true;
    return number;
}

void
Processor::removePlugin(int number)
{
    QMutexLocker locker(&m_mutex);

    if (m_plugins.find(number) == m_plugins.end()) {
        cerr << "ERROR: Processor::removePlugin: No such plugin number "
                  << number << endl;
        return;
    }
    
    Vamp::Plugin *plugin = m_plugins[number];
    bool done = false;

    for (StepSizePluginMap::iterator i = m_processingMap.begin();
         i != m_processingMap.end(); ++i) {

        BlockSizePluginMap::iterator j;

        for (j = i->second.begin(); j != i->second.end(); ++j) {

            if (j->second.find(plugin) != j->second.end()) {

                j->second.erase(plugin);

                if (j->second.empty()) { // no plugins with this step & block

                    i->second.erase(j->first);

                    if (i->second.empty()) { // no plugins with this step
                        
                        m_processingMap.erase(i->first);
                        m_stepSizeReaderMap.erase(i->first);
                        m_unusedReaders.insert(i->first);
                    }
                }
                
                done = true;
                break;
            }
        }

        if (done) break;
    }

    m_plugins.erase(number);
    m_pluginRMap.erase(plugin);
    if (m_plugins.empty()) m_havePlugins = false;
    delete plugin;
}

void
Processor::addRule(Rule *rule)
{
    cerr << "adding rule " << rule << endl;
    m_rules.insert(rule);
}

bool
Processor::processRules()
{
    for (RuleSet::iterator i = m_rules.begin(); i != m_rules.end(); ++i) {

        Rule *rule(*i);

        bool passed = false;
        bool oneChanged = false;

        for (Rule::ConditionList::const_iterator j = rule->getConditions().begin();
             j != rule->getConditions().end(); ++j) {

            Condition condition(*j);

            int pluginIndex = condition.getPluginIndex();
            int outputNumber = condition.getOutputNumber();
            Condition::Type type = condition.getType();
            float argument = condition.getArgument();

            OutputState state = m_pluginStates[pluginIndex][outputNumber];
        
#ifdef DEBUG_RUN_PROCESSOR
            std::cerr << "Present = " << state.present << ", changed = " << state.changed << ", value = " << state.value << std::endl;
#endif

            passed = false;

            if (state.silentInput) {
                break;
            }

            if (state.changed) {
//                std::cerr << "State changed with present = " << state.present << ", value = " << state.value << std::endl;
                oneChanged = true;
            }

            switch (type) {

            case Condition::GreaterThan:
                passed = (state.value > argument);
                break;
                
            case Condition::LessThan:
                passed = (state.value < argument);
                break;
                
            case Condition::EqualTo:
                passed = fabsf(state.value - argument) < 0.000001;
                break;
                
            case Condition::NotEqualTo:
                passed = fabsf(state.value - argument) > 0.000001;
                break;
                
            case Condition::Present:
                passed = state.present;
                if (passed) oneChanged = true;
                break;
                
            case Condition::Changed:
                passed = state.changed;
                break;
                
            case Condition::GapGreaterThan:
                passed = (state.gap > Vamp::RealTime::fromSeconds(argument));
                break;
                
            case Condition::GapLessThan:
                std::cout << "GapLessThan: gap is " << state.gap << ", argument is " << argument << std::endl;
                passed = (state.gap < Vamp::RealTime::fromSeconds(argument));
                break;
            }

            if (!passed) break;
        }

        if (passed && oneChanged) {
            for (Rule::ActionList::const_iterator ai = rule->getActions().begin();
                 ai != rule->getActions().end(); ++ai) {
                Action *action = *ai;
                if (action) {
//                    std::cerr << "FIRING RULE: " << action->getName().toStdString() << "!" << std::endl;
                    action->fire();
                }
            }
        }
    }

    return true;
}        

void
printFeatures(int plugno, int frame, int sr, int output, Vamp::Plugin::FeatureSet &features)
{
    return;//!!!

    if (output > 0) return;//!!!

    for (unsigned int i = 0; i < features[output].size(); ++i) {
        Vamp::RealTime rt = Vamp::RealTime::frame2RealTime(frame, sr);
        if (features[output][i].hasTimestamp) {
            rt = features[output][i].timestamp;
        }
        cout << plugno << ":" << output << " -" << rt.toString() << ":";
        for (unsigned int j = 0; j < features[output][i].values.size(); ++j) {
            cout << " " << features[output][i].values[j];
        }
        cout << endl;
    }
}
        
void
transformInput(float *buffer, size_t size)
{
    double *inbuf = new double[size * 2];
    double *outbuf = new double[size * 2];

    // Copy across with Hanning window
    for (size_t i = 0; i < size; ++i) {
        inbuf[i] = double(buffer[i]) * (0.50 - 0.50 * cos(2 * M_PI * i / size));
        inbuf[i + size] = 0.0;
    }
    
    for (size_t i = 0; i < size/2; ++i) {
        double temp = inbuf[i];
        inbuf[i] = inbuf[i + size/2];
        inbuf[i + size/2] = temp;
    }

    fft(size, false, inbuf, inbuf + size, outbuf, outbuf + size);

    for (size_t i = 0; i <= size/2; ++i) {
        buffer[i * 2] = outbuf[i];
        buffer[i * 2 + 1] = outbuf[i + size];
    }
    
    delete[] inbuf;
    delete[] outbuf;
}

void
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;
	}
    }
}