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:   KeyPositionTracker.cpp: parses continuous key position and detects the
andrewm@0:   state of the key.
andrewm@0: */
andrewm@0: 
andrewm@0: #include "KeyPositionTracker.h"
andrewm@0: 
andrewm@0: // Default constructor
andrewm@0: KeyPositionTracker::KeyPositionTracker(capacity_type capacity, Node<key_position>& keyBuffer)
andrewm@0: : Node<KeyPositionTrackerNotification>(capacity), keyBuffer_(keyBuffer), engaged_(false) {
andrewm@0:     reset();
andrewm@0: }
andrewm@0: 
andrewm@0: // Copy constructor
andrewm@0: /*KeyPositionTracker::KeyPositionTracker(KeyPositionTracker const& obj)
andrewm@0: : Node<int>(obj), keyBuffer_(obj.keyBuffer_), engaged_(obj.engaged_) {
andrewm@0:     if(engaged_)
andrewm@0:         registerForTrigger(&keyBuffer_);
andrewm@0: }*/
andrewm@0: 
andrewm@0: // Calculate (MIDI-style) key press velocity from continuous key position
andrewm@0: std::pair<timestamp_type, key_velocity> KeyPositionTracker::pressVelocity() {
andrewm@0:     return pressVelocity(pressVelocityEscapementPosition_);
andrewm@0: }
andrewm@0: 
andrewm@0: std::pair<timestamp_type, key_velocity> KeyPositionTracker::pressVelocity(key_position escapementPosition) {
andrewm@0:     // Check that we have a valid start point from which to calculate
andrewm@0:     if(missing_value<timestamp_type>::isMissing(startTimestamp_)) {
andrewm@0:         return std::pair<timestamp_type, key_velocity>(missing_value<timestamp_type>::missing(),
andrewm@0:                                                        missing_value<key_velocity>::missing());
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Find where the key position crosses the indicated level
andrewm@0:     key_buffer_index index = startIndex_;
andrewm@0:     if(index < keyBuffer_.beginIndex() + 2)
andrewm@0:         index = keyBuffer_.beginIndex() + 2;
andrewm@0: 
andrewm@0:     while(index < keyBuffer_.endIndex() - kPositionTrackerSamplesNeededForPressVelocityAfterEscapement) {
andrewm@0:         // If the key press has a defined end, make sure we don't go past it
andrewm@0:         if(pressIndex_ != 0 && index >= pressIndex_)
andrewm@0:             break;
andrewm@0:         
andrewm@0:         if(keyBuffer_[index] > escapementPosition) {
andrewm@0:             // Found the place the position crosses the indicated threshold
andrewm@0:             // Now find the exact (interpolated) timestamp and velocity
andrewm@0:             timestamp_type exactPressTimestamp = keyBuffer_.timestampAt(index); // TODO
andrewm@0:             
andrewm@0:             // Velocity is calculated by an average of 2 samples before and 1 after
andrewm@0:             key_position diffPosition = keyBuffer_[index + kPositionTrackerSamplesNeededForPressVelocityAfterEscapement] - keyBuffer_[index - 2];
andrewm@0:             timestamp_diff_type diffTimestamp = keyBuffer_.timestampAt(index + kPositionTrackerSamplesNeededForPressVelocityAfterEscapement) - keyBuffer_.timestampAt(index - 2);
andrewm@0:             key_velocity velocity = calculate_key_velocity(diffPosition, diffTimestamp);
andrewm@0:             
andrewm@0:             return std::pair<timestamp_type, key_velocity>(exactPressTimestamp, velocity);
andrewm@0:         }
andrewm@0:         index++;
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Didn't find anything matching that threshold
andrewm@0:     return std::pair<timestamp_type, key_velocity>(missing_value<timestamp_type>::missing(),
andrewm@0:                                                    missing_value<key_velocity>::missing());
andrewm@0: }
andrewm@0: 
andrewm@0: // Calculate (MIDI-style) key release velocity from continuous key position
andrewm@0: std::pair<timestamp_type, key_velocity> KeyPositionTracker::releaseVelocity() {
andrewm@0:     return releaseVelocity(releaseVelocityEscapementPosition_);
andrewm@0: }
andrewm@0: 
andrewm@0: std::pair<timestamp_type, key_velocity> KeyPositionTracker::releaseVelocity(key_position returnPosition) {
andrewm@0:     // Check that we have a valid start point from which to calculate
andrewm@0:     if(missing_value<timestamp_type>::isMissing(releaseBeginTimestamp_)) {
andrewm@0:         return std::pair<timestamp_type, key_velocity>(missing_value<timestamp_type>::missing(),
andrewm@0:                                                        missing_value<key_velocity>::missing());
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Find where the key position crosses the indicated level
andrewm@0:     key_buffer_index index = releaseBeginIndex_;
andrewm@0:     if(index < keyBuffer_.beginIndex() + 2)
andrewm@0:         index = keyBuffer_.beginIndex() + 2;
andrewm@0: 
andrewm@0:     while(index < keyBuffer_.endIndex() - kPositionTrackerSamplesNeededForReleaseVelocityAfterEscapement) {
andrewm@0:         // Check for whether we've hit the end of the release interval, assuming
andrewm@0:         // the interval exists yet
andrewm@0:         if(releaseEndIndex_ != 0 && index >= releaseEndIndex_)
andrewm@0:             break;
andrewm@0:         
andrewm@0:         if(keyBuffer_[index] < returnPosition) {
andrewm@0:             // Found the place the position crosses the indicated threshold
andrewm@0:             // Now find the exact (interpolated) timestamp and velocity
andrewm@0:             timestamp_type exactPressTimestamp = keyBuffer_.timestampAt(index); // TODO
andrewm@0:             
andrewm@0:             // Velocity is calculated by an average of 2 samples before and 1 after
andrewm@0:             key_position diffPosition = keyBuffer_[index + kPositionTrackerSamplesNeededForReleaseVelocityAfterEscapement] - keyBuffer_[index - 2];
andrewm@0:             timestamp_diff_type diffTimestamp = keyBuffer_.timestampAt(index + kPositionTrackerSamplesNeededForReleaseVelocityAfterEscapement) - keyBuffer_.timestampAt(index - 2);
andrewm@0:             key_velocity velocity = calculate_key_velocity(diffPosition, diffTimestamp);
andrewm@0:             
andrewm@0:             std::cout << "found release velocity " << velocity << "(diffp " << diffPosition << ", diffT " << diffTimestamp << ")" << std::endl;
andrewm@0:             
andrewm@0:             return std::pair<timestamp_type, key_velocity>(exactPressTimestamp, velocity);
andrewm@0:         }
andrewm@0:         index++;
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Didn't find anything matching that threshold
andrewm@0:     return std::pair<timestamp_type, key_velocity>(missing_value<timestamp_type>::missing(),
andrewm@0:                                                    missing_value<key_velocity>::missing());
andrewm@0: }
andrewm@0: 
andrewm@0: // Calculate and return features about the percussiveness of the key press
andrewm@0: KeyPositionTracker::PercussivenessFeatures KeyPositionTracker::pressPercussiveness() {
andrewm@0:     PercussivenessFeatures features;
andrewm@0:     key_buffer_index index;
andrewm@0:     key_velocity maximumVelocity, largestVelocityDifference;
andrewm@0:     key_buffer_index maximumVelocityIndex, largestVelocityDifferenceIndex;
andrewm@0:     
andrewm@0:     // Check that we have a valid start point from which to calculate
andrewm@0:     if(missing_value<timestamp_type>::isMissing(startTimestamp_) || keyBuffer_.beginIndex() > startIndex_ - 1) {
andrewm@0:         std::cout << "*** no start time\n";
andrewm@0:         features.percussiveness = missing_value<float>::missing();
andrewm@0:         return features;
andrewm@0:     }
andrewm@0:     
andrewm@0:     // From the start of the key press, look for an initial maximum in velocity
andrewm@0:     index = startIndex_;
andrewm@0:     
andrewm@0:     maximumVelocity = scale_key_velocity(0);
andrewm@0:     maximumVelocityIndex = startIndex_;
andrewm@0:     largestVelocityDifference = scale_key_velocity(0);
andrewm@0:     largestVelocityDifferenceIndex = startIndex_;
andrewm@0:     
andrewm@0:     std::cout << "*** start index " << index << std::endl;
andrewm@0:     
andrewm@0:     while(index < keyBuffer_.endIndex()) {
andrewm@0:         if(pressIndex_ != 0 && index >= pressIndex_)
andrewm@0:             break;
andrewm@0:         
andrewm@0:         key_position diffPosition = keyBuffer_[index] - keyBuffer_[index - 1];
andrewm@0:         timestamp_diff_type diffTimestamp = keyBuffer_.timestampAt(index) - keyBuffer_.timestampAt(index - 1);
andrewm@0:         key_velocity velocity = calculate_key_velocity(diffPosition, diffTimestamp);
andrewm@0:         
andrewm@0:         // Look for maximum of velocity
andrewm@0:         if(velocity > maximumVelocity) {
andrewm@0:             maximumVelocity = velocity;
andrewm@0:             maximumVelocityIndex = index;
andrewm@0:             std::cout << "*** found new max velocity " << maximumVelocity << " at index " << index << std::endl;
andrewm@0:         }
andrewm@0:         
andrewm@0:         // And given the difference between the max and the current sample,
andrewm@0:         // look for the largest rebound (velocity hitting a peak and falling)
andrewm@0:         if(maximumVelocity - velocity > largestVelocityDifference) {
andrewm@0:             largestVelocityDifference = maximumVelocity - velocity;
andrewm@0:             largestVelocityDifferenceIndex = index;
andrewm@0:             std::cout << "*** found new diff velocity " << largestVelocityDifference << " at index " << index << std::endl;
andrewm@0:         }
andrewm@0:         
andrewm@0:         // Only look at the early part of the key press: if the key position
andrewm@0:         // makes it more than a certain amount down, assume the initial spike
andrewm@0:         // has passed and finish up. But always allow at least 5 points for the
andrewm@0:         // fastest key presses to be considered.
andrewm@0:         if(index - startIndex_ >= 4 && keyBuffer_[index] > kPositionTrackerPositionThresholdForPercussivenessCalculation) {
andrewm@0:             break;
andrewm@0:         }
andrewm@0:         
andrewm@0:         index++;
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Now transfer what we've found to the data structure
andrewm@0:     features.velocitySpikeMaximum = Event(maximumVelocityIndex, maximumVelocity, keyBuffer_.timestampAt(maximumVelocityIndex));
andrewm@0:     features.velocitySpikeMinimum = Event(largestVelocityDifferenceIndex, maximumVelocity - largestVelocityDifference,
andrewm@0:                                           keyBuffer_.timestampAt(largestVelocityDifferenceIndex));
andrewm@0:     features.timeFromStartToSpike = keyBuffer_.timestampAt(maximumVelocityIndex) - keyBuffer_.timestampAt(startIndex_);
andrewm@0:     
andrewm@0:     // Check if we found a meaningful difference. If not, percussiveness is set to 0
andrewm@0:     if(largestVelocityDifference == scale_key_velocity(0)) {
andrewm@0:         features.percussiveness = 0.0;
andrewm@0:         features.areaPrecedingSpike = scale_key_velocity(0);
andrewm@0:         features.areaFollowingSpike = scale_key_velocity(0);
andrewm@0:         return features;
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Calculate the area under the velocity curve before and after the maximum
andrewm@0:     features.areaPrecedingSpike = scale_key_velocity(0);
andrewm@0:     for(index = startIndex_; index < maximumVelocityIndex; index++) {
andrewm@0:         key_position diffPosition = keyBuffer_[index] - keyBuffer_[index - 1];
andrewm@0:         timestamp_diff_type diffTimestamp = keyBuffer_.timestampAt(index) - keyBuffer_.timestampAt(index - 1);
andrewm@0:         features.areaPrecedingSpike += calculate_key_velocity(diffPosition, diffTimestamp);
andrewm@0:     }
andrewm@0:     features.areaFollowingSpike = scale_key_velocity(0);
andrewm@0:     for(index = maximumVelocityIndex; index < largestVelocityDifferenceIndex; index++) {
andrewm@0:         key_position diffPosition = keyBuffer_[index] - keyBuffer_[index - 1];
andrewm@0:         timestamp_diff_type diffTimestamp = keyBuffer_.timestampAt(index) - keyBuffer_.timestampAt(index - 1);
andrewm@0:         features.areaFollowingSpike += calculate_key_velocity(diffPosition, diffTimestamp);
andrewm@0:     }
andrewm@0:     
andrewm@0:     std::cout << "area before = " << features.areaPrecedingSpike << " after = " << features.areaFollowingSpike << std::endl;
andrewm@0:     
andrewm@0:     features.percussiveness = features.velocitySpikeMaximum.position;
andrewm@0:     
andrewm@0:     return features;
andrewm@0: }
andrewm@0: 
andrewm@0: // Register to receive messages from the key buffer on each new sample
andrewm@0: void KeyPositionTracker::engage() {
andrewm@0:     if(engaged_)
andrewm@0:         return;
andrewm@0: 
andrewm@0:     registerForTrigger(&keyBuffer_);
andrewm@0:     engaged_ = true;
andrewm@0: }
andrewm@0: 
andrewm@0: // Unregister from receiving message on new samples
andrewm@0: void KeyPositionTracker::disengage() {
andrewm@0:     if(!engaged_)
andrewm@0:         return;
andrewm@0:     
andrewm@0:     unregisterForTrigger(&keyBuffer_);
andrewm@0:     engaged_ = false;
andrewm@0: }
andrewm@0: 
andrewm@0: // Clear current state and reset to unknown state
andrewm@0: void KeyPositionTracker::reset() {
andrewm@0: 	Node<KeyPositionTrackerNotification>::clear();
andrewm@0:     
andrewm@0:     currentState_ = kPositionTrackerStateUnknown;
andrewm@0:     currentlyAvailableFeatures_ = KeyPositionTrackerNotification::kFeaturesNone;
andrewm@0:     currentMinIndex_ = currentMaxIndex_ = startIndex_ = pressIndex_ = 0;
andrewm@0:     releaseBeginIndex_ = releaseEndIndex_ = 0;
andrewm@0:     lastMinMaxPosition_ = startPosition_ = pressPosition_ = missing_value<key_position>::missing();
andrewm@0:     releaseBeginPosition_ = releaseEndPosition_ = missing_value<key_position>::missing();
andrewm@0:     currentMinPosition_ = currentMaxPosition_ = missing_value<key_position>::missing();
andrewm@0:     startTimestamp_ = pressTimestamp_ = missing_value<timestamp_type>::missing();
andrewm@0:     currentMinTimestamp_ = currentMaxTimestamp_ = missing_value<timestamp_type>::missing();
andrewm@0:     releaseBeginTimestamp_ = releaseEndTimestamp_ = missing_value<timestamp_type>::missing();
andrewm@0:     pressVelocityEscapementPosition_ = kPositionTrackerDefaultPositionForPressVelocityCalculation;
andrewm@0:     releaseVelocityEscapementPosition_ = kPositionTrackerDefaultPositionForReleaseVelocityCalculation;
andrewm@0:     pressVelocityAvailableIndex_ = releaseVelocityAvailableIndex_ = percussivenessAvailableIndex_ = 0;
andrewm@0:     releaseVelocityWaitingForThresholdCross_ = false;
andrewm@0: }
andrewm@0: 
andrewm@0: // Evaluator function. Update the current state
andrewm@0: void KeyPositionTracker::triggerReceived(TriggerSource* who, timestamp_type timestamp) {
andrewm@0: 
andrewm@0: 	if(who != &keyBuffer_)
andrewm@0: 		return;
andrewm@0:     
andrewm@0:     // Always start in the partial press state after a reset, retroactively locating
andrewm@0:     // the start position for this key press
andrewm@0:     if(empty()) {
andrewm@0:         findKeyPressStart(timestamp);
andrewm@0:         changeState(kPositionTrackerStatePartialPressAwaitingMax, timestamp);
andrewm@0:     }
andrewm@0:     
andrewm@0:     key_position currentKeyPosition = keyBuffer_.latest();
andrewm@0:     key_buffer_index currentBufferIndex = keyBuffer_.endIndex() - 1;
andrewm@0:     
andrewm@0:     // First, check queued actions to see if we can calculate a new feature
andrewm@0:     // ** Press Velocity **
andrewm@0:     if(pressVelocityAvailableIndex_ != 0) {
andrewm@0:         if(currentBufferIndex >= pressVelocityAvailableIndex_) {
andrewm@0:             // Can now calculate press velocity
andrewm@0:             currentlyAvailableFeatures_ |= KeyPositionTrackerNotification::kFeaturePressVelocity;
andrewm@0:             notifyFeature(KeyPositionTrackerNotification::kNotificationTypeFeatureAvailableVelocity, timestamp);
andrewm@0:             pressVelocityAvailableIndex_ = 0;
andrewm@0:         }
andrewm@0:     }
andrewm@0:     // ** Release Velocity **
andrewm@0:     if(releaseVelocityWaitingForThresholdCross_) {
andrewm@0:         if(currentKeyPosition < releaseVelocityEscapementPosition_)
andrewm@0:             prepareReleaseVelocityFeature(currentBufferIndex, timestamp);
andrewm@0:     }
andrewm@0:     else if(releaseVelocityAvailableIndex_ != 0) {
andrewm@0:         if(currentBufferIndex >= releaseVelocityAvailableIndex_) {
andrewm@0:             // Can now calculate release velocity
andrewm@0:             currentlyAvailableFeatures_ |= KeyPositionTrackerNotification::kFeatureReleaseVelocity;
andrewm@0:             notifyFeature(KeyPositionTrackerNotification::kNotificationTypeFeatureAvailableReleaseVelocity, timestamp);
andrewm@0:             releaseVelocityAvailableIndex_ = 0;
andrewm@0:         }
andrewm@0:     }
andrewm@0:     // ** Percussiveness **
andrewm@0:     if(percussivenessAvailableIndex_ != 0) {
andrewm@0:         if(currentBufferIndex >= percussivenessAvailableIndex_) {
andrewm@0:             // Can now calculate percussiveness
andrewm@0:             currentlyAvailableFeatures_ |= KeyPositionTrackerNotification::kFeaturePercussiveness;
andrewm@0:             notifyFeature(KeyPositionTrackerNotification::kNotificationTypeFeatureAvailablePercussiveness, timestamp);
andrewm@0:             percussivenessAvailableIndex_ = 0;
andrewm@0:         }
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Major state transitions next, centered on whether the key is pressed
andrewm@0:     // fully or partially
andrewm@0:     if(currentState_ == kPositionTrackerStatePartialPressAwaitingMax ||
andrewm@0:        currentState_ == kPositionTrackerStatePartialPressFoundMax) {
andrewm@0:         // These are collectively the pre-press states
andrewm@0:         if(currentKeyPosition >= kPositionTrackerPressPosition + kPositionTrackerPressHysteresis) {
andrewm@0:             // Key has gone far enough down to be considered pressed, but hasn't necessarily
andrewm@0:             // made it down yet.
andrewm@0:             pressIndex_ = 0;
andrewm@0:             pressPosition_ = missing_value<key_position>::missing();
andrewm@0:             pressTimestamp_ = missing_value<timestamp_type>::missing();
andrewm@0:             
andrewm@0:             changeState(kPositionTrackerStatePressInProgress, timestamp);
andrewm@0:         }
andrewm@0:     }
andrewm@0:     else if(currentState_ == kPositionTrackerStateReleaseInProgress ||
andrewm@0:             currentState_ == kPositionTrackerStateReleaseFinished) {
andrewm@0:         if(currentKeyPosition >= kPositionTrackerPressPosition + kPositionTrackerPressHysteresis) {
andrewm@0:             // Key was releasing but is now back down. Need to reprime the start
andrewm@0:             // position information, which will be taken as the last minimum.
andrewm@0:             startIndex_ = currentMinIndex_;
andrewm@0:             startPosition_ = currentMinPosition_;
andrewm@0:             startTimestamp_ = currentMinTimestamp_;
andrewm@0:             pressIndex_ = 0;
andrewm@0:             pressPosition_ = missing_value<key_position>::missing();
andrewm@0:             pressTimestamp_ = missing_value<timestamp_type>::missing();
andrewm@0:             
andrewm@0:             changeState(kPositionTrackerStatePressInProgress, timestamp);
andrewm@0:         }
andrewm@0:     }
andrewm@0:     else if(currentState_ == kPositionTrackerStatePressInProgress) {
andrewm@0:         // Press has started, wait to find its max position before labeling the key as "down"
andrewm@0:         if(currentKeyPosition < kPositionTrackerPressPosition - kPositionTrackerPressHysteresis) {
andrewm@0:             // Key is on its way back up: find where release began
andrewm@0:             findKeyReleaseStart(timestamp);
andrewm@0: 
andrewm@0:             changeState(kPositionTrackerStateReleaseInProgress, timestamp);
andrewm@0:         }
andrewm@0:     }
andrewm@0:     else if(currentState_ == kPositionTrackerStateDown) {
andrewm@0:         if(currentKeyPosition < kPositionTrackerPressPosition - kPositionTrackerPressHysteresis) {
andrewm@0:             // Key is on its way back up: find where release began
andrewm@0:             findKeyReleaseStart(timestamp);
andrewm@0:             
andrewm@0:             changeState(kPositionTrackerStateReleaseInProgress, timestamp);
andrewm@0:         }
andrewm@0:     }
andrewm@0: 
andrewm@0:     // Find the maxima and minima of the key motion
andrewm@0:     if(missing_value<key_position>::isMissing(currentMaxPosition_) ||
andrewm@0:        currentKeyPosition > currentMaxPosition_) {
andrewm@0:         // Found a new local maximum
andrewm@0:         currentMaxIndex_ = currentBufferIndex;
andrewm@0:         currentMaxPosition_ = currentKeyPosition;
andrewm@0:         currentMaxTimestamp_ = timestamp;
andrewm@0:         
andrewm@0:         // If we previously found a maximum, go back to the original
andrewm@0:         // state so we can process the new max that is in progress
andrewm@0:         if(currentState_ == kPositionTrackerStatePartialPressFoundMax)
andrewm@0:             changeState(kPositionTrackerStatePartialPressAwaitingMax, timestamp);
andrewm@0:     }
andrewm@0:     else if(missing_value<key_position>::isMissing(currentMinPosition_) ||
andrewm@0:             currentKeyPosition < currentMinPosition_) {
andrewm@0:         // Found a new local minimum
andrewm@0:         currentMinIndex_ = currentBufferIndex;
andrewm@0:         currentMinPosition_ = currentKeyPosition;
andrewm@0:         currentMinTimestamp_ = timestamp;
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Check if the deviation between min and max exceeds the threshold of significance,
andrewm@0:     // and if so, figure out when a peak occurs
andrewm@0:     if(!missing_value<key_position>::isMissing(currentMaxPosition_) &&
andrewm@0:        !missing_value<key_position>::isMissing(lastMinMaxPosition_)) {
andrewm@0:         if(currentMaxPosition_ - lastMinMaxPosition_ >= kPositionTrackerMinMaxSpacingThreshold && currentBufferIndex != currentMaxIndex_) {
andrewm@0:             // We need to come down off the current maximum before we can be sure that we've found the right location.
andrewm@0:             // Implement a sliding threshold that gets lower the farther away from the maximum we get
andrewm@0:             key_position triggerThreshold = kPositionTrackerMinMaxSpacingThreshold / (key_position)(currentBufferIndex - currentMaxIndex_);
andrewm@0:             
andrewm@0:             if(currentKeyPosition < currentMaxPosition_ - triggerThreshold) {
andrewm@0:                 // Found the local maximum and the position has already retreated from it
andrewm@0:                 lastMinMaxPosition_ = currentMaxPosition_;
andrewm@0:                 
andrewm@0:                 if(currentState_ == kPositionTrackerStatePressInProgress) {
andrewm@0:                     // If we were waiting for a press to complete, this is it.
andrewm@0:                     pressIndex_ = currentMaxIndex_;
andrewm@0:                     pressPosition_ = currentMaxPosition_;
andrewm@0:                     pressTimestamp_ = currentMaxTimestamp_;
andrewm@0:                     
andrewm@0:                     // Insert the state change into the buffer timestamped according to
andrewm@0:                     // when the maximum arrived, unless that would put it earlier than what's already there
andrewm@0:                     timestamp_type stateChangeTimestamp = latestTimestamp() > currentMaxTimestamp_ ? latestTimestamp() : currentMaxTimestamp_;
andrewm@0:                     changeState(kPositionTrackerStateDown, stateChangeTimestamp);
andrewm@0:                 }
andrewm@0:                 else if(currentState_ == kPositionTrackerStatePartialPressAwaitingMax) {
andrewm@0:                     // Otherwise if we were waiting for a maximum to occur that was
andrewm@0:                     // short of a full press, this might be it if it is of sufficient size
andrewm@0:                     if(currentMaxPosition_ >= kPositionTrackerFirstMaxThreshold) {
andrewm@0:                         timestamp_type stateChangeTimestamp = latestTimestamp() > currentMaxTimestamp_ ? latestTimestamp() : currentMaxTimestamp_;
andrewm@0:                         changeState(kPositionTrackerStatePartialPressFoundMax, stateChangeTimestamp);
andrewm@0:                     }
andrewm@0:                 }
andrewm@0:                 
andrewm@0:                 // Reinitialize the minimum value for the next search
andrewm@0:                 currentMinIndex_ = currentBufferIndex;
andrewm@0:                 currentMinPosition_ = currentKeyPosition;
andrewm@0:                 currentMinTimestamp_ = timestamp;
andrewm@0:             }
andrewm@0:         }
andrewm@0:     }
andrewm@0:     if(!missing_value<key_position>::isMissing(currentMinPosition_) &&
andrewm@0:        !missing_value<key_position>::isMissing(lastMinMaxPosition_)) {
andrewm@0:         if(lastMinMaxPosition_ - currentMinPosition_ >= kPositionTrackerMinMaxSpacingThreshold && currentBufferIndex != currentMinIndex_) {
andrewm@0:             // We need to come up from the current minimum before we can be sure that we've found the right location.
andrewm@0:             // Implement a sliding threshold that gets lower the farther away from the minimum we get
andrewm@0:             key_position triggerThreshold = kPositionTrackerMinMaxSpacingThreshold / (key_position)(currentBufferIndex - currentMinIndex_);
andrewm@0: 
andrewm@0:             if(currentKeyPosition > currentMinPosition_ + triggerThreshold) {
andrewm@0:                 // Found the local minimum and the position has already retreated from it
andrewm@0:                 lastMinMaxPosition_ = currentMinPosition_;
andrewm@0:                 
andrewm@0:                 // If in the middle of releasing, see whether this minimum appears to have completed the release
andrewm@0:                 if(currentState_ == kPositionTrackerStateReleaseInProgress) {
andrewm@0:                     if(currentMinPosition_ < kPositionTrackerReleaseFinishPosition) {
andrewm@0:                         releaseEndIndex_ = currentMinIndex_;
andrewm@0:                         releaseEndPosition_ = currentMinPosition_;
andrewm@0:                         releaseEndTimestamp_ = currentMinTimestamp_;
andrewm@0:                         
andrewm@0:                         timestamp_type stateChangeTimestamp = latestTimestamp() > currentMinTimestamp_ ? latestTimestamp() : currentMinTimestamp_;
andrewm@0:                         changeState(kPositionTrackerStateReleaseFinished, stateChangeTimestamp);
andrewm@0:                     }
andrewm@0:                 }
andrewm@0:                 
andrewm@0:                 // Reinitialize the maximum value for the next search
andrewm@0:                 currentMaxIndex_ = currentBufferIndex;
andrewm@0:                 currentMaxPosition_ = currentKeyPosition;
andrewm@0:                 currentMaxTimestamp_ = timestamp;
andrewm@0:             }
andrewm@0:         }
andrewm@0:     }
andrewm@0: }
andrewm@0: 
andrewm@0: // Change the current state of the tracker and generate a notification
andrewm@0: void KeyPositionTracker::changeState(int newState, timestamp_type timestamp) {
andrewm@0:     KeyPositionTracker::key_buffer_index index;
andrewm@0:     KeyPositionTracker::key_buffer_index mostRecentIndex = 0;
andrewm@0:     
andrewm@0:     if(keyBuffer_.empty())
andrewm@0:         mostRecentIndex = keyBuffer_.endIndex() - 1;
andrewm@0:     
andrewm@0:     // Manage features based on state
andrewm@0:     switch(newState) {
andrewm@0:         case kPositionTrackerStatePressInProgress:
andrewm@0:             // Clear features for a retrigger
andrewm@0:             if(currentState_ == kPositionTrackerStateReleaseInProgress ||
andrewm@0:                currentState_ == kPositionTrackerStateReleaseFinished)
andrewm@0:                 currentlyAvailableFeatures_ = KeyPositionTrackerNotification::kFeaturesNone;
andrewm@0:             
andrewm@0:             // Look for percussiveness first since it will always be available by the time of
andrewm@0:             // key press. That means we can count on it arriving before velocity every time.
andrewm@0:             if((currentlyAvailableFeatures_ & KeyPositionTrackerNotification::kFeaturePercussiveness) == 0
andrewm@0:                && percussivenessAvailableIndex_ == 0) {
andrewm@0:                 currentlyAvailableFeatures_ |= KeyPositionTrackerNotification::kFeaturePercussiveness;
andrewm@0:                 notifyFeature(KeyPositionTrackerNotification::kNotificationTypeFeatureAvailablePercussiveness, timestamp);
andrewm@0:                 percussivenessAvailableIndex_ = 0;
andrewm@0:             }
andrewm@0:             
andrewm@0:             // Start looking for the data needed for MIDI onset velocity.
andrewm@0:             // Where did the key cross the escapement position? How many more samples do
andrewm@0:             // we need to calculate velocity?
andrewm@0:             index = findMostRecentKeyPositionCrossing(pressVelocityEscapementPosition_, false, 1000);
andrewm@0:             if(index + kPositionTrackerSamplesNeededForPressVelocityAfterEscapement <= mostRecentIndex) {
andrewm@0:                 // Here, we already have the velocity information
andrewm@0:                 currentlyAvailableFeatures_ |= KeyPositionTrackerNotification::kFeaturePressVelocity;
andrewm@0:                 notifyFeature(KeyPositionTrackerNotification::kNotificationTypeFeatureAvailableVelocity, timestamp);
andrewm@0:             }
andrewm@0:             else {
andrewm@0:                 // Otherwise, we need to send a notification when the information becomes available
andrewm@0:                 pressVelocityAvailableIndex_ = index + kPositionTrackerSamplesNeededForPressVelocityAfterEscapement;
andrewm@0:             }
andrewm@0:             break;
andrewm@0:         case kPositionTrackerStateReleaseInProgress:
andrewm@0:             // Start looking for the data needed for MIDI release velocity.
andrewm@0:             // Where did the key cross the release escaoentb position? How many more samples do
andrewm@0:             // we need to calculate velocity?
andrewm@0:             prepareReleaseVelocityFeature(mostRecentIndex, timestamp);
andrewm@0:             break;
andrewm@0:         case kPositionTrackerStatePartialPressFoundMax:
andrewm@0:             // Also look for the percussiveness features, if not already present
andrewm@0:             if((currentlyAvailableFeatures_ & KeyPositionTrackerNotification::kFeaturePercussiveness) == 0
andrewm@0:                && percussivenessAvailableIndex_ == 0) {
andrewm@0:                 currentlyAvailableFeatures_ |= KeyPositionTrackerNotification::kFeaturePercussiveness;
andrewm@0:                 notifyFeature(KeyPositionTrackerNotification::kNotificationTypeFeatureAvailablePercussiveness, timestamp);
andrewm@0:                 percussivenessAvailableIndex_ = 0;
andrewm@0:             }
andrewm@0:             break;
andrewm@0:         case kPositionTrackerStatePartialPressAwaitingMax:
andrewm@0:         case kPositionTrackerStateUnknown:
andrewm@0:             // Reset all features
andrewm@0:             currentlyAvailableFeatures_ = KeyPositionTrackerNotification::kFeaturesNone;
andrewm@0:             break;
andrewm@0:         case kPositionTrackerStateDown:
andrewm@0:         case kPositionTrackerStateReleaseFinished:
andrewm@0:         default:
andrewm@0:             // Don't change features
andrewm@0:             break;
andrewm@0:     }
andrewm@0:     
andrewm@0:     currentState_ = newState;
andrewm@0:     
andrewm@0:     KeyPositionTrackerNotification notification;
andrewm@0:     notification.type = KeyPositionTrackerNotification::kNotificationTypeStateChange;
andrewm@0:     notification.state = newState;
andrewm@0:     notification.features = currentlyAvailableFeatures_;
andrewm@0:     
andrewm@0:     insert(notification, timestamp);
andrewm@0: }
andrewm@0: 
andrewm@0: // Notify listeners that a given feature has become available
andrewm@0: void KeyPositionTracker::notifyFeature(int notificationType, timestamp_type timestamp) {
andrewm@0:     // Can now calculate press velocity
andrewm@0:     KeyPositionTrackerNotification notification;
andrewm@0:     
andrewm@0:     notification.state = currentState_;
andrewm@0:     notification.type = notificationType;
andrewm@0:     notification.features = currentlyAvailableFeatures_;
andrewm@0:     
andrewm@0:     insert(notification, timestamp);
andrewm@0: }
andrewm@0: 
andrewm@0: // When starting from a blank state, retroactively locate
andrewm@0: // the start of the key press so it can be used to calculate
andrewm@0: // features of key motion
andrewm@0: void KeyPositionTracker::findKeyPressStart(timestamp_type timestamp) {
andrewm@0:     if(keyBuffer_.size() < kPositionTrackerSamplesToAverageForStartVelocity + 1)
andrewm@0:         return;
andrewm@0:     
andrewm@0:     key_buffer_index index = keyBuffer_.endIndex() - 1;
andrewm@0:     int searchBackCounter = 0;
andrewm@0:     
andrewm@0:     while(index >= keyBuffer_.beginIndex() + kPositionTrackerSamplesToAverageForStartVelocity && searchBackCounter <= kPositionTrackerSamplesToSearchForStartLocation) {
andrewm@0:         // Take the N-sample velocity average and compare to a minimum threshold
andrewm@0:         key_position diffPosition = keyBuffer_[index] - keyBuffer_[index - kPositionTrackerSamplesToAverageForStartVelocity];
andrewm@0:         timestamp_diff_type diffTimestamp = keyBuffer_.timestampAt(index) - keyBuffer_.timestampAt(index - kPositionTrackerSamplesToAverageForStartVelocity);
andrewm@0:         key_velocity velocity = calculate_key_velocity(diffPosition, diffTimestamp);
andrewm@0:         
andrewm@0:         if(velocity < kPositionTrackerStartVelocityThreshold) {
andrewm@0:             break;
andrewm@0:         }
andrewm@0:         
andrewm@0:         searchBackCounter++;
andrewm@0:         index--;
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Having either found the minimum velocity or reached the beginning of the search period,
andrewm@0:     // store the key start information. Since the velocity is calculated over a window, choose
andrewm@0:     // a start position in the middle of the window.
andrewm@0:     startIndex_ = index - kPositionTrackerSamplesToAverageForStartVelocity/2;
andrewm@0:     startPosition_ = keyBuffer_[index - kPositionTrackerSamplesToAverageForStartVelocity/2];
andrewm@0:     startTimestamp_ = keyBuffer_.timestampAt(index - kPositionTrackerSamplesToAverageForStartVelocity/2);
andrewm@0:     lastMinMaxPosition_ = startPosition_;
andrewm@0:     
andrewm@0:     // After saving that information, look further back for a specified number of samples to see if there
andrewm@0:     // is another mini-spike at the beginning of the key press. This can happen with highly percussive presses.
andrewm@0:     // If so, the start is actually the earlier time.
andrewm@0:     
andrewm@0:     // Leave index where it was...
andrewm@0:     searchBackCounter = 0;
andrewm@0:     bool haveFoundVelocitySpike = false, haveFoundNewMinimum = false;
andrewm@0:     
andrewm@0:     while(index >= keyBuffer_.beginIndex() + kPositionTrackerSamplesToAverageForStartVelocity && searchBackCounter <= kPositionTrackerSamplesToSearchBeyondStartLocation) {
andrewm@0:         // Take the N-sample velocity average and compare to a minimum threshold
andrewm@0:         key_position diffPosition = keyBuffer_[index] - keyBuffer_[index - kPositionTrackerSamplesToAverageForStartVelocity];
andrewm@0:         timestamp_diff_type diffTimestamp = keyBuffer_.timestampAt(index) - keyBuffer_.timestampAt(index - kPositionTrackerSamplesToAverageForStartVelocity);
andrewm@0:         key_velocity velocity = calculate_key_velocity(diffPosition, diffTimestamp);
andrewm@0:         
andrewm@0:         if(velocity > kPositionTrackerStartVelocitySpikeThreshold) {
andrewm@0:             std::cout << "At index " << index << ", velocity is " << velocity << std::endl;
andrewm@0:             haveFoundVelocitySpike = true;
andrewm@0:         }
andrewm@0:         
andrewm@0:         if(velocity < kPositionTrackerStartVelocityThreshold && haveFoundVelocitySpike) {
andrewm@0:             std::cout << "At index " << index << ", velocity is " << velocity << std::endl;
andrewm@0:             haveFoundNewMinimum = true;
andrewm@0:             break;
andrewm@0:         }
andrewm@0:         
andrewm@0:         searchBackCounter++;
andrewm@0:         index--;
andrewm@0:     }
andrewm@0:     
andrewm@0:     if(haveFoundNewMinimum) {
andrewm@0:         // Here we looked back beyond a small spike and found an earlier start time
andrewm@0:         startIndex_ = index - kPositionTrackerSamplesToAverageForStartVelocity/2;
andrewm@0:         startPosition_ = keyBuffer_[index - kPositionTrackerSamplesToAverageForStartVelocity/2];
andrewm@0:         startTimestamp_ = keyBuffer_.timestampAt(index - kPositionTrackerSamplesToAverageForStartVelocity/2);
andrewm@0:         lastMinMaxPosition_ = startPosition_;
andrewm@0:         
andrewm@0:         std::cout << "Found previous location\n";
andrewm@0:     }
andrewm@0: }
andrewm@0: 
andrewm@0: // When a key is released, retroactively locate where the release started
andrewm@0: void KeyPositionTracker::findKeyReleaseStart(timestamp_type timestamp) {
andrewm@0:     if(keyBuffer_.size() < kPositionTrackerSamplesToAverageForStartVelocity + 1)
andrewm@0:         return;
andrewm@0:     
andrewm@0:     key_buffer_index index = keyBuffer_.endIndex() - 1;
andrewm@0:     int searchBackCounter = 0;
andrewm@0:     
andrewm@0:     while(index >= keyBuffer_.beginIndex() + kPositionTrackerSamplesToAverageForStartVelocity && searchBackCounter <= kPositionTrackerSamplesToSearchForReleaseLocation) {
andrewm@0:         // Take the N-sample velocity average and compare to a minimum threshold
andrewm@0:         key_position diffPosition = keyBuffer_[index] - keyBuffer_[index - kPositionTrackerSamplesToAverageForStartVelocity];
andrewm@0:         timestamp_diff_type diffTimestamp = keyBuffer_.timestampAt(index) - keyBuffer_.timestampAt(index - kPositionTrackerSamplesToAverageForStartVelocity);
andrewm@0:         key_velocity velocity = calculate_key_velocity(diffPosition, diffTimestamp);
andrewm@0:         
andrewm@0:         if(velocity > kPositionTrackerReleaseVelocityThreshold) {
andrewm@0:             std::cout << "Found release at index " << index << " (vel = " << velocity << ")\n";
andrewm@0:             break;
andrewm@0:         }
andrewm@0:         
andrewm@0:         searchBackCounter++;
andrewm@0:         index--;
andrewm@0:     }
andrewm@0:     
andrewm@0:     // Having either found the minimum velocity or reached the beginning of the search period,
andrewm@0:     // store the key release information.
andrewm@0:     releaseBeginIndex_ = index - kPositionTrackerSamplesToAverageForStartVelocity/2;
andrewm@0:     releaseBeginPosition_ = keyBuffer_[index - kPositionTrackerSamplesToAverageForStartVelocity/2];
andrewm@0:     releaseBeginTimestamp_ = keyBuffer_.timestampAt(index - kPositionTrackerSamplesToAverageForStartVelocity/2);
andrewm@0:     lastMinMaxPosition_ = releaseBeginPosition_;
andrewm@0:     
andrewm@0:     // Clear the release end position so there's no possibility of an inconsistent state
andrewm@0:     releaseEndIndex_ = 0;
andrewm@0:     releaseEndPosition_ = missing_value<key_position>::missing();
andrewm@0:     releaseEndTimestamp_ = missing_value<timestamp_type>::missing();
andrewm@0: }
andrewm@0: 
andrewm@0: // Find the index at which the key position crosses the given threshold. Returns 0 if not found.
andrewm@0: KeyPositionTracker::key_buffer_index KeyPositionTracker::findMostRecentKeyPositionCrossing(key_position threshold, bool greaterThan, int maxDistance) {
andrewm@0:     if(keyBuffer_.empty())
andrewm@0:         return 0;
andrewm@0:     
andrewm@0:     key_buffer_index index = keyBuffer_.endIndex() - 1;
andrewm@0:     int searchBackCounter = 0;
andrewm@0:     
andrewm@0:     // Check if the most recent sample already meets the criterion. If so,
andrewm@0:     // there's no crossing yet.
andrewm@0:     if(keyBuffer_[index] >= threshold && greaterThan)
andrewm@0:         return 0;
andrewm@0:     if(keyBuffer_[index] <= threshold && !greaterThan)
andrewm@0:         return 0;
andrewm@0:     
andrewm@0:     while(index >= keyBuffer_.beginIndex() && searchBackCounter <= maxDistance) {
andrewm@0:         if(keyBuffer_[index] >= threshold && greaterThan)
andrewm@0:             return index;
andrewm@0:         else if(keyBuffer_[index] <= threshold && !greaterThan)
andrewm@0:             return index;
andrewm@0:         
andrewm@0:         searchBackCounter++;
andrewm@0:         index--;
andrewm@0:     }
andrewm@0:     
andrewm@0:     return 0;
andrewm@0: }
andrewm@0: 
andrewm@0: void KeyPositionTracker::prepareReleaseVelocityFeature(KeyPositionTracker::key_buffer_index mostRecentIndex, timestamp_type timestamp) {
andrewm@0:     KeyPositionTracker::key_buffer_index index;
andrewm@0: 
andrewm@0:     // Find the sample where the key position crosses the release threshold. What is returned
andrewm@0:     // will be the last sample which is above the threshold. What we need is the first sample
andrewm@0:     // below the threshold plus at least one more (SamplesNeededForReleaseVelocity...) to
andrewm@0:     // perform a local velocity calculation.
andrewm@0:     index = findMostRecentKeyPositionCrossing(releaseVelocityEscapementPosition_, true, 1000);
andrewm@0: 
andrewm@0:     if(index == 0) {
andrewm@0:         // Haven't crossed the threshold yet
andrewm@0:         releaseVelocityWaitingForThresholdCross_ = true;
andrewm@0:     }
andrewm@0:     else if(index + kPositionTrackerSamplesNeededForReleaseVelocityAfterEscapement + 1 <= mostRecentIndex) {
andrewm@0:         // Here, we already have the velocity information
andrewm@0:         std::cout << "release available, at index = " << keyBuffer_[index] << ", most recent position = " << keyBuffer_[mostRecentIndex] << std::endl;
andrewm@0:         currentlyAvailableFeatures_ |= KeyPositionTrackerNotification::kFeatureReleaseVelocity;
andrewm@0:         notifyFeature(KeyPositionTrackerNotification::kNotificationTypeFeatureAvailableReleaseVelocity, timestamp);
andrewm@0:         releaseVelocityWaitingForThresholdCross_ = false;
andrewm@0:     }
andrewm@0:     else {
andrewm@0:         // Otherwise, we need to send a notification when the information becomes available
andrewm@0:         std::cout << "release available at index " << index + kPositionTrackerSamplesNeededForReleaseVelocityAfterEscapement + 1 << std::endl;
andrewm@0:         releaseVelocityAvailableIndex_ = index + kPositionTrackerSamplesNeededForReleaseVelocityAfterEscapement + 1;
andrewm@0:         releaseVelocityWaitingForThresholdCross_ = false;
andrewm@0:     }
andrewm@0: }