Chris@42: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
Chris@42: 
Chris@42: /*
Chris@42:     Tipic
Chris@42: 
Chris@42:     Centre for Digital Music, Queen Mary, University of London.
Chris@42: 
Chris@42:     This program is free software; you can redistribute it and/or
Chris@42:     modify it under the terms of the GNU General Public License as
Chris@42:     published by the Free Software Foundation; either version 2 of the
Chris@42:     License, or (at your option) any later version.  See the file
Chris@42:     COPYING included with this distribution for more information.
Chris@42: */
Chris@5: 
Chris@5: #include "PitchFilterbank.h"
Chris@5: 
Chris@42: #include "dsp/rateconversion/Resampler.h"
Chris@42: #include "dsp/signalconditioning/Filter.h"
Chris@5: 
Chris@5: #include "delays.h"
Chris@5: #include "filter-a.h"
Chris@5: #include "filter-b.h"
Chris@5: 
Chris@11: #include <deque>
Chris@11: #include <map>
Chris@6: #include <stdexcept>
Chris@11: #include <iostream>
Chris@59: #include <algorithm>
Chris@6: 
Chris@27: #include <cmath>
Chris@27: 
Chris@5: using namespace std;
Chris@5: 
Chris@5: static const int HIGHEST_FILTER_INDEX_AT_882 = 38;   // MIDI pitch 59
Chris@5: static const int HIGHEST_FILTER_INDEX_AT_4410 = 74;  // MIDI pitch 95
Chris@5: static const int HIGHEST_FILTER_INDEX_AT_22050 = 87; // MIDI pitch 108
Chris@5: static const int HIGHEST_FILTER_INDEX = HIGHEST_FILTER_INDEX_AT_22050;
Chris@5: 
Chris@5: class PitchFilterbank::D
Chris@5: {
Chris@5: public:
Chris@27:     D(int sampleRate, double tuningFrequency) :
Chris@11: 	m_nfilters(HIGHEST_FILTER_INDEX + 1),
Chris@15: 	m_sampleRate(sampleRate),
Chris@28: 	m_tuningFrequency(tuningFrequency),
Chris@28: 	m_blockNo(0)
Chris@5:     {
Chris@28: 	// To handle a non-440Hz tuning frequency, we resample the
Chris@28: 	// input and then adjust the output block timings
Chris@28: 	// accordingly. For a tuning freq >440 we want to lower the
Chris@28: 	// pitch of the input audio by slowing it down, therefore we
Chris@28: 	// want to pretend that it came in at a lower sample rate than
Chris@28: 	// it really did; for >440 the opposite applies. The effective
Chris@28: 	// input sample rate is the rate at which we pretend the audio
Chris@28: 	// was supplied. Rounding to the nearest int (because our
Chris@28: 	// resampler only supports integer rates) gives around 0.1Hz
Chris@28: 	// quantization close to 440Hz in 44.1kHz audio -- we could do
Chris@28: 	// better by using multiples of our source and target sample
Chris@28: 	// rates, but I think it probably isn't necessary.
Chris@29: 	m_effectiveInputRate =
Chris@28: 	    int(round(m_sampleRate * (440.0 / m_tuningFrequency)));
Chris@27: 
Chris@18: 	//!!! nb "we use forward-backward filtering such that the
Chris@18: 	// resulting output signal has precisely zero phase distortion
Chris@18: 	// and a magnitude modified by the square of the filter’s
Chris@18: 	// magnitude response" -- we are not doing forward-backward
Chris@18: 	// here & so need to adapt magnitudes in compensation to match
Chris@18: 	// original
Chris@29: 
Chris@29: 	double snr = 50.0;
Chris@29: 	double bw = 0.05;
Chris@29: 	m_resamplers[882] = new Resampler(m_effectiveInputRate, 882, snr, bw);
Chris@29: 	m_resamplers[4410] = new Resampler(m_effectiveInputRate, 4410, snr, bw);
Chris@29: 	m_resamplers[22050] = new Resampler(m_effectiveInputRate, 22050, snr, bw);
Chris@11: 	
Chris@11: 	for (int i = 0; i < m_nfilters; ++i) {
Chris@5: 	    int ix = i + 20;
Chris@5: 	    int coeffs = sizeof(filter_a[0]) / sizeof(filter_a[0][0]);
Chris@5: 	    vector<double> a(filter_a[ix], filter_a[ix] + coeffs);
Chris@5: 	    vector<double> b(filter_b[ix], filter_b[ix] + coeffs);
Chris@10: 	    m_filters.push_back(new Filter({ a, b }));
Chris@12: 	    m_toCompensate.push_back(totalDelay(i));
Chris@5: 	}
Chris@11: 
Chris@11: 	m_filtered.resize(m_nfilters);
Chris@11: 	m_energies.resize(m_nfilters);
Chris@5:     }
Chris@5: 	
Chris@9:     ~D() {
Chris@11: 	for (auto f: m_filters) delete f;
Chris@11: 	for (auto r: m_resamplers) delete r.second;
Chris@9:     }
Chris@5: 
Chris@9:     int getSampleRate() const { return m_sampleRate; }
Chris@27:     double getTuningFrequency() const { return m_tuningFrequency; }
Chris@5: 
Chris@10:     RealBlock process(const RealSequence &in) {
Chris@5: 
Chris@11: 	for (auto r: m_resamplers) {
Chris@11: 	    m_resampled[r.first] = r.second->process(in.data(), in.size());
Chris@11: 	}
Chris@5: 
Chris@11: 	for (int i = 0; i < m_nfilters; ++i) {
Chris@11: 	    int rate = filterRate(i);
Chris@11: 	    if (m_resampled.find(rate) == m_resampled.end()) {
Chris@11: 		throw logic_error("No resampled output for rate of filter");
Chris@10: 	    }
Chris@13: 	    pushFiltered(i, m_resampled[rate], false);
Chris@10: 	}
Chris@10: 
Chris@12: 	return energiesFromFiltered(false);
Chris@12:     }
Chris@12: 
Chris@12:     RealBlock getRemainingOutput() {
Chris@12: 
Chris@12: 	for (auto r: m_resamplers) {
Chris@12: 	    RealSequence in(r.second->getLatency(), 0.0);
Chris@12: 	    m_resampled[r.first] = r.second->process(in.data(), in.size());
Chris@12: 	}
Chris@12: 
Chris@12: 	for (int i = 0; i < m_nfilters; ++i) {
Chris@12: 	    int rate = filterRate(i);
Chris@13: 	    pushFiltered(i, m_resampled[rate], true);
Chris@12: 	}
Chris@12: 	
Chris@12: 	return energiesFromFiltered(true);
Chris@12:     }
Chris@28: 
Chris@28:     struct WindowPosition {
Chris@28: 	uint64_t start;
Chris@28: 	int size;
Chris@28: 	double factor;
Chris@28:     };
Chris@28:     
Chris@28:     WindowPosition windowPosition(int block, int i) {
Chris@28: 
Chris@32: 	int hop = 2205;
Chris@28: 
Chris@28: 	double rate = filterRate(i);
Chris@28: 	double topRate = 22050.0;
Chris@28: 	double rateRatio = topRate / rate;
Chris@29: 	double tuningRatio = m_sampleRate / double(m_effectiveInputRate);
Chris@28: 	double sizeRatio = tuningRatio / rateRatio;
Chris@28: 
Chris@32: 	uint64_t start(round((hop * uint64_t(block)) * sizeRatio));
Chris@28: 	int size(round((hop * 2) * sizeRatio));
Chris@28: 
Chris@28: 	return { start, size, rateRatio };
Chris@28:     }
Chris@12:     
Chris@12:     RealBlock energiesFromFiltered(bool drain) {
Chris@10: 
Chris@11: 	for (int i = 0; i < m_nfilters; ++i) {
Chris@11: 
Chris@28: 	    WindowPosition here = windowPosition(m_blockNo, i);
Chris@28: 	    WindowPosition next = windowPosition(m_blockNo + 1, i);
Chris@28: 
Chris@28: 	    int n = here.size;
Chris@28: 	    int hop = next.start - here.start;
Chris@12: 
Chris@12: 	    unsigned int minReq = n;
Chris@12: 	    if (drain) minReq = hop;
Chris@12: 
Chris@31: 	    // we use a separate buffer for each filter (not just one
Chris@31: 	    // per resampling ratio) because each filter has a
Chris@31: 	    // different delay, which we are compensating for when
Chris@31: 	    // first filling the buffer.
Chris@31: 	    
Chris@12: 	    while (m_filtered[i].size() >= minReq) {
Chris@28: 		double energy = calculateEnergy(m_filtered[i], n, here.factor);
Chris@11: 		m_energies[i].push_back(energy);
Chris@11: 		m_filtered[i] = RealSequence(m_filtered[i].begin() + hop,
Chris@11: 					     m_filtered[i].end());
Chris@10: 	    }
Chris@10: 	}
Chris@10: 
Chris@28: 	++m_blockNo;
Chris@28: 	
Chris@13: 	int minCols = 0, maxCols = 0;
Chris@11: 	for (int i = 0; i < m_nfilters; ++i) {
Chris@11: 	    int n = m_energies[i].size();
Chris@13: 	    if (i == 0 || n < minCols) minCols = n;
Chris@13: 	    if (i == 0 || n > maxCols) maxCols = n;
Chris@13: 	}
Chris@13: 
Chris@13: 	RealBlock out;
Chris@13: 	
Chris@13: 	if (drain) {
Chris@13: 	    out.resize(maxCols);
Chris@13: 	    for (int j = 0; j < maxCols; ++j) {
Chris@13: 		for (int i = 0; i < m_nfilters; ++i) {
Chris@13: 		    if (m_energies[i].size() == 0) {
Chris@13: 			out[j].push_back(0.0);
Chris@13: 		    } else {
Chris@13: 			out[j].push_back(m_energies[i][0]);
Chris@13: 			m_energies[i].pop_front();
Chris@13: 		    }
Chris@13: 		}
Chris@11: 	    }
Chris@13: 	} else {
Chris@13: 	    out.resize(minCols);
Chris@13: 	    for (int j = 0; j < minCols; ++j) {
Chris@13: 		for (int i = 0; i < m_nfilters; ++i) {
Chris@13: 		    out[j].push_back(m_energies[i][0]);
Chris@13: 		    m_energies[i].pop_front();
Chris@13: 		}
Chris@10: 	    }
Chris@10: 	}
Chris@10: 
Chris@10: 	return out;
Chris@10:     }
Chris@10:     
Chris@13:     void pushFiltered(int i, const RealSequence &resampled, bool drain) {
Chris@12: 
Chris@13: 	RealSequence in(resampled);
Chris@13: 	if (drain) {
Chris@13: 	    RealSequence pad(filterDelay(i), 0.0);
Chris@13: 	    in.insert(in.end(), pad.begin(), pad.end());
Chris@13: 	}
Chris@13: 	
Chris@13: 	int n = in.size();
Chris@10: 	RealSequence filtered(n, 0.0);
Chris@13: 	m_filters[i]->process(in.data(), filtered.data(), n);
Chris@12: 
Chris@12: 	int pushStart = 0, pushCount = n;
Chris@12: 
Chris@12: 	if (m_toCompensate[i] > 0) {
Chris@12: 	    pushCount = max(pushCount - m_toCompensate[i], 0);
Chris@12: 	    int compensating = n - pushCount;
Chris@12: 	    pushStart += compensating;
Chris@12: 	    m_toCompensate[i] -= compensating;
Chris@12: 	    if (m_toCompensate[i] < 0) {
Chris@12: 		throw logic_error("Compensated for more latency than exists");
Chris@12: 	    }
Chris@12: 	}
Chris@12: 	
Chris@12: 	m_filtered[i].insert(m_filtered[i].end(),
Chris@12: 			     filtered.begin() + pushStart,
Chris@12: 			     filtered.begin() + pushStart + pushCount);
Chris@10:     }	
Chris@10: 
Chris@10:     double calculateEnergy(const RealSequence &seq, int n, double factor) {
Chris@10: 	double energy = 0.0;
Chris@12: 	int sz = seq.size();
Chris@12: 	if (n > sz) n = sz;
Chris@10: 	for (int i = 0; i < n; ++i) {
Chris@11: 	    energy += seq[i] * seq[i];
Chris@10: 	}
Chris@11: 	return energy * factor;
Chris@10:     }
Chris@10:     
Chris@5: private:
Chris@11:     int m_nfilters;
Chris@5:     int m_sampleRate;
Chris@29:     int m_effectiveInputRate;
Chris@27:     double m_tuningFrequency;
Chris@5: 
Chris@5:     // This vector is initialised with 88 filter instances.
Chris@5:     // m_filters[n] (for n from 0 to 87) is for MIDI pitch 21+n, so we
Chris@5:     // span the MIDI range from pitch 21 to 108. Then m_filters[n] has
Chris@5:     // coefficients drawn from filter_a[20+n] and filter_b[20+n], and
Chris@5:     // has effective delay filter_delay[20+n].
Chris@10:     vector<Filter *> m_filters;
Chris@5: 
Chris@11:     map<int, Resampler *> m_resamplers; // rate -> resampler
Chris@11:     map<int, RealSequence> m_resampled;
Chris@12:     vector<int> m_toCompensate; // latency remaining at start, per filter
Chris@11:     vector<RealSequence> m_filtered;
Chris@11:     vector<deque<double>> m_energies;
Chris@28:     int m_blockNo;
Chris@16: 
Chris@11:     Resampler *resamplerFor(int filterIndex) {
Chris@11: 	int rate = filterRate(filterIndex);
Chris@11: 	if (m_resamplers.find(rate) == m_resamplers.end()) {
Chris@11: 	    throw std::logic_error("Filter index has unknown or unexpected rate");
Chris@11: 	}
Chris@11: 	return m_resamplers[rate];
Chris@11:     }
Chris@11: 
Chris@11:     int filterRate(int filterIndex) const {
Chris@11: 	if (filterIndex <= HIGHEST_FILTER_INDEX_AT_882) {
Chris@11: 	    return 882;
Chris@11: 	} else if (filterIndex <= HIGHEST_FILTER_INDEX_AT_4410) {
Chris@11: 	    return 4410;
Chris@11: 	} else {
Chris@11: 	    return 22050;
Chris@11: 	}
Chris@5:     }
Chris@12:     int resamplerDelay(int filterIndex) const {
Chris@12: 	return const_cast<D *>(this)->resamplerFor(filterIndex)->getLatency();
Chris@12:     }
Chris@6:     int filterDelay(int filterIndex) const {
Chris@5: 	return filter_delay[20 + filterIndex];
Chris@5:     }
Chris@6:     int totalDelay(int filterIndex) const {
Chris@12: 	return resamplerDelay(filterIndex) + filterDelay(filterIndex);
Chris@5:     }
Chris@5: };
Chris@5: 
Chris@27: PitchFilterbank::PitchFilterbank(int sampleRate, double tuningFrequency) :
Chris@15:     m_d(new D(sampleRate, tuningFrequency))
Chris@9: {
Chris@9: }
Chris@9: 
Chris@9: PitchFilterbank::~PitchFilterbank()
Chris@9: {
Chris@9:     delete m_d;
Chris@9: }
Chris@9: 
Chris@9: void
Chris@9: PitchFilterbank::reset()
Chris@9: {
Chris@9:     int rate = m_d->getSampleRate();
Chris@27:     double freq = m_d->getTuningFrequency();
Chris@9:     delete m_d;
Chris@15:     m_d = new D(rate, freq);
Chris@9: }
Chris@9: 
Chris@19: RealBlock
Chris@9: PitchFilterbank::process(const RealSequence &in)
Chris@9: {
Chris@9:     return m_d->process(in);
Chris@9: }
Chris@9: 
Chris@19: RealBlock
Chris@9: PitchFilterbank::getRemainingOutput()
Chris@9: {
Chris@9:     return m_d->getRemainingOutput();
Chris@9: }
Chris@9: 
Chris@27: void
Chris@27: PitchFilterbank::getPitchRange(int &minMidiPitch, int &maxMidiPitch)
Chris@27: {
Chris@27:     minMidiPitch = 21;
Chris@27:     maxMidiPitch = 108;
Chris@27: }
Chris@27: 
Chris@32: double
Chris@32: PitchFilterbank::getOutputSampleRate()
Chris@32: {
Chris@32:     return 22050.0 / 2205.0;
Chris@32: }
Chris@27: