andrewm@0: /*
andrewm@0:   TouchKeys: multi-touch musical keyboard control software
andrewm@0:   Copyright (c) 2013 Andrew McPherson
andrewm@0: 
andrewm@0:   This program is free software: you can redistribute it and/or modify
andrewm@0:   it under the terms of the GNU General Public License as published by
andrewm@0:   the Free Software Foundation, either version 3 of the License, or
andrewm@0:   (at your option) any later version.
andrewm@0:  
andrewm@0:   This program is distributed in the hope that it will be useful,
andrewm@0:   but WITHOUT ANY WARRANTY; without even the implied warranty of
andrewm@0:   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
andrewm@0:   GNU General Public License for more details.
andrewm@0: 
andrewm@0:   You should have received a copy of the GNU General Public License
andrewm@0:   along with this program.  If not, see <http://www.gnu.org/licenses/>.
andrewm@0:  
andrewm@0:   =====================================================================
andrewm@0: 
andrewm@0:   IIRFilter.h: template class handling an Nth-order IIR filter on data
andrewm@0:   in a given Node.
andrewm@0: */
andrewm@0: 
andrewm@0: #ifndef touchkeys_IIRFilter_h
andrewm@0: #define touchkeys_IIRFilter_h
andrewm@0: 
andrewm@0: 
andrewm@0: #include <iostream>
andrewm@0: #include <exception>
andrewm@0: #include <vector>
andrewm@0: #include "Node.h"
andrewm@0: 
andrewm@0: /*
andrewm@0:  * IIRFilterNode
andrewm@0:  *
andrewm@0:  * This template class performs IIR (infinite impulse response) filtering on incoming Node data.
andrewm@0:  * It does not take into account timestamps so assumes the data is regularly sampled. The filter
andrewm@0:  * coefficients can be specified and changed, and the operation is selectable between always
andrewm@0:  * filtering on each new sample or only filtering on request. In the latter case, it will go back
andrewm@0:  * and filter from the most recent available sample, assuming the signal starts from 0 if there is
andrewm@0:  * any break in data between what was already calculated and what input data is now available.
andrewm@0:  */
andrewm@0: 
andrewm@0: template<typename DataType>
andrewm@0: class IIRFilterNode : public Node<DataType> {
andrewm@0: public:
andrewm@0: 	typedef typename Node<DataType>::capacity_type capacity_type;
andrewm@0: 	//typedef typename Node<return_type>::size_type size_type;
andrewm@0: 	
andrewm@0: 	// ***** Constructors *****
andrewm@0:     
andrewm@0: 	IIRFilterNode(capacity_type capacity, Node<DataType>& input) : Node<DataType>(capacity), input_(input),
andrewm@0:       autoCalculate_(false), inputHistory_(0), outputHistory_(0), lastInputIndex_(0) {
andrewm@0: 	}
andrewm@0:     
andrewm@0: 	// Copy constructor
andrewm@0: 	IIRFilterNode(IIRFilterNode<DataType> const& obj) : Node<DataType>(obj), input_(obj.input_), autoCalculate_(obj.autoCalculate_),
andrewm@0:      aCoefficients_(obj.aCoefficients_), bCoefficients_(obj.bCoefficients_), lastInputIndex_(obj.lastInputIndex_) {
andrewm@0:          if(obj.inputHistory_ != 0)
andrewm@0:              inputHistory_ = new boost::circular_buffer<DataType>(*obj.inputHistory_);
andrewm@0:          else
andrewm@0:              inputHistory_ = 0;
andrewm@0:          if(obj.outputHistory_ != 0)
andrewm@0:              outputHistory_ = new boost::circular_buffer<DataType>(*obj.outputHistory_);
andrewm@0:          else
andrewm@0:              outputHistory_ = 0;
andrewm@0:          if(autoCalculate_) {
andrewm@0:              // Bring up to date and register for further updates
andrewm@0:              calculate();
andrewm@0:              this->registerForTrigger(&input_);
andrewm@0:          }
andrewm@0: 	}
andrewm@0:     
andrewm@0:     // ***** Destructor *****
andrewm@0:     ~IIRFilterNode() {
andrewm@0:         if(inputHistory_ != 0)
andrewm@0:             delete inputHistory_;
andrewm@0:         if(outputHistory_ != 0)
andrewm@0:             delete outputHistory_;
andrewm@0:     }
andrewm@0: 	
andrewm@0: 	// ***** Modifiers *****
andrewm@0: 	//
andrewm@0: 	// Override this method to clear the input sample buffer
andrewm@0: 	
andrewm@0: 	void clear() {
andrewm@0: 		Node<DataType>::clear();
andrewm@0:         clearInputOutputHistory();
andrewm@0: 	}
andrewm@0:     
andrewm@0:     // Switch whether calculations happen automatically or only upon request
andrewm@0:     // If switching on, optionally specify how far back to calculate in bringing
andrewm@0:     // the filter up to date.
andrewm@0:     void setAutoCalculate(bool newAutoCalculate, int maximumLookback = -1) {
andrewm@0:         if(autoCalculate_ && !newAutoCalculate)
andrewm@0:             this->unregisterForTrigger(&input_);
andrewm@0:         else if(!autoCalculate_ && newAutoCalculate) {
andrewm@0:             // Bring the buffer up to date
andrewm@0:             calculate(maximumLookback);
andrewm@0:             this->registerForTrigger(&input_);
andrewm@0:         }
andrewm@0:         autoCalculate_ = newAutoCalculate;
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Set the coefficients of the filter and allocate the proper buffer
andrewm@0:     // to hold past inputs. Optional last argument specifies whether to
andrewm@0:     // clear the past sample history or not (defaults to clearing it).
andrewm@0:     // If filter lengths are different, the buffer is always cleared.
andrewm@0:     void setCoefficients(std::vector<DataType> const& bCoeffs,
andrewm@0:                          std::vector<DataType> const& aCoeffs,
andrewm@0:                          bool clearBuffer = true) {
andrewm@0:         if(bCoeffs.empty()) // Can't have an empty feedforward coefficient set
andrewm@0:             return;
andrewm@0:         bool shouldClear = clearBuffer;
andrewm@0:         aCoefficients_ = aCoeffs;
andrewm@0:         bCoefficients_ = bCoeffs;
andrewm@0:         
andrewm@0:         if(inputHistory_ == 0) {
andrewm@0:             inputHistory_ = new boost::circular_buffer<DataType>(bCoeffs.size());
andrewm@0:             shouldClear = true;
andrewm@0:         }
andrewm@0:         else if(bCoeffs.size() != inputHistory_->capacity()) {
andrewm@0:             inputHistory_->set_capacity(bCoeffs.size());
andrewm@0:             shouldClear = true;
andrewm@0:         }
andrewm@0:         
andrewm@0:         if(outputHistory_ == 0) {
andrewm@0:             outputHistory_ = new boost::circular_buffer<DataType>(aCoeffs.size());
andrewm@0:             shouldClear = true;
andrewm@0:         }
andrewm@0:         else if(aCoeffs.size() != outputHistory_->capacity()) {
andrewm@0:             outputHistory_->set_capacity(aCoeffs.size());
andrewm@0:             shouldClear = true;
andrewm@0:         }
andrewm@0:         
andrewm@0:         if(shouldClear)
andrewm@0:             clearInputOutputHistory();
andrewm@0:     }
andrewm@0:     
andrewm@0:     // If not automatically calculating, bring the samples up to date by
andrewm@0:     // processing any available input that we have not yet seen. Returns
andrewm@0:     // the most recent output. Optional argument specifies how far back
andrewm@0:     // to look before starting fresh (if more samples have elapsed since
andrewm@0:     // last calculation).
andrewm@0:     typename Node<DataType>::return_value_type calculate(int maximumLookback = -1) {
andrewm@0:         typename Node<DataType>::size_type index = lastInputIndex_;
andrewm@0:         
andrewm@0:         if(maximumLookback >= 0 && index < input_.endIndex() - 1 - maximumLookback) {
andrewm@0:             //std::cout << "IIRFilterNode: clearing history at index " << index << std::endl;
andrewm@0:             // More samples gone by than we want to calculate... clear input
andrewm@0:             clearInputOutputHistory();
andrewm@0:             index = input_.endIndex() - 1 - maximumLookback;
andrewm@0:             if(index < input_.beginIndex())
andrewm@0:                 index = input_.beginIndex();
andrewm@0:         }
andrewm@0:         else if(index < input_.beginIndex()) {
andrewm@0:             // More samples gone by than are now available... clear input
andrewm@0:             //std::cout << "IIRFilterNode: clearing history at index " << index << std::endl;
andrewm@0:             clearInputOutputHistory();
andrewm@0:             index = input_.beginIndex();
andrewm@0:         }
andrewm@0:         while(index < input_.endIndex()) {
andrewm@0:             processOneSample(input_[index], input_.timestampAt(index));
andrewm@0:             index++;
andrewm@0:         }
andrewm@0:         
andrewm@0:         lastInputIndex_ = index;
andrewm@0:         if(!this->empty())
andrewm@0:             return this->latest();
andrewm@0:         //std::cout << "IIRFilterNode: empty\n";
andrewm@0:         return missing_value<DataType>::missing();
andrewm@0:     }
andrewm@0: 
andrewm@0: 	// ***** Evaluator *****
andrewm@0: 	//
andrewm@0: 	// This is called when the input gets a new data point.  Accumulate its value and store it in our buffer.
andrewm@0: 	// Storing it will also cause a trigger to be sent to anyone who's listening.
andrewm@0: 	
andrewm@0: 	void triggerReceived(TriggerSource* who, timestamp_type timestamp) {
andrewm@0: 		if(who != &input_ || !autoCalculate_)
andrewm@0: 			return;
andrewm@0:         
andrewm@0:         processOneSample(input_.latest(), timestamp);
andrewm@0: 	}
andrewm@0: 	
andrewm@0: private:
andrewm@0:     // ***** Internal Methods *****
andrewm@0:     // Run the filter once with a new sample. Put the result into the
andrewm@0:     // end of the buffer.
andrewm@0:     void processOneSample(DataType const& sample, timestamp_type timestamp) {
andrewm@0:         if(!bCoefficients_.empty()) {
andrewm@0:             // Always need at least one feedforward coefficient
andrewm@0:             DataType result = bCoefficients_[0] * sample;
andrewm@0:             typename boost::circular_buffer<DataType>::reverse_iterator rit = inputHistory_->rbegin();
andrewm@0:             
andrewm@0:             // Feedforward part
andrewm@0:             for(int i = 1; i < bCoefficients_.size() && rit != inputHistory_->rend(); i++) {
andrewm@0:                 result += *rit * bCoefficients_[i];
andrewm@0:                 rit++;
andrewm@0:             }
andrewm@0:             // Feedback part
andrewm@0:             rit = outputHistory_->rbegin();
andrewm@0:             for(int i = 0; i < aCoefficients_.size() && rit != outputHistory_->rend(); i++) {
andrewm@0:                 result -= *rit * aCoefficients_[i];
andrewm@0:                 rit++;
andrewm@0:             }
andrewm@0:             
andrewm@0:             // Update input history and put output in our buffer
andrewm@0:             inputHistory_->push_back(sample);
andrewm@0:             outputHistory_->push_back(result);
andrewm@0:             this->insert(result, timestamp);
andrewm@0:         }
andrewm@0:         else {
andrewm@0:             // Pass through when no coefficients present
andrewm@0:             this->insert(sample, timestamp);
andrewm@0:         }
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Clear the recent history of input/output data and fill it with zeros
andrewm@0:     void clearInputOutputHistory() {
andrewm@0:         if(inputHistory_ != 0) {
andrewm@0:             inputHistory_->clear();
andrewm@0:             while(!inputHistory_->full())
andrewm@0:                 inputHistory_->push_back(DataType());
andrewm@0:         }
andrewm@0:         if(outputHistory_ != 0) {
andrewm@0:             outputHistory_->clear();
andrewm@0:             while(!outputHistory_->full())
andrewm@0:                 outputHistory_->push_back(DataType());
andrewm@0:         }
andrewm@0:     }
andrewm@0:     
andrewm@0:     
andrewm@0:     // ***** Member Variables *****
andrewm@0:     
andrewm@0: 	Node<DataType>& input_;
andrewm@0: 	bool autoCalculate_;        // Whether we're automatically calculating new output values
andrewm@0: 
andrewm@0:     // Variables below are for filter calculation. We need to hold the past input samples
andrewm@0:     // ourselves because we can't consistently count on enough samples in the source buffer.
andrewm@0:     // Likewise, we need to hold past output samples, even though we have our own buffer, because
andrewm@0:     // when we clear the buffer for new calculations we don't want to lose what we've previously
andrewm@0:     // calculated.
andrewm@0:     boost::circular_buffer<DataType>* inputHistory_;
andrewm@0:     boost::circular_buffer<DataType>* outputHistory_;
andrewm@0:     std::vector<DataType> aCoefficients_, bCoefficients_;
andrewm@0:     typename Node<DataType>::size_type lastInputIndex_;              // Where in the input buffer we had the last sample
andrewm@0: };
andrewm@0: 
andrewm@0: // ***** Static Filter Design Methods *****
andrewm@0: 
andrewm@0: // These methods calculate specific coefficients and store them in the provided
andrewm@0: // A and B vectors. These will only work for double/float datatypes or others that
andrewm@0: // can be multiplied with them. Details: http://freeverb3.sourceforge.net/iir_filter.shtml
andrewm@0: 
andrewm@0: void designFirstOrderLowpass(std::vector<double>& bCoeffs, std::vector<double>& aCoeffs,
andrewm@0:                                     double cutoffFrequency, double sampleFrequency);
andrewm@0: 
andrewm@0: void designFirstOrderHighpass(std::vector<double>& bCoeffs, std::vector<double>& aCoeffs,
andrewm@0:                               double cutoffFrequency, double sampleFrequency);
andrewm@0: 
andrewm@0: void designSecondOrderLowpass(std::vector<double>& bCoeffs, std::vector<double>& aCoeffs,
andrewm@0:                               double cutoffFrequency, double q, double sampleFrequency);
andrewm@0: 
andrewm@0: void designSecondOrderHighpass(std::vector<double>& bCoeffs, std::vector<double>& aCoeffs,
andrewm@0:                                double cutoffFrequency, double q, double sampleFrequency);
andrewm@0: 
andrewm@0: void designSecondOrderBandpass(std::vector<double>& bCoeffs, std::vector<double>& aCoeffs,
andrewm@0:                                double cutoffFrequency, double q, double sampleFrequency);
andrewm@0: 
andrewm@0: #endif