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:   PianoKeyCalibrator.cpp: handles calibration of continuous key position data
andrewm@0: */
andrewm@0: 
andrewm@0: #include "PianoKeyCalibrator.h"
andrewm@0: 
andrewm@0: // Constructor
andrewm@0: PianoKeyCalibrator::PianoKeyCalibrator(bool pressValueGoesDown, key_position* warpTable)
andrewm@0: : status_(kPianoKeyNotCalibrated), prevStatus_(kPianoKeyNotCalibrated),
andrewm@0:   pressValueGoesDown_(pressValueGoesDown), history_(0), warpTable_(warpTable) {}
andrewm@0: 
andrewm@0: // Destructor
andrewm@0: PianoKeyCalibrator::~PianoKeyCalibrator() {
andrewm@0:     if(history_ != 0)
andrewm@0:         delete history_;
andrewm@0:     
andrewm@0: 	// warpTable_ is passed in externally-- don't delete it
andrewm@0: }
andrewm@0: 
andrewm@0: // Produce the calibrated value for a raw sample
andrewm@0: key_position PianoKeyCalibrator::evaluate(int rawValue) {
andrewm@0: 	key_position calibratedValue, calibratedValueDenominator;
andrewm@0: 
andrewm@0:     ScopedLock sl(calibrationMutex_);
andrewm@0: 	
andrewm@0: 	switch(status_) {
andrewm@0: 		case kPianoKeyCalibrated:
andrewm@0: 			if(missing_value<int>::isMissing(quiescent_) ||
andrewm@0: 			   missing_value<int>::isMissing(press_)) {
andrewm@0: 				return missing_value<key_position>::missing();
andrewm@0: 			}
andrewm@0: 			
andrewm@0: 			// Do the calculation either in integer or floating-point arithmetic
andrewm@0: 			calibratedValueDenominator = (key_position)(press_ - quiescent_);
andrewm@0: 			
andrewm@0: 			// Prevent divide-by-0 errors
andrewm@0: 			if(calibratedValueDenominator == 0)
andrewm@0: 				calibratedValue = missing_value<key_position>::missing();
andrewm@0: 			else {
andrewm@0:                 // Scale the value and clip it to a sensible range (for badly calibrated sensors)
andrewm@0: 				calibratedValue = (scale_key_position((rawValue - quiescent_))) / calibratedValueDenominator;
andrewm@0:                 if(calibratedValue < -0.5)
andrewm@0:                     calibratedValue = -0.5;
andrewm@0:                 if(calibratedValue > 1.2)
andrewm@0:                     calibratedValue = 1.2;
andrewm@0:             }
andrewm@0: 			
andrewm@0: 			if(warpTable_ != 0) {
andrewm@0: 				// TODO: warping
andrewm@0: 			}
andrewm@0: 			return calibratedValue;
andrewm@0: 		case kPianoKeyInCalibration:
andrewm@0: 			historyMutex_.enter();
andrewm@0: 
andrewm@0: 			// Add the sample to the calibration buffer, and wait until we have enough samples to do anything
andrewm@0: 			history_->push_back(rawValue);
andrewm@0: 			if(history_->size() < kPianoKeyCalibrationPressLength) {
andrewm@0: 				historyMutex_.exit();
andrewm@0: 				return missing_value<key_position>::missing();
andrewm@0: 			}
andrewm@0:             
andrewm@0: 			if(pressValueGoesDown_) {      // Pressed keys have a lower value than quiescent keys
andrewm@0: 				int currentAverage = averagePosition(kPianoKeyCalibrationPressLength);
andrewm@0:                 
andrewm@0: 				// Look for minimum overall value
andrewm@0: 				if(currentAverage < newPress_ || missing_value<int>::isMissing(newPress_)) {
andrewm@0: 					newPress_ = currentAverage;
andrewm@0:                 }
andrewm@0: 			}
andrewm@0: 			else {                          // Pressed keys have a higher value than quiescent keys
andrewm@0: 				int currentAverage = averagePosition(kPianoKeyCalibrationPressLength);
andrewm@0: 				
andrewm@0: 				// Look for maximum overall value
andrewm@0: 				if(currentAverage > newPress_ || missing_value<int>::isMissing(newPress_)) {
andrewm@0: 					newPress_ = currentAverage;
andrewm@0:                 }
andrewm@0: 			}
andrewm@0: 			
andrewm@0: 			// Don't return a value while calibrating
andrewm@0: 			historyMutex_.exit();
andrewm@0: 			return missing_value<key_position>::missing();
andrewm@0: 		case kPianoKeyNotCalibrated:	// Don't do anything
andrewm@0: 		default:
andrewm@0: 			return missing_value<key_position>::missing();
andrewm@0: 	}
andrewm@0: }
andrewm@0: 
andrewm@0: // Begin the calibrating process.
andrewm@0: void PianoKeyCalibrator::calibrationStart() {
andrewm@0: 	if(status_ == kPianoKeyInCalibration)	// Throw away the old results if we're already in progress
andrewm@0: 		calibrationAbort();					// This will clear the slate
andrewm@0: 	
andrewm@0:     historyMutex_.enter();
andrewm@0:     if(history_ != 0)
andrewm@0:         delete history_;
andrewm@0:     history_ = new boost::circular_buffer<int>(kPianoKeyCalibrationBufferSize);
andrewm@0:     historyMutex_.exit();
andrewm@0:     
andrewm@0: 	calibrationMutex_.enter();
andrewm@0:     newPress_ = quiescent_ = missing_value<int>::missing();
andrewm@0: 	changeStatus(kPianoKeyInCalibration);
andrewm@0: 	calibrationMutex_.exit();
andrewm@0: }
andrewm@0: 
andrewm@0: // Finish calibrating and accept the new results. Returns true if
andrewm@0: // calibration was successful; false if one or more values were missing
andrewm@0: // or if insufficient range is available.
andrewm@0: 
andrewm@0: bool PianoKeyCalibrator::calibrationFinish() {
andrewm@0:     bool updatedCalibration = false;
andrewm@0:     int oldQuiescent = quiescent_;
andrewm@0:     
andrewm@0: 	if(status_ != kPianoKeyInCalibration)
andrewm@0: 		return false;
andrewm@0:     
andrewm@0:     ScopedLock sl(calibrationMutex_);
andrewm@0:     
andrewm@0:     // Check that we were successfully able to update the quiescent value
andrewm@0:     // (should always be the case but this is a sanity check)
andrewm@0: 	bool updatedQuiescent = internalUpdateQuiescent();
andrewm@0:     
andrewm@0:     if(updatedQuiescent && abs(newPress_ - quiescent_) >= kPianoKeyCalibrationMinimumRange) {
andrewm@0:         press_ = newPress_;
andrewm@0:         changeStatus(kPianoKeyCalibrated);
andrewm@0:         updatedCalibration = true;
andrewm@0:     }
andrewm@0:     else {
andrewm@0:         quiescent_ = oldQuiescent;
andrewm@0:         
andrewm@0:         if(prevStatus_ == kPianoKeyCalibrated) {	// There may or may not have been valid data in press_ and quiescent_ before, depending on whether
andrewm@0:             changeStatus(kPianoKeyCalibrated);      // they were previously calibrated.
andrewm@0:         }
andrewm@0:         else {
andrewm@0:             changeStatus(kPianoKeyNotCalibrated);
andrewm@0:         }
andrewm@0:     }
andrewm@0:     
andrewm@0:     cleanup();
andrewm@0:     return updatedCalibration;
andrewm@0: }
andrewm@0: 
andrewm@0: // Finish calibrating without saving results
andrewm@0: void PianoKeyCalibrator::calibrationAbort() {
andrewm@0:     ScopedLock sl(calibrationMutex_);
andrewm@0: 	cleanup();
andrewm@0: 	if(prevStatus_ == kPianoKeyCalibrated) {	// There may or may not have been valid data in press_ and quiescent_ before, depending on whether
andrewm@0: 		changeStatus(kPianoKeyCalibrated);	// they were previously calibrated.
andrewm@0: 	}
andrewm@0: 	else {
andrewm@0: 		changeStatus(kPianoKeyNotCalibrated);
andrewm@0: 	}
andrewm@0: }
andrewm@0: 
andrewm@0: // Clear the existing calibration, reverting to an uncalibrated state
andrewm@0: void PianoKeyCalibrator::calibrationClear() {
andrewm@0: 	if(status_ == kPianoKeyInCalibration)
andrewm@0: 		calibrationAbort();
andrewm@0:     ScopedLock sl(calibrationMutex_);
andrewm@0: 	status_ = prevStatus_ = kPianoKeyNotCalibrated;
andrewm@0: }
andrewm@0: 
andrewm@0: // Generate new quiescent values without changing the press values
andrewm@0: void PianoKeyCalibrator::calibrationUpdateQuiescent() {
andrewm@0: 	calibrationStart();
andrewm@20: 	Thread::sleep(250);			// Wait 0.25 seconds for data to collect
andrewm@0: 	internalUpdateQuiescent();
andrewm@0: 	calibrationAbort();
andrewm@0: }
andrewm@0: 
andrewm@0: // Load calibration data from an XML string
andrewm@0: void PianoKeyCalibrator::loadFromXml(const XmlElement& baseElement) {
andrewm@0: 	// Abort any calibration in progress and reset to default values
andrewm@0: 	if(status_ == kPianoKeyInCalibration)
andrewm@0: 		calibrationAbort();
andrewm@0: 	calibrationClear();
andrewm@0: 	
andrewm@0: 	XmlElement *calibrationElement = baseElement.getChildByName("Calibration");
andrewm@0: 	
andrewm@0: 	if(calibrationElement != 0) {
andrewm@0:         if(calibrationElement->hasAttribute("quiescent") &&
andrewm@0:            calibrationElement->hasAttribute("press")) {
andrewm@0:             quiescent_ = calibrationElement->getIntAttribute("quiescent");
andrewm@0:             press_ = calibrationElement->getIntAttribute("press");
andrewm@0:             changeStatus(kPianoKeyCalibrated);
andrewm@0:         }
andrewm@0: 	}
andrewm@0: }
andrewm@0: 
andrewm@0: // Saves calibration data within the provided XML Element.  Child elements
andrewm@0: // will be added for each sequence.  Returns true if valid data was saved.
andrewm@0: bool PianoKeyCalibrator::saveToXml(XmlElement& baseElement) {
andrewm@0: 	if(status_ != kPianoKeyCalibrated)
andrewm@0: 		return false;
andrewm@0: 
andrewm@0:     XmlElement *newElement = baseElement.createNewChildElement("Calibration");
andrewm@0:     
andrewm@0:     if(newElement == 0)
andrewm@0:         return false;
andrewm@0:     
andrewm@0:     newElement->setAttribute("quiescent", quiescent_);
andrewm@0:     newElement->setAttribute("press", press_);
andrewm@0: 
andrewm@0: 	return true;
andrewm@0: }
andrewm@0: 
andrewm@0: // ***** Internal Methods *****
andrewm@0: 
andrewm@0: // Internal method to clean up after a calibration session.
andrewm@0: void PianoKeyCalibrator::cleanup() {
andrewm@0:     ScopedLock sl(historyMutex_);
andrewm@0:     if(history_ != 0)
andrewm@0:         delete history_;
andrewm@0:     history_ = 0;
andrewm@0:     newPress_ = missing_value<int>::missing();
andrewm@0: }
andrewm@0: 
andrewm@0: // This internal method actually calculates the new quiescent values.  Used by calibrationUpdateQuiescent()
andrewm@0: // and calibrationFinish(). Returns true if successful.
andrewm@0: bool PianoKeyCalibrator::internalUpdateQuiescent() {
andrewm@0:     ScopedLock sl(historyMutex_);
andrewm@0:     if(history_ == 0) {
andrewm@0:         return false;
andrewm@0:     }
andrewm@0:     if(history_->size() < kPianoKeyCalibrationPressLength) {
andrewm@0:         return false;
andrewm@0:     }
andrewm@0:     quiescent_  = averagePosition(kPianoKeyCalibrationBufferSize);
andrewm@0:     return true;
andrewm@0: }
andrewm@0: 
andrewm@0: // Get the average position of several samples in the buffer. 
andrewm@0: int PianoKeyCalibrator::averagePosition(int length) {
andrewm@0: 	boost::circular_buffer<int>::reverse_iterator rit = history_->rbegin();
andrewm@0: 	int count = 0, sum = 0;
andrewm@0: 	
andrewm@0: 	while(rit != history_->rend() && count < length) {
andrewm@0: 		sum += *rit++;
andrewm@0: 		count++;
andrewm@0: 	}
andrewm@0: 	
andrewm@0: 	if(count == 0) {
andrewm@0: 		return missing_value<int>::missing();
andrewm@0:     }
andrewm@0:     
andrewm@0:     return (int)(sum / count);
andrewm@0: }