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:   MRPMapping.cpp: mapping class for magnetic resonator piano using continuous
andrewm@0:   key position.
andrewm@0: */
andrewm@0: 
andrewm@0: #include "MRPMapping.h"
andrewm@0: #include <vector>
andrewm@0: 
andrewm@0: // Class constants
andrewm@0: // Useful constants for mapping MRP messages
andrewm@0: const int MRPMapping::kMIDINoteOnMessage = 0x90;
andrewm@0: const int MRPMapping::kDefaultMIDIChannel = 15;
andrewm@0: const float MRPMapping::kDefaultAftertouchScaler = 100.0;
andrewm@0: 
andrewm@0: // Parameters for vibrato detection and mapping
andrewm@0: const key_velocity MRPMapping::kVibratoVelocityThreshold = scale_key_velocity(2.0);
andrewm@0: const timestamp_diff_type MRPMapping::kVibratoMinimumPeakSpacing = microseconds_to_timestamp(60000);
andrewm@0: const timestamp_diff_type MRPMapping::kVibratoTimeout = microseconds_to_timestamp(500000);
andrewm@0: const int MRPMapping::kVibratoMinimumOscillations = 4;
andrewm@0: const float MRPMapping::kVibratoRateScaler = 0.005;
andrewm@0: 
andrewm@0: // Main constructor takes references/pointers from objects which keep track
andrewm@0: // of touch location, continuous key position and the state detected from that
andrewm@0: // position. The PianoKeyboard object is strictly required as it gives access to
andrewm@0: // Scheduler and OSC methods. The others are optional since any given system may
andrewm@0: // contain only one of continuous key position or touch sensitivity
andrewm@0: MRPMapping::MRPMapping(PianoKeyboard &keyboard, MappingFactory *factory, int noteNumber, Node<KeyTouchFrame>* touchBuffer,
andrewm@0:                      Node<key_position>* positionBuffer, KeyPositionTracker* positionTracker)
andrewm@0: : Mapping(keyboard, factory, noteNumber, touchBuffer, positionBuffer, positionTracker),
andrewm@0:   noteIsOn_(false), lastIntensity_(missing_value<float>::missing()),
andrewm@0:   lastBrightness_(missing_value<float>::missing()), lastPitch_(missing_value<float>::missing()),
andrewm@0:   lastHarmonic_(missing_value<float>::missing()),
andrewm@0:   shouldLookForPitchBends_(true), rawVelocity_(kMRPMappingVelocityBufferLength),
andrewm@0:   filteredVelocity_(kMRPMappingVelocityBufferLength, rawVelocity_), lastCalculatedVelocityIndex_(0),
andrewm@0:   vibratoActive_(false), vibratoVelocityPeakCount_(0), vibratoLastPeakTimestamp_(missing_value<timestamp_type>::missing())
andrewm@0: {
andrewm@0:     setAftertouchSensitivity(1.0);
andrewm@0:     
andrewm@0:     // Initialize the filter coefficients for filtered key velocity (used for vibrato detection)
andrewm@0:     std::vector<double> bCoeffs, aCoeffs;
andrewm@0:     designSecondOrderLowpass(bCoeffs, aCoeffs, 15.0, 0.707, 1000.0);
andrewm@0:     std::vector<float> bCf(bCoeffs.begin(), bCoeffs.end()), aCf(aCoeffs.begin(), aCoeffs.end());
andrewm@0:     filteredVelocity_.setCoefficients(bCf, aCf);
andrewm@0: }
andrewm@0: 
andrewm@0: // Copy constructor
andrewm@0: /*MRPMapping::MRPMapping(MRPMapping const& obj)
andrewm@0: : Mapping(obj), lastIntensity_(obj.lastIntensity_), lastBrightness_(obj.lastBrightness_),
andrewm@0: aftertouchScaler_(obj.aftertouchScaler_), noteIsOn_(obj.noteIsOn_), lastPitch_(obj.lastPitch_),
andrewm@0: lastHarmonic_(obj.lastHarmonic_),
andrewm@0: shouldLookForPitchBends_(obj.shouldLookForPitchBends_), activePitchBends_(obj.activePitchBends_),
andrewm@0: rawVelocity_(obj.rawVelocity_), filteredVelocity_(obj.filteredVelocity_),
andrewm@0: lastCalculatedVelocityIndex_(obj.lastCalculatedVelocityIndex_), vibratoActive_(obj.vibratoActive_),
andrewm@0: vibratoVelocityPeakCount_(obj.vibratoVelocityPeakCount_), vibratoLastPeakTimestamp_(obj.vibratoLastPeakTimestamp_) {
andrewm@0: 
andrewm@0: }*/
andrewm@0: 
andrewm@0: MRPMapping::~MRPMapping() {
andrewm@0:     //std::cerr << "~MRPMapping(): " << this << std::endl;
andrewm@0:     
andrewm@0:     try {
andrewm@0:         disengage();
andrewm@0:     }
andrewm@0:     catch(...) {
andrewm@0:         std::cerr << "~MRPMapping(): exception during disengage()\n";
andrewm@0:     }
andrewm@0:     
andrewm@0:     //std::cerr << "~MRPMapping(): done\n";
andrewm@0: }
andrewm@0: 
andrewm@0: // Turn off mapping of data. Remove our callback from the scheduler
andrewm@0: void MRPMapping::disengage() {
andrewm@0:     Mapping::disengage();
andrewm@0:     if(noteIsOn_) {
andrewm@0:         int newNoteNumber = noteNumber_;
andrewm@0:         //int newNoteNumber = ((noteNumber_ - 21) * 25)%88 + 21;
andrewm@0:         keyboard_.sendMessage("/mrp/midi",
andrewm@0:                               "iii", (int)(kMIDINoteOnMessage + kDefaultMIDIChannel), (int)newNoteNumber, (int)0, LO_ARGS_END);
andrewm@0:        // if(!touchBuffer_->empty())
andrewm@0:        //     keyboard_.testLog_ << touchBuffer_->latestTimestamp() << " /mrp/midi iii " << (kMIDINoteOnMessage + kDefaultMIDIChannel) << " " << newNoteNumber << " " << 0 << endl;
andrewm@0:         
andrewm@0:         // Reset qualities
andrewm@0:         lastPitch_ = lastHarmonic_ = lastBrightness_ = lastIntensity_ = missing_value<float>::missing();
andrewm@0:     }
andrewm@0:     noteIsOn_ = false;
andrewm@0:     shouldLookForPitchBends_ = true;
andrewm@0: }
andrewm@0: 
andrewm@0: // Reset state back to defaults
andrewm@0: void MRPMapping::reset() {
andrewm@0:     Mapping::reset();
andrewm@0:     noteIsOn_ = false;
andrewm@0:     shouldLookForPitchBends_ = true;
andrewm@0: }
andrewm@0: 
andrewm@0: // Set the aftertouch sensitivity on continuous key position
andrewm@0: // 0 means no aftertouch, 1 means default sensitivity, upward
andrewm@0: // from there
andrewm@0: void MRPMapping::setAftertouchSensitivity(float sensitivity) {
andrewm@0:     if(sensitivity <= 0)
andrewm@0:         aftertouchScaler_ = 0;
andrewm@0:     else
andrewm@0:         aftertouchScaler_ = kDefaultAftertouchScaler * sensitivity;
andrewm@0: }
andrewm@0: 
andrewm@0: // This is called by another MRPMapping when it finds a pitch bend starting.
andrewm@0: // Add the sending note to our list of bends, with the sending note marked
andrewm@0: // as controlling the bend
andrewm@0: void MRPMapping::enablePitchBend(int toNote, Node<key_position>* toPositionBuffer,
andrewm@0:                                 KeyPositionTracker *toPositionTracker) {
andrewm@0:     if(toPositionBuffer == 0 || toPositionTracker == 0)
andrewm@0:         return;
andrewm@0:     
andrewm@0:     std::cout << "enablePitchBend(): this note = " << noteNumber_ << " note = " << toNote << " posBuf = " << toPositionBuffer << " posTrack = " << toPositionTracker << "\n";
andrewm@0:     PitchBend newBend = {toNote, true, false, toPositionBuffer, toPositionTracker};
andrewm@0:     activePitchBends_.push_back(newBend);
andrewm@0: }
andrewm@0: 
andrewm@0: // Trigger method. This receives updates from the TouchKey data or from state changes in
andrewm@0: // the continuous key position (KeyPositionTracker). It will potentially change the scheduled
andrewm@0: // behavior of future mapping calls, but the actual OSC messages should be transmitted in a different
andrewm@0: // thread.
andrewm@0: void MRPMapping::triggerReceived(TriggerSource* who, timestamp_type timestamp) {
andrewm@0:     if(who == 0)
andrewm@0:         return;
andrewm@0:     if(who == positionTracker_) {
andrewm@0:         // The state of the key (based on continuous position) just changed.
andrewm@0:         // Might want to alter our mapping strategy.
andrewm@0:     }
andrewm@0:     else if(who == touchBuffer_) {
andrewm@0:         // TouchKey data is available
andrewm@0:     }
andrewm@0: }
andrewm@0: 
andrewm@0: // Mapping method. This actually does the real work of sending OSC data in response to the
andrewm@0: // latest information from the touch sensors or continuous key angle
andrewm@0: timestamp_type MRPMapping::performMapping() {
andrewm@0:     if(!engaged_)
andrewm@0:         return 0;
andrewm@0:     
andrewm@0:     timestamp_type currentTimestamp = keyboard_.schedulerCurrentTimestamp();
andrewm@0:     float intensity = 0;
andrewm@0:     float brightness = 0;
andrewm@0:     float pitch = 0;
andrewm@0:     float harmonic = 0;
andrewm@0: 
andrewm@0:     // Calculate the output features as a function of input sensor data
andrewm@0:     if(positionBuffer_ == 0) {
andrewm@0:         // No buffer -> all 0
andrewm@0:     }
andrewm@0:     else if(positionBuffer_->empty()) {
andrewm@0:         // No samples -> all 0
andrewm@0:     }
andrewm@0:     else {
andrewm@0:         // TODO: IIR filter on the position data before mapping it
andrewm@0:         key_position latestPosition = positionBuffer_->latest();
andrewm@0:         int trackerState = kPositionTrackerStateUnknown;
andrewm@0:         if(positionTracker_ != 0)
andrewm@0:             trackerState = positionTracker_->currentState();
andrewm@0:         
andrewm@0:         // Get the latest velocity measurements
andrewm@0:         key_velocity latestVelocity = updateVelocityMeasurements();
andrewm@0:         
andrewm@0:         // Every time we enter a state of PartialPress, check whether this key
andrewm@0:         // is part of a multi-key pitch bend gesture with another key that's already
andrewm@0:         // down. Only do this once, though, since keys that go down after we enter
andrewm@0:         // PartialPress state are not part of such a gesture.
andrewm@0:         if(shouldLookForPitchBends_) {
andrewm@0:             if(trackerState == kPositionTrackerStatePartialPressAwaitingMax ||
andrewm@0:                trackerState == kPositionTrackerStatePartialPressFoundMax) {
andrewm@0:                 // Look for a pitch bend gesture by searching for neighboring
andrewm@0:                 // keys which are in the Down state and reached that state before
andrewm@0:                 // this one reached PartialPress state.
andrewm@0:                 for(int neighborNote = noteNumber_ - 2; neighborNote < noteNumber_; neighborNote++) {
andrewm@0:                     // If one of the lower keys is in the Down state, then this note should bend it up
andrewm@0:                     MRPMapping *neighborMapper = dynamic_cast<MRPMapping*>(keyboard_.mapping(neighborNote));
andrewm@0:                     if(neighborMapper == 0)
andrewm@0:                         continue;
andrewm@0:                     if(neighborMapper->positionTracker_ != 0) {
andrewm@0:                         int neighborState = neighborMapper->positionTracker_->currentState();
andrewm@0:                         if(neighborState == kPositionTrackerStateDown) {
andrewm@0:                             // Here we've found a neighboring note in the Down state. But did it precede our transition?
andrewm@0:                             timestamp_type timeOfDownTransition = neighborMapper->positionTracker_->latestTimestamp();
andrewm@0:                             timestamp_type timeOfOurPartialActivation = findTimestampOfPartialPress();
andrewm@0:                             
andrewm@0:                             cout << "Found key " << neighborNote << " in Down state\n";
andrewm@0:                             
andrewm@0:                             if(!missing_value<timestamp_type>::isMissing(timeOfOurPartialActivation)) {
andrewm@0:                                 if(timeOfOurPartialActivation > timeOfDownTransition) {
andrewm@0:                                     // The neighbor note went down before us; pitch bend should engage
andrewm@0:                                     cout << "Found pitch bend: " << noteNumber_ << " to " << neighborNote << endl;
andrewm@0:                                     
andrewm@0:                                     // Insert the details for the neighboring note into our buffer. The bend
andrewm@0:                                     // is controlled by our own key, and the target is the neighbor note.
andrewm@0:                                     PitchBend newBend = {neighborNote, false, false, neighborMapper->positionBuffer_,
andrewm@0:                                         neighborMapper->positionTracker_};
andrewm@0:                                     activePitchBends_.push_back(newBend);
andrewm@0:                                 
andrewm@0:                                     // Tell the other note to bend its pitch based on our position
andrewm@0:                                     neighborMapper->enablePitchBend(noteNumber_, positionBuffer_, positionTracker_);
andrewm@0:                                 }
andrewm@0:                             }
andrewm@0:                         }
andrewm@0:                     }
andrewm@0:                 }
andrewm@0:                 for(int neighborNote = noteNumber_ + 1; neighborNote < noteNumber_ + 3; neighborNote++) {
andrewm@0:                     // If one of the upper keys is in the Down state, then this note should bend it down
andrewm@0:                     MRPMapping *neighborMapper = dynamic_cast<MRPMapping*>(keyboard_.mapping(neighborNote));
andrewm@0:                     if(neighborMapper == 0)
andrewm@0:                         continue;
andrewm@0:                     if(neighborMapper->positionTracker_ != 0) {
andrewm@0:                         int neighborState = neighborMapper->positionTracker_->currentState();
andrewm@0:                         if(neighborState == kPositionTrackerStateDown) {
andrewm@0:                             // Here we've found a neighboring note in the Down state. But did it precede our transition?
andrewm@0:                             timestamp_type timeOfDownTransition = neighborMapper->positionTracker_->latestTimestamp();
andrewm@0:                             timestamp_type timeOfOurPartialActivation = findTimestampOfPartialPress();
andrewm@0:                             
andrewm@0:                             cout << "Found key " << neighborNote << " in Down state\n";
andrewm@0:                             
andrewm@0:                             if(!missing_value<timestamp_type>::isMissing(timeOfOurPartialActivation)) {
andrewm@0:                                 if(timeOfOurPartialActivation > timeOfDownTransition) {
andrewm@0:                                     // The neighbor note went down before us; pitch bend should engage
andrewm@0:                                     cout << "Found pitch bend: " << noteNumber_ << " to " << neighborNote << endl;
andrewm@0:                                     
andrewm@0:                                     // Insert the details for the neighboring note into our buffer. The bend
andrewm@0:                                     // is controlled by our own key, and the target is the neighbor note.
andrewm@0:                                     PitchBend newBend = {neighborNote, false, false, neighborMapper->positionBuffer_,
andrewm@0:                                         neighborMapper->positionTracker_};
andrewm@0:                                     activePitchBends_.push_back(newBend);
andrewm@0:                                     
andrewm@0:                                     // Tell the other note to bend its pitch based on our position
andrewm@0:                                     neighborMapper->enablePitchBend(noteNumber_, positionBuffer_, positionTracker_);
andrewm@0:                                 }
andrewm@0:                             }
andrewm@0:                         }
andrewm@0:                     }
andrewm@0:                 }
andrewm@0:                 
andrewm@0:                 shouldLookForPitchBends_ = false;
andrewm@0:             }
andrewm@0:         }
andrewm@0:         
andrewm@0:         if(trackerState == kPositionTrackerStatePartialPressAwaitingMax ||
andrewm@0:            trackerState == kPositionTrackerStatePartialPressFoundMax) {
andrewm@0:             // Look for active vibrato gestures which are defined as oscillating
andrewm@0:             // motion in the key velocity. They could conceivably occur at a variety
andrewm@0:             // of raw key positions, as long as the key is not yet down
andrewm@0:             
andrewm@0:             if(missing_value<timestamp_type>::isMissing(vibratoLastPeakTimestamp_))
andrewm@0:                 vibratoLastPeakTimestamp_ = currentTimestamp;
andrewm@0:             
andrewm@0:             if(vibratoVelocityPeakCount_ % 2 == 0) {
andrewm@0:                 if(latestVelocity > kVibratoVelocityThreshold && currentTimestamp - vibratoLastPeakTimestamp_ > kVibratoMinimumPeakSpacing) {
andrewm@0:                     std::cout << "Vibrato count = " << vibratoVelocityPeakCount_ << std::endl;
andrewm@0:                     vibratoVelocityPeakCount_++;
andrewm@0:                     vibratoLastPeakTimestamp_ = currentTimestamp;
andrewm@0:                 }
andrewm@0:             }
andrewm@0:             else {
andrewm@0:                 if(latestVelocity < -kVibratoVelocityThreshold && currentTimestamp - vibratoLastPeakTimestamp_ > kVibratoMinimumPeakSpacing) {
andrewm@0:                     std::cout << "Vibrato count = " << vibratoVelocityPeakCount_ << std::endl;
andrewm@0:                     vibratoVelocityPeakCount_++;
andrewm@0:                     vibratoLastPeakTimestamp_ = currentTimestamp;
andrewm@0:                 }
andrewm@0:             }
andrewm@0:             
andrewm@0:             if(vibratoVelocityPeakCount_ >= kVibratoMinimumOscillations) {
andrewm@0:                 vibratoActive_ = true;
andrewm@0:             }
andrewm@0:             
andrewm@0:             
andrewm@0:             if(vibratoActive_) {
andrewm@0:                 // Update the harmonic parameter, which increases linearly with the absolute
andrewm@0:                 // value of velocity. The value will accumulate over the course of a vibrato
andrewm@0:                 // gesture and retain its value when the vibrato finishes. It reverts to minimum
andrewm@0:                 // when the note finishes.
andrewm@0:                 if(missing_value<float>::isMissing(lastHarmonic_))
andrewm@0:                     lastHarmonic_ = 0.0;
andrewm@0:                 harmonic = lastHarmonic_ + fabsf(latestVelocity) * kVibratoRateScaler;
andrewm@0:                 std::cout << "harmonic = " << harmonic << std::endl;
andrewm@0:                 
andrewm@0:                 // Check whether the current vibrato has timed out
andrewm@0:                 if(currentTimestamp - vibratoLastPeakTimestamp_ > kVibratoTimeout) {
andrewm@0:                     std::cout << "Vibrato timed out\n";
andrewm@0:                     vibratoActive_ = false;
andrewm@0:                     vibratoVelocityPeakCount_ = 0;
andrewm@0:                     vibratoLastPeakTimestamp_ = currentTimestamp;
andrewm@0:                 }
andrewm@0:             }
andrewm@0:         }
andrewm@0:         else {
andrewm@0:             // Vibrato can't be active in these states
andrewm@0:             //std::cout << "Vibrato finished from state change\n";
andrewm@0:             vibratoActive_ = false;
andrewm@0:             vibratoVelocityPeakCount_ = 0;
andrewm@0:             vibratoLastPeakTimestamp_ = currentTimestamp;
andrewm@0:         }
andrewm@0:         
andrewm@0:         if(trackerState != kPositionTrackerStateReleaseFinished) {
andrewm@0:             // For all active states except post-release, calculate
andrewm@0:             // Intensity and Brightness parameters based on key position
andrewm@0:             
andrewm@0:             if(latestPosition > 1.0) {
andrewm@0:                 intensity = 1.0;
andrewm@0:                 brightness = (latestPosition - 1.0) * aftertouchScaler_;
andrewm@0:             }
andrewm@0:             else if(latestPosition < 0.0) {
andrewm@0:                 intensity = 0.0;
andrewm@0:                 brightness = 0.0;
andrewm@0:             }
andrewm@0:             else {
andrewm@0:                 intensity = latestPosition;
andrewm@0:                 brightness = 0.0;
andrewm@0:             }
andrewm@0:             
andrewm@0:             if(!activePitchBends_.empty()) {
andrewm@0:                 // Look for active multi-key pitch bend gestures
andrewm@0:                 std::vector<PitchBend>::iterator it = activePitchBends_.begin();
andrewm@0:                 pitch = 0.0;
andrewm@0:                 
andrewm@0:                 for(it = activePitchBends_.begin(); it != activePitchBends_.end(); it++) {
andrewm@0:                     PitchBend& bend(*it);
andrewm@0: 
andrewm@0:                     if(bend.isControllingBend) {
andrewm@0:                         // First find out of the bending key is still in a PartialPress state
andrewm@0:                         // If not, remove it and move on
andrewm@0:                         if((bend.positionTracker->currentState() != kPositionTrackerStatePartialPressAwaitingMax &&
andrewm@0:                            bend.positionTracker->currentState() != kPositionTrackerStatePartialPressFoundMax)
andrewm@0:                            || !bend.positionTracker->engaged()) {
andrewm@0:                             cout << "Removing bend from note " << bend.note << endl;
andrewm@0:                             bend.isFinished = true;
andrewm@0:                             continue;
andrewm@0:                         }
andrewm@0:                         
andrewm@0:                         // This is the case where the other note is controlling our pitch
andrewm@0:                         if(bend.positionBuffer->empty()) {
andrewm@0:                             continue;
andrewm@0:                         }
andrewm@0:                         
andrewm@0:                         float noteDifference = (float)(bend.note - noteNumber_);
andrewm@0:                         key_position latestBenderPosition = bend.positionBuffer->latest();
andrewm@0:                         
andrewm@0:                         // Key position at 0 = 0 pitch bend; key position at max = most pitch bend
andrewm@0:                         float bendAmount = key_position_to_float(latestBenderPosition - kPianoKeyDefaultIdlePositionThreshold*2) /
andrewm@0:                                                 key_position_to_float(1.0 - kPianoKeyDefaultIdlePositionThreshold*2);
andrewm@0:                         if(bendAmount < 0)
andrewm@0:                             bendAmount = 0;
andrewm@0:                         pitch += noteDifference * bendAmount;
andrewm@0:                     }
andrewm@0:                     else {
andrewm@0:                         // This is the case where we're controlling the other note's pitch. Our own
andrewm@0:                         // pitch is the inverse of what we're sending to the neighboring note.
andrewm@0:                         // Compared to the above case, we know a few things since we're using our own
andrewm@0:                         // position: the buffer isn't empty and the tracker is engaged.
andrewm@0:                         
andrewm@0:                         if(trackerState != kPositionTrackerStatePartialPressAwaitingMax &&
andrewm@0:                            trackerState != kPositionTrackerStatePartialPressFoundMax) {
andrewm@0:                             cout << "Removing our bend on note " << bend.note << endl;
andrewm@0:                             bend.isFinished = true;
andrewm@0:                             continue;
andrewm@0:                         }
andrewm@0:   
andrewm@0:                         float noteDifference = (float)(bend.note - noteNumber_);
andrewm@0:                         
andrewm@0:                         // Key position at 0 = 0 pitch bend; key position at max = most pitch bend
andrewm@0:                         float bendAmount = key_position_to_float(latestPosition - kPianoKeyDefaultIdlePositionThreshold*2) /
andrewm@0:                                             key_position_to_float(1.0 - kPianoKeyDefaultIdlePositionThreshold*2);
andrewm@0:                         if(bendAmount < 0)
andrewm@0:                             bendAmount = 0;
andrewm@0:                         pitch += noteDifference * (1.0 - bendAmount);
andrewm@0:                     }
andrewm@0:                 }
andrewm@0:                 
andrewm@0:                 // Now reiterate to remove any of them that have finished
andrewm@0:                 it = activePitchBends_.begin();
andrewm@0:                 
andrewm@0:                 while(it != activePitchBends_.end()) {
andrewm@0:                     if(it->isFinished) {
andrewm@0:                         // Go back to beginning and look again after erasing each one
andrewm@0:                         // This isn't very efficient but there will never be more than 4 elements anyway
andrewm@0:                         activePitchBends_.erase(it);
andrewm@0:                         it = activePitchBends_.begin();
andrewm@0:                     }
andrewm@0:                     else
andrewm@0:                         it++;
andrewm@0:                 }
andrewm@0:                 
andrewm@0:                 std::cout << "pitch = " << pitch << std::endl;
andrewm@0:             }
andrewm@0:             else
andrewm@0:                 pitch = 0.0;
andrewm@0:         }
andrewm@0:         else {
andrewm@0:             intensity = 0.0;
andrewm@0:             brightness = 0.0;
andrewm@0:             if(noteIsOn_) {
andrewm@0:                         int newNoteNumber = noteNumber_;
andrewm@0:                 //int newNoteNumber = ((noteNumber_ - 21) * 25)%88 + 21;
andrewm@0:                 keyboard_.sendMessage("/mrp/midi",
andrewm@0:                                       "iii", (int)(kMIDINoteOnMessage + kDefaultMIDIChannel), (int)newNoteNumber, (int)0, LO_ARGS_END);
andrewm@0:                 //keyboard_.testLog_ << currentTimestamp << " /mrp/midi iii " << (kMIDINoteOnMessage + kDefaultMIDIChannel) << " " << newNoteNumber << " " << 0 << endl;
andrewm@0:             }
andrewm@0:             noteIsOn_ = false;
andrewm@0:             shouldLookForPitchBends_ = true;
andrewm@0:         }
andrewm@0:     }
andrewm@0:     
andrewm@0:     // TODO: TouchKeys mapping
andrewm@0:     
andrewm@0:     // Send OSC message with these parameters unless they are unchanged from before
andrewm@0:     if(!noteIsOn_ && intensity > 0.0) {
andrewm@0:                 int newNoteNumber = noteNumber_;
andrewm@0:         //int newNoteNumber = ((noteNumber_ - 21) * 25)%88 + 21;
andrewm@0:         keyboard_.sendMessage("/mrp/midi",
andrewm@0:                               "iii", (int)(kMIDINoteOnMessage + kDefaultMIDIChannel), (int)newNoteNumber, (int)127, LO_ARGS_END);
andrewm@0:         //keyboard_.testLog_ << currentTimestamp << " /mrp/midi iii " << (kMIDINoteOnMessage + kDefaultMIDIChannel) << " " << newNoteNumber << " " << 127 << endl;
andrewm@0:         noteIsOn_ = true;
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Set key LED color according to key parameters
andrewm@0:     // Partial press --> green of varying intensity
andrewm@0:     // Aftertouch (brightness) --> green moving to red depending on brightness parameter
andrewm@0:     // Pitch bend --> note bends toward blue as pitch value departs from center
andrewm@0:     // Harmonic glissando --> cycle through hues with whitish tint (lower saturation)
andrewm@0:     if(intensity != lastIntensity_ || brightness != lastBrightness_ || pitch != lastPitch_ || harmonic != lastHarmonic_) {
andrewm@0:         if(harmonic != 0.0) {
andrewm@0:             float hue = fmodf(harmonic, 1.0);
andrewm@0:             keyboard_.setKeyLEDColorHSV(noteNumber_, hue, 0.25, 0.5);
andrewm@0:         }
andrewm@0:         else if(intensity >= 1.0) {
andrewm@0:             if(pitch != 0.0)
andrewm@0:                 keyboard_.setKeyLEDColorHSV(noteNumber_, 0.33 + 0.33 * fabsf(pitch) - (brightness * 0.2), 1.0, intensity);
andrewm@0:             else
andrewm@0:                 keyboard_.setKeyLEDColorHSV(noteNumber_, 0.33 - (brightness * 0.2), 1.0, 1.0);
andrewm@0:         }
andrewm@0:         else {
andrewm@0:             if(pitch != 0.0)
andrewm@0:                 keyboard_.setKeyLEDColorHSV(noteNumber_, 0.33 + 0.33 * fabsf(pitch), 1.0, intensity);
andrewm@0:             else
andrewm@0:                 keyboard_.setKeyLEDColorHSV(noteNumber_, 0.33, 1.0, intensity);
andrewm@0:         }
andrewm@0:     }
andrewm@0:         
andrewm@0:     if(intensity != lastIntensity_) {
andrewm@0:                 int newNoteNumber = noteNumber_;
andrewm@0:         //int newNoteNumber = ((noteNumber_ - 21) * 25)%88 + 21;
andrewm@0:         keyboard_.sendMessage("/mrp/quality/intensity",
andrewm@0:                               "iif", (int)kDefaultMIDIChannel, (int)newNoteNumber, (float)intensity, LO_ARGS_END);
andrewm@0:         //keyboard_.testLog_ << currentTimestamp << " /mrp/quality/intensity iif " << kDefaultMIDIChannel << " " << newNoteNumber << " " << intensity << endl;
andrewm@0:     }
andrewm@0:     if(brightness != lastBrightness_) {
andrewm@0:                 int newNoteNumber = noteNumber_;
andrewm@0:         //int newNoteNumber = ((noteNumber_ - 21) * 25)%88 + 21;
andrewm@0:         keyboard_.sendMessage("/mrp/quality/brightness",
andrewm@0:                               "iif", (int)kDefaultMIDIChannel, (int)newNoteNumber, (float)brightness, LO_ARGS_END);
andrewm@0:         //keyboard_.testLog_ << currentTimestamp << " /mrp/quality/brightness iif " << kDefaultMIDIChannel << " " << newNoteNumber << " " << brightness << endl;
andrewm@0:     }
andrewm@0:     if(pitch != lastPitch_) {
andrewm@0:                 int newNoteNumber = noteNumber_;
andrewm@0:         //int newNoteNumber = ((noteNumber_ - 21) * 25)%88 + 21;
andrewm@0:         keyboard_.sendMessage("/mrp/quality/pitch",
andrewm@0:                               "iif", (int)kDefaultMIDIChannel, (int)newNoteNumber, (float)pitch, LO_ARGS_END);
andrewm@0:         //keyboard_.testLog_ << currentTimestamp << " /mrp/quality/pitch iif " << kDefaultMIDIChannel << " " << newNoteNumber << " " << pitch << endl;
andrewm@0:     }
andrewm@0:     if(harmonic != lastHarmonic_) {
andrewm@0:         int newNoteNumber = noteNumber_;
andrewm@0:         //int newNoteNumber = ((noteNumber_ - 21) * 25)%88 + 21;
andrewm@0:         keyboard_.sendMessage("/mrp/quality/harmonic",
andrewm@0:                               "iif", (int)kDefaultMIDIChannel, (int)newNoteNumber, (float)harmonic, LO_ARGS_END);
andrewm@0:         //keyboard_.testLog_ << currentTimestamp << " /mrp/quality/harmonic iif " << kDefaultMIDIChannel << " " << newNoteNumber << " " << harmonic << endl;
andrewm@0:     }
andrewm@0:     
andrewm@0:     lastIntensity_ = intensity;
andrewm@0:     lastBrightness_ = brightness;
andrewm@0:     lastPitch_ = pitch;
andrewm@0:     lastHarmonic_ = harmonic;
andrewm@0:     
andrewm@0:     // Register for the next update by returning its timestamp
andrewm@0:     nextScheduledTimestamp_ = currentTimestamp + updateInterval_;
andrewm@0:     return nextScheduledTimestamp_;
andrewm@0: }
andrewm@0: 
andrewm@0: // Helper function that brings the velocity buffer up to date with the latest
andrewm@0: // samples. Velocity is not updated on every new position sample since it's not
andrewm@0: // efficient to run that many triggers all the time. Instead, it's brought up to
andrewm@0: // date on an as-needed basis during performMapping().
andrewm@0: key_velocity MRPMapping::updateVelocityMeasurements() {
andrewm@0:     positionBuffer_->lock_mutex();
andrewm@0:     
andrewm@0:     // Need at least 2 samples to calculate velocity (first difference)
andrewm@0:     if(positionBuffer_->size() < 2) {
andrewm@0:         positionBuffer_->unlock_mutex();
andrewm@0:         return missing_value<key_velocity>::missing();
andrewm@0:     }
andrewm@0:     
andrewm@0:     if(lastCalculatedVelocityIndex_ < positionBuffer_->beginIndex() + 1) {
andrewm@0:         // Fell off the beginning of the position buffer. Reset calculations.
andrewm@0:         filteredVelocity_.clear();
andrewm@0:         rawVelocity_.clear();
andrewm@0:         lastCalculatedVelocityIndex_ = positionBuffer_->beginIndex() + 1;
andrewm@0:     }
andrewm@0:     
andrewm@0:     while(lastCalculatedVelocityIndex_ < positionBuffer_->endIndex()) {
andrewm@0:         // Calculate the velocity and add to buffer
andrewm@0:         key_position diffPosition = (*positionBuffer_)[lastCalculatedVelocityIndex_] - (*positionBuffer_)[lastCalculatedVelocityIndex_ - 1];
andrewm@0:         timestamp_diff_type diffTimestamp = positionBuffer_->timestampAt(lastCalculatedVelocityIndex_) - positionBuffer_->timestampAt(lastCalculatedVelocityIndex_ - 1);
andrewm@0:         key_velocity vel;
andrewm@0:         
andrewm@0:         if(diffTimestamp != 0)
andrewm@0:             vel = calculate_key_velocity(diffPosition, diffTimestamp);
andrewm@0:         else
andrewm@0:             vel = 0; // Bad measurement: replace with 0 so as not to mess up IIR calculations
andrewm@0:         
andrewm@0:         // Add the raw velocity to the buffer
andrewm@0:         rawVelocity_.insert(vel, positionBuffer_->timestampAt(lastCalculatedVelocityIndex_));
andrewm@0:         lastCalculatedVelocityIndex_++;
andrewm@0:     }
andrewm@0:     
andrewm@0:     positionBuffer_->unlock_mutex();
andrewm@0:     
andrewm@0:     // Bring the filtered velocity up to date
andrewm@0:     key_velocity filteredVel = filteredVelocity_.calculate();
andrewm@0:     //std::cout << "Key " << noteNumber_ << " velocity " << filteredVel << std::endl;
andrewm@0:     return filteredVel;
andrewm@0: }
andrewm@0: 
andrewm@0: // Helper function that locates the timestamp at which this key entered the
andrewm@0: // PartialPress (i.e. first non-idle) state. Returns missing value if the
andrewm@0: // state can't be located.
andrewm@0: timestamp_type MRPMapping::findTimestampOfPartialPress() {
andrewm@0:     if(positionTracker_ == 0)
andrewm@0:         return missing_value<timestamp_type>::missing();
andrewm@0:     if(positionTracker_->empty())
andrewm@0:         return missing_value<timestamp_type>::missing();
andrewm@0:     //Node<int>::reverse_iterator it = positionTracker_->rbegin();
andrewm@0:     Node<int>::size_type index = positionTracker_->endIndex() - 1;
andrewm@0:     bool foundPartialPressState = false;
andrewm@0:     timestamp_type earliestPartialPressTimestamp;
andrewm@0: 
andrewm@0:     // Search backwards from present
andrewm@0:     while(index >= positionTracker_->beginIndex()/*it != positionTracker_->rend()*/) {
andrewm@0:         if((*positionTracker_)[index].state == kPositionTrackerStatePartialPressAwaitingMax ||
andrewm@0:            (*positionTracker_)[index].state == kPositionTrackerStatePartialPressFoundMax) {
andrewm@0:             cout << "index " << index << " state " << (*positionTracker_)[index].state << endl;
andrewm@0:             foundPartialPressState = true;
andrewm@0:             earliestPartialPressTimestamp = positionTracker_->timestampAt(index);
andrewm@0:         }
andrewm@0:         else {
andrewm@0:             // This state is not a PartialPress state. Two cases: either
andrewm@0:             // we haven't yet encountered a partial press or we have found
andrewm@0:             // a state before the partial press, in which case the previous
andrewm@0:             // state we found was the first.
andrewm@0:                         cout << "index " << index << " state " << (*positionTracker_)[index].state << endl;
andrewm@0:             if(foundPartialPressState) {
andrewm@0:                 return earliestPartialPressTimestamp;
andrewm@0:             }
andrewm@0:         }
andrewm@0:         
andrewm@0:         // Step backwards one sample, but stop if we hit the beginning index
andrewm@0:         if(index == 0)
andrewm@0:             break;
andrewm@0:         index--;
andrewm@0:     }
andrewm@0:     
andrewm@0:     if(foundPartialPressState)
andrewm@0:         return earliestPartialPressTimestamp;
andrewm@0:     
andrewm@0:     // Didn't find anything if we get here
andrewm@0:     return missing_value<timestamp_type>::missing();
andrewm@0: }