view Source/TouchKeys/TouchkeyDevice.cpp @ 31:88287c1c2c92

Added an auxiliary MIDI input control, and moved the logging out of the window into the menu to make space in the GUI. Also updated the main window to be rescalable vertically for showing more mappings.

/*
  TouchKeys: multi-touch musical keyboard control software
  Copyright (c) 2013 Andrew McPherson

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.
 
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
  =====================================================================
 
  TouchkeyDevice.cpp: handles communication with the TouchKeys hardware
*/

#include <iomanip>
#include <stdio.h>
#include <stdlib.h>
#include "TouchkeyDevice.h"

const char* kKeyNames[13] = {"C ", "C#", "D ", "D#", "E ", "F ", "F#", "G ", "G#", "A ", "A#", "B ", "c "};

// Constructor

TouchkeyDevice::TouchkeyDevice(PianoKeyboard& keyboard) 
: keyboard_(keyboard),
#ifdef _MSC_VER
serialHandle_(INVALID_HANDLE_VALUE),
#else
device_(-1),
#endif
ioThread_(boost::bind(&TouchkeyDevice::runLoop, this, _1), "TouchKeyDevice::ioThread"),
rawDataThread_(boost::bind(&TouchkeyDevice::rawDataRunLoop, this, _1), "TouchKeyDevice::rawDataThread"),
autoGathering_(false), shouldStop_(false), sendRawOscMessages_(false),
verbose_(0), numOctaves_(0), lowestMidiNote_(48), lowestKeyPresentMidiNote_(48),
updatedLowestMidiNote_(48), deviceSoftwareVersion_(-1), deviceHardwareVersion_(-1),
expectedLengthWhite_(kTransmissionLengthWhiteNewHardware),
expectedLengthBlack_(kTransmissionLengthBlackNewHardware), deviceHasRGBLEDs_(false),
ledThread_(boost::bind(&TouchkeyDevice::ledUpdateLoop, this, _1), "TouchKeyDevice::ledThread"),
isCalibrated_(false), calibrationInProgress_(false),
keyCalibrators_(0), keyCalibratorsLength_(0), sensorDisplay_(0)
{
    // Tell the piano keyboard class how to call us back
    keyboard_.setTouchkeyDevice(this);
	
	// Initialize the frame -> timestamp synchronization.  Frame interval is nominally 1ms,
	// but this class helps us find the actual rate which might drift slightly, and it keeps
	// the time stamps of each data point in sync with other streams.
	timestampSynchronizer_.initialize(Time::getMillisecondCounterHiRes(), keyboard_.schedulerCurrentTimestamp());
	timestampSynchronizer_.setNominalSampleInterval(.001);
	timestampSynchronizer_.setFrameModulus(65536);
    
    for(int i = 0; i < 4; i++)
        analogLastFrame_[i] = 0;
    
    logFileCreated_ = false;
    loggingActive_ = false;
}


// ------------------------------------------------------
// create a new MIDI log file, ready to have data written to it
void TouchkeyDevice::createLogFiles(string keyTouchLogFilename, string analogLogFilename, string path)
{
    if (path.compare("") != 0)
    {
        path = path + "/";
    }
    
    keyTouchLogFilename = path + keyTouchLogFilename;
    analogLogFilename = path + analogLogFilename;
    
    char *fileName = (char*)keyTouchLogFilename.c_str();
    
    // create output file for key touch
    keyTouchLog_.open (fileName, ios::out | ios::binary);
    keyTouchLog_.seekp(0);

    fileName = (char*)analogLogFilename.c_str();
    
    // create output file for analog data
    analogLog_.open (fileName, ios::out | ios::binary);
    analogLog_.seekp(0);
    
    // indicate that we have created a log file (so we can close it later)
    logFileCreated_ = true;
}

// ------------------------------------------------------
// close the log file
void TouchkeyDevice::closeLogFile()
{
    if (logFileCreated_)
    {
        keyTouchLog_.close();
        analogLog_.close();
        logFileCreated_ = false;
    }
    loggingActive_ = false;
}

// ------------------------------------------------------
// start logging midi data
void TouchkeyDevice::startLogging()
{
    loggingActive_ = true;
}

// ------------------------------------------------------
// stop logging midi data
void TouchkeyDevice::stopLogging()
{
    loggingActive_ = false;
}

// Open the touchkey device (a USB CDC device).  Returns true on success.

bool TouchkeyDevice::openDevice(const char * inputDevicePath) {
	// If the device is already open, close it
	if(isOpen())
		closeDevice();
	
	// Open the device
#ifdef _MSC_VER
	// Open the serial port
	serialHandle_ = CreateFile(inputDevicePath, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
	if(serialHandle_ == INVALID_HANDLE_VALUE) {
		Logger::writeToLog("Unable to open serial port " + String(inputDevicePath));
		return false;
	}

	// Set some serial parameters, though they don't actually affect the operation
	// of the port since it is all native USB
	DCB serialParams = { 0 };
	serialParams.DCBlength = sizeof(serialParams);

	if(!BuildCommDCBA("baud=1000000 data=8 parity=N stop=1 dtr=on rts=on", &serialParams)) {
		Logger::writeToLog("Unable to create port settings\n");
		CloseHandle(serialHandle_);
		serialHandle_ = INVALID_HANDLE_VALUE;
		return false;
	}

	if(!SetCommState(serialHandle_, &serialParams)) {
		Logger::writeToLog("Unable to set comm state\n");
		CloseHandle(serialHandle_);
		serialHandle_ = INVALID_HANDLE_VALUE;
		return false;
	}

	// Set timeouts
	COMMTIMEOUTS timeout = { 0 };
	timeout.ReadIntervalTimeout = MAXDWORD;
	timeout.ReadTotalTimeoutConstant = 0;
	timeout.ReadTotalTimeoutMultiplier = 0;
	timeout.WriteTotalTimeoutConstant = 0;
	timeout.WriteTotalTimeoutMultiplier = 0;

	if(!SetCommTimeouts(serialHandle_, &timeout)) {
		Logger::writeToLog("Unable to set timeouts\n");
		CloseHandle(serialHandle_);
		serialHandle_ = INVALID_HANDLE_VALUE;
		return false;
	}
#else
	device_ = open(inputDevicePath, O_RDWR | O_NOCTTY | O_NDELAY);

	if(device_ < 0) {
		return false;
    }
#endif
	return true;
}

// Close the touchkey serial device
void TouchkeyDevice::closeDevice() {
	if(!isOpen())
		return;
	
	stopAutoGathering();
	keysPresent_.clear();

#ifdef _MSC_VER
	CloseHandle(serialHandle_);
	serialHandle_ = INVALID_HANDLE_VALUE;
#else
	close(device_);
    device_ = -1;
#endif
}

bool TouchkeyDevice::isOpen() {
#ifdef _MSC_VER
	return serialHandle_ != INVALID_HANDLE_VALUE;
#else
	return device_ >= 0; 
#endif
}

// Check if the device is present and ready to respond.  If status is not null, store the current
// controller status information.

bool TouchkeyDevice::checkIfDevicePresent(int millisecondsToWait) {
	//struct timeval startTime, currentTime;
    double startTime, currentTime;
	unsigned char ch;
	bool controlSeq = false, startingFrame = false;
    
	if(!isOpen())
		return false;
    deviceFlush(false);
    
	if(deviceWrite((char*)kCommandStatus, 5) < 0) {	// Write status command
        if(verbose_ >= 1)
            cout << "ERROR: unable to write status command.  errno = " << errno << endl;
		return false;
	}	


	// Wait the specified amount of time for a response before giving up
    startTime = Time::getMillisecondCounterHiRes();
    currentTime = startTime;

	while(currentTime - startTime < (double)millisecondsToWait) {
        long count = deviceRead((char *)&ch, 1);

		if(count < 0) {				// Check if an error occurred on read
			if(errno != EAGAIN) {
                if(verbose_ >= 1)
                    cout << "Unable to read from device (error " << errno << ").  Aborting.\n";
				return false;
			}
		}
		else if(count > 0) {		// Data received
			// Wait for a frame back that is of type status.  We don't even care what the 
			// status is at this point, just that we got something.
			
			if(controlSeq) {
				controlSeq = false;
				if(ch == kControlCharacterFrameBegin)
					startingFrame = true;
                else
                    startingFrame = false;
			}
			else {
				if(ch == ESCAPE_CHARACTER) {
					controlSeq = true;
                }
				else if(startingFrame) {
					if(ch == kFrameTypeStatus) {
						ControllerStatus status;
						unsigned char statusBuf[TOUCHKEY_MAX_FRAME_LENGTH];
						int statusBufLength = 0;
						bool frameError = false;
						
						// Gather and parse the status frame
						
						while(currentTime - startTime < millisecondsToWait) {
							count = deviceRead((char *)&ch, 1);
						
							if(count == 0)
								continue;
							if(count < 0) {
								if(errno != EAGAIN && verbose_ >= 1) {	// EAGAIN just means no data was available
                                    if(verbose_ >= 1)
                                        cout << "Unable to read from device (error " << errno << ").  Aborting.\n";
									return false;
								}
								
								continue;
							}
							
							if(controlSeq) {
								controlSeq = false;
								if(ch == kControlCharacterFrameEnd)	{		// frame finished?
									break;
                                }
								else if(ch == kControlCharacterFrameError)	// device telling us about an internal comm error
									frameError = true;
								else if(ch == ESCAPE_CHARACTER) {			// double-escape means a literal escape character
									statusBuf[statusBufLength++] = (unsigned char)ch;
									if(statusBufLength >= TOUCHKEY_MAX_FRAME_LENGTH) {
										frameError = true;
										break;
									}				
								}
								else if(ch == kControlCharacterNak && verbose_ >= 1) {
									cout << "Warning: received NAK\n";
								}			
							}
							else {
								if(ch == ESCAPE_CHARACTER)
									controlSeq = true;
								else {
									statusBuf[statusBufLength++] = (unsigned char)ch;
									if(statusBufLength >= TOUCHKEY_MAX_FRAME_LENGTH) {
										frameError = true;
										break;
									}
								}
							}
							
							currentTime = Time::getMillisecondCounterHiRes();
						}
						
						if(frameError) {
                            if(verbose_ >= 1)
                                cout << "Warning: device present, but frame error received trying to get status.\n";
						}
						else if(processStatusFrame(statusBuf, statusBufLength, &status)) {
							// Clear keys present in preparation to read new list of keys
							keysPresent_.clear();
							
							numOctaves_ = status.octaves;
                            deviceSoftwareVersion_ = status.softwareVersionMajor;
                            deviceHardwareVersion_ = status.hardwareVersion;
                            deviceHasRGBLEDs_ = status.hasRGBLEDs;
                            lowestKeyPresentMidiNote_ = 127;
							
							if(verbose_ >= 1) {
								cout << endl << "Found Device: Hardware Version " << status.hardwareVersion;
								cout << " Software Version " << status.softwareVersionMajor << "." << status.softwareVersionMinor;
								cout << endl << "  " << status.octaves << " octaves connected" << endl;
							}
                            
							for(int i = 0; i < status.octaves; i++) {
								bool foundKey = false;
								
								if(verbose_ >= 1) cout << "  Octave " << i << ": ";
								for(int j = 0; j < 13; j++) {
									if(status.connectedKeys[i] & (1<<j)) {
										if(verbose_ >= 1) cout << kKeyNames[j] << " ";
										keysPresent_.insert(octaveNoteToIndex(i, j));
										foundKey = true;
                                        if(octaveKeyToMidi(i, j) < lowestKeyPresentMidiNote_)
                                            lowestKeyPresentMidiNote_ = octaveKeyToMidi(i, j);
									}
									else {
										if(verbose_ >= 1) cout << "-  ";
									}

								}

								if(verbose_ >= 1) cout << endl;
							}
                            
                            // Hardware version determines whether all keys have XY or not
                            if(status.hardwareVersion >= 2) {
                                expectedLengthWhite_ = kTransmissionLengthWhiteNewHardware;
                                expectedLengthBlack_ = kTransmissionLengthBlackNewHardware;
                                whiteMaxX_ = kWhiteMaxXValueNewHardware;
                                whiteMaxY_ = kWhiteMaxYValueNewHardware;
                                blackMaxX_ = kBlackMaxXValueNewHardware;
                                blackMaxY_ = kBlackMaxYValueNewHardware;
                            }
                            else {
                                expectedLengthWhite_ = kTransmissionLengthWhiteOldHardware;
                                expectedLengthBlack_ = kTransmissionLengthBlackOldHardware;
                                whiteMaxX_ = kWhiteMaxXValueOldHardware;
                                whiteMaxY_ = kWhiteMaxYValueOldHardware;
                                blackMaxX_ = 1.0; // irrelevant -- no X data
                                blackMaxY_ = kBlackMaxYValueOldHardware;
                            }
                            
                            // Software version indicates what information is available. On version
                            // 2 and greater, can indicate which is lowest sensor available. Might
                            // be different from lowest connected key.
                            if(status.softwareVersionMajor >= 2) {
                                lowestKeyPresentMidiNote_ = octaveKeyToMidi(0, status.lowestHardwareNote);
                            }
                            else if(lowestKeyPresentMidiNote_ == 127) // No keys found and old device software
                                lowestKeyPresentMidiNote_ = lowestMidiNote_;
   
                            keyboard_.setKeyboardGUIRange(lowestKeyPresentMidiNote_, lowestMidiNote_ + 12*numOctaves_);
                            calibrationInit(12*numOctaves_ + 1); // One more for the top C
						}
						else {
							if(verbose_ >= 1) cout << "Warning: device present, but received invalid status frame.\n";
                            deviceFlush(true);
							return false;					// Yes... found the device
						}

                        deviceFlush(true);
						return true;					// Yes... found the device
					}
				}
                
                startingFrame = false;
			}
		}
	
		currentTime = Time::getMillisecondCounterHiRes();
	}
	
	return false;
}

// Start a run loop thread to receive centroid data.  Returns true
// on success.

bool TouchkeyDevice::startAutoGathering() {
    // Can only start if the device is open
	if(!isOpen())
		return false;
    // Already running?
	if(autoGathering_)
		return true;
	shouldStop_ = false;
    ledShouldStop_ = false;
	
	if(verbose_ >= 1)
		cout << "Starting auto centroid collection\n";
	
    // Start the data input and LED threads
    ioThread_.startThread();
    ledThread_.startThread();
	autoGathering_ = true;
    
    // Tell the device to start scanning for new data
	if(deviceWrite((char*)kCommandStartScanning, 5) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write startAutoGather command.  errno = " << errno << endl;
	}

	keyboard_.sendMessage("/touchkeys/allnotesoff", "", LO_ARGS_END);
	if(keyboard_.gui() != 0) {
		// Update display: touch sensing enabled, which keys connected, no current touches
		keyboard_.gui()->setTouchSensingEnabled(true);
		for(set<int>::iterator it = keysPresent_.begin(); it != keysPresent_.end(); ++it) {
			keyboard_.gui()->setTouchSensorPresentForKey(octaveKeyToMidi(indexToOctave(*it), indexToNote(*it)), true);
		}
		keyboard_.gui()->clearAllTouches();
        keyboard_.gui()->clearAnalogData();
	}

	return true;
}

// Stop the run loop if applicable
void TouchkeyDevice::stopAutoGathering(bool writeStopCommandToDevice) {
    // Check if actually running
	if(!autoGathering_ || !isOpen())
		return;
    // Stop any calibration in progress
    calibrationAbort();	
    
    if(writeStopCommandToDevice) {
        // Tell device to stop scanning
        if(deviceWrite((char*)kCommandStopScanning, 5) < 0) {
            if(verbose_ >= 1)
                cout << "ERROR: unable to write stopAutoGather command.  errno = " << errno << endl;
        }
    }
	
    // Setting this to true tells the run loop to exit what it's doing
	shouldStop_ = true;
    ledShouldStop_ = true;
	
	if(verbose_ >= 1)
		cout << "Stopping auto centroid collection\n";
	
    // Wait for run loop thread to finish. Set a timeout in case there's
    // some sort of device hangup
    if(ioThread_.getThreadId() != Thread::getCurrentThreadId())
        if(ioThread_.isThreadRunning())
            ioThread_.stopThread(3000);
    if(ledThread_.getThreadId() != Thread::getCurrentThreadId())
        if(ledThread_.isThreadRunning())
            ledThread_.stopThread(3000);
    if(rawDataThread_.getThreadId() != Thread::getCurrentThreadId())
        if(rawDataThread_.isThreadRunning())
            rawDataThread_.stopThread(3000);
	
    // Stop any currently playing notes
	keyboard_.sendMessage("/touchkeys/allnotesoff", "", LO_ARGS_END);
	
	// Clear touch for all keys
	//std::pair<int, int> keyboardRange = keyboard_.keyboardRange();
	//for(int i = keyboardRange.first; i <= keyboardRange.second; i++)
    for(int i = 0; i <= 127; i++)
        if(keyboard_.key(i) != 0)
            keyboard_.key(i)->touchOff(lastTimestamp_);
								   
	if(keyboard_.gui() != 0) {
		// Update display: touch sensing disabled
		keyboard_.gui()->clearAllTouches();		
		keyboard_.gui()->setTouchSensingEnabled(false);
        keyboard_.gui()->clearAnalogData();
	}
	
	if(verbose_ >= 2)
		cout << "...done.\n";

	autoGathering_ = false;
}

// Begin raw data collection from a given single key

bool TouchkeyDevice::startRawDataCollection(int octave, int key, int mode, int scaler) {
	if(!isOpen())
		return false;
	
	stopAutoGathering();	// Stop the thread if it's running	
    
    // Account for the high C which is ID 12 on the top octave
    if(octave == numOctaves_ && key == 0) {
        octave--;
        key = 12;
    }
    
    rawDataCurrentOctave_ = octave;
    rawDataCurrentKey_ = key;
    rawDataCurrentMode_ = mode;
    rawDataCurrentScaler_ = scaler;
    rawDataShouldChangeMode_ = true;
    
	shouldStop_ = false;
    rawDataThread_.startThread();
    
	if(verbose_ >= 1)
		cout << "Starting raw data collection from octave " << octave << ", key " << key << endl;
	
	autoGathering_ = true;
	
	return true;
}

void TouchkeyDevice::rawDataChangeKeyAndMode(int octave, int key, int mode, int scaler) {
    // Account for the high C which is ID 12 on the top octave
    if(octave == numOctaves_ && key == 0) {
        octave--;
        key = 12;
    }
    
    rawDataCurrentOctave_ = octave;
    rawDataCurrentKey_ = key;
    rawDataCurrentMode_ = mode;
    rawDataCurrentScaler_ = scaler;
    rawDataShouldChangeMode_ = true;
}

// Set the scan interval in milliseconds.  Returns true on success.

bool TouchkeyDevice::setScanInterval(int intervalMilliseconds) {
	if(!isOpen())
		return false;	
	if(intervalMilliseconds <= 0 || intervalMilliseconds > 255)
		return false;
	
	unsigned char command[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin,
		kFrameTypeScanRate, (unsigned char)(intervalMilliseconds & 0xFF), ESCAPE_CHARACTER, kControlCharacterFrameEnd};
	
	// Send command
	if(deviceWrite((char*)command, 6) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write startRawDataCollection command.  errno = " << errno << endl;
	}
	
	if(verbose_ >= 2)
		cout << "Setting scan interval to " << intervalMilliseconds << endl;
	
	// Return value depends on ACK or NAK received
	return checkForAck(250);
}

// Key parameters.  Setting octave or key to -1 means all octaves or all keys, respectively.
// This controls the sensitivity of the capacitive touch sensing system on each key.
// It is a balance between achieving the best range of data and not saturating the sensors
// for the largest touches.
bool TouchkeyDevice::setKeySensitivity(int octave, int key, int value) {
	unsigned char chOctave, chKey, chVal;
	
	if(!isOpen())
		return false;
	if(octave > 255)
		return false;
	if(key > 12)
		return false;
	if(value > 255 || value < 0)
		return false;
	if(octave < 0)
		chOctave = 0xFF;
	else 
		chOctave = (unsigned char)(octave & 0xFF);
	if(key < 0)
		chKey = 0xFF;
	else
		chKey = (unsigned char)(key & 0xFF);
	chVal = (unsigned char)value;
	
	unsigned char command[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin, kFrameTypeSensitivity,
		chOctave, chKey, chVal, ESCAPE_CHARACTER, kControlCharacterFrameEnd};
	
	// Send command
	if(deviceWrite((char*)command, 8) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write setKeySensitivity command.  errno = " << errno << endl;
	}

	if(verbose_ >= 2)
		cout << "Setting sensitivity to " << value << endl;
	
	// Return value depends on ACK or NAK received
	return checkForAck(250);	
}

// Change how the calculated centroids are scaled to fit in a single byte. They
// will be right-shifted by the indicated number of bits before being transmitted.
bool TouchkeyDevice::setKeyCentroidScaler(int octave, int key, int value) {
	unsigned char chOctave, chKey, chVal;
	
	if(!isOpen())
		return false;	
	if(octave > 255)
		return false;
	if(key > 12)
		return false;
	if(value > 7 || value < 0)
		return false;
	if(octave < 0)
		chOctave = 0xFF;
	else 
		chOctave = (unsigned char)(octave & 0xFF);
	if(key < 0)
		chKey = 0xFF;
	else
		chKey = (unsigned char)(key & 0xFF);
	chVal = (unsigned char)value;
	
	unsigned char command[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin, kFrameTypeSizeScaler,
		chOctave, chKey, chVal, ESCAPE_CHARACTER, kControlCharacterFrameEnd};
	
	// Send command
	if(deviceWrite((char*)command, 8) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write setKeyCentroidScaler command.  errno = " << errno << endl;
	}
    
	if(verbose_ >= 2)
		cout << "Setting size scaler to " << value << endl;
	
	// Return value depends on ACK or NAK received
	return checkForAck(250);
}

// Set the minimum size of a centroid calculated on the key which is considered
// "real" and not noise.
bool TouchkeyDevice::setKeyMinimumCentroidSize(int octave, int key, int value) {
	unsigned char chOctave, chKey, chValHi, chValLo;
	
	if(!isOpen())
		return false;	
	if(octave > 255)
		return false;
	if(key > 12)
		return false;
	if(value > 0xFFFF || value < 0)
		return false;
	if(octave < 0)
		chOctave = 0xFF;
	else 
		chOctave = (unsigned char)(octave & 0xFF);
	if(key < 0)
		chKey = 0xFF;
	else
		chKey = (unsigned char)(key & 0xFF);
	chValHi = (unsigned char)((value >> 8) & 0xFF);
	chValLo = (unsigned char)(value & 0xFF);
	
	unsigned char command[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin, kFrameTypeMinimumSize,
		chOctave, chKey, chValHi, chValLo, ESCAPE_CHARACTER, kControlCharacterFrameEnd};
	
	// Send command
	if(deviceWrite((char*)command, 9) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write setKeyMinimumCentroidSize command.  errno = " << errno << endl;
	}

	if(verbose_ >= 2)
		cout << "Setting minimum centroid size to " << value << endl;
	
	// Return value depends on ACK or NAK received
	return checkForAck(250);	
}

// Set the noise threshold for individual sensor pads: the reading must exceed
// the background value by this amount to be considered an actual touch.
bool TouchkeyDevice::setKeyNoiseThreshold(int octave, int key, int value) {
	unsigned char chOctave, chKey, chVal;
	
	if(octave > 255)
		return false;
	if(key > 12)
		return false;
	if(value > 255 || value < 0)
		return false;
	if(octave < 0)
		chOctave = 0xFF;
	else 
		chOctave = (unsigned char)(octave & 0xFF);
	if(key < 0)
		chKey = 0xFF;
	else
		chKey = (unsigned char)(key & 0xFF);
	chVal = (unsigned char)value;
	
	unsigned char command[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin, kFrameTypeNoiseThreshold,
		chOctave, chKey, chVal, ESCAPE_CHARACTER, kControlCharacterFrameEnd};
	
	// Send command
	if(deviceWrite((char*)command, 8) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write setKeyNoiseThreshold command.  errno = " << errno << endl;
	}

	if(verbose_ >= 2)
		cout << "Setting noise threshold to " << value << endl;
	
	// Return value depends on ACK or NAK received
	return checkForAck(250);	
}

// Update the baseline sensor values on the given key
bool TouchkeyDevice::setKeyUpdateBaseline(int octave, int key) {
    unsigned char baselineCommand[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin,
        kFrameTypeSendI2CCommand, (unsigned char)octave, (unsigned char)key,
        2 /* xmit */, 0 /* response */, 0 /* command offset */, 6 /* baseline update */,
        ESCAPE_CHARACTER, kControlCharacterFrameEnd};
    
    // Send command
	if(deviceWrite((char*)baselineCommand, 11) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write baseline update command.  errno = " << errno << endl;
	}

	if(verbose_ >= 2)
		cout << "Updating baseline on octave " << octave << " key " << key << endl;
	
    checkForAck(100);
    
    unsigned char commandPrepareRead[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin,
        kFrameTypeSendI2CCommand, (unsigned char)octave, (unsigned char)key,
        1 /* xmit */, 0 /* response */, 6 /* data offset */,
        ESCAPE_CHARACTER, kControlCharacterFrameEnd};
    
	if(deviceWrite((char*)commandPrepareRead, 10) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write prepareRead command.  errno = " << errno << endl;
	}

	// Return value depends on ACK or NAK received
	return checkForAck(100);
}

// Jump to the built-in bootloader of the TouchKeys device
void TouchkeyDevice::jumpToBootloader() {
	unsigned char command[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin, kFrameTypeEnterSelfProgramMode,
		ESCAPE_CHARACTER, kControlCharacterFrameEnd};
	
	// Send command
	if(deviceWrite((char*)command, 5) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write jumpToBootloader command.  errno = " << errno << endl;
	}
}

// Set the LED color for the given MIDI note (if RGB LEDs are present). This method
// does not directly communicate with the device, but it schedules an update to take
// place in the relevant thread.
void TouchkeyDevice::rgbledSetColor(const int midiNote, const float red, const float green, const float blue) {
    RGBLEDUpdate updateStructure;
    
    updateStructure.allLedsOff = false;
    updateStructure.midiNote = midiNote;
    
    // Convert 0-1 floating point range to 0-255
    updateStructure.red = (int)(red * 4095.0);
    updateStructure.green = (int)(green * 4095.0);
    updateStructure.blue = (int)(blue * 4095.0);
    
    ledUpdateQueue_.push_front(updateStructure);
}

// Same as rgbledSetColor() but uses HSV format color instead of RGB
void TouchkeyDevice::rgbledSetColorHSV(const int midiNote, const float hue, const float saturation, const float value) {
    float red = 0, green = 0, blue = 0;
    float chroma = value * saturation;
    
    // Hue will lie on one of 6 segments from 0 to 1; convert this from 0 to 6.
    float hueSegment = hue * 6.0;
    float x = chroma * (1.0 - fabsf(fmodf(hueSegment, 2.0) - 1.0));
    
    if(hueSegment < 1.0) {
        red = chroma;
        green = x;
        blue = 0;
    }
    else if(hueSegment < 2.0) {
        red = x;
        green = chroma;
        blue = 0;
    }
    else if(hueSegment < 3.0) {
        red = 0;
        green = chroma;
        blue = x;
    }
    else if(hueSegment < 4.0) {
        red = 0;
        green = x;
        blue = chroma;
    }
    else if(hueSegment < 5.0) {
        red = x;
        green = 0;
        blue = chroma;
    }
    else {
        red = chroma;
        green = 0;
        blue = x;
    }

    rgbledSetColor(midiNote, red, green, blue);
}

// Set all RGB LEDs off (if RGB LEDs are present). This method does not
// directly communicate with the device, but it schedules an update to take
// place in the relevant thread.
void TouchkeyDevice::rgbledAllOff() {
    RGBLEDUpdate updateStructure;
    
    updateStructure.allLedsOff = true;
    updateStructure.midiNote = 0;
    updateStructure.red = 0;
    updateStructure.green = 0;
    updateStructure.blue = 0;
    
    ledUpdateQueue_.push_front(updateStructure);
}

// Set the color of a given RGB LED (piano scanner boards only). LEDs are numbered from 0-24
// starting at left. Boards are numbered 0-3 starting at left.
bool TouchkeyDevice::internalRGBLEDSetColor(const int device, const int led, const int red, const int green, const int blue) {
	if(!isOpen())
		return false;
    if(!deviceHasRGBLEDs_)
        return false;
    if(device < 0 || device > 3)
        return false;
    if(led < 0 || led > 24)
        return false;
    if(red < 0 || red > 4095)
        return false;
    if(green < 0 || green > 4095)
        return false;
    if(blue < 0 || blue > 4095)
        return false;
    
    unsigned char command[17]; // 11 bytes + possibly 6 doubled characters
    
    // There's a chance that one of the bytes will come out to ESCAPE_CHARACTER (0xFE) depending
    // on LED color. We need to double up any bytes that come in that way.
    
    command[0] = ESCAPE_CHARACTER;
    command[1] = kControlCharacterFrameBegin;
    command[2] = kFrameTypeRGBLEDSetColors;
    
    int byte, location = 3;
    
    byte = (((unsigned char)device & 0xFF) << 6) | (unsigned char)led;
    command[location++] = byte;
    if(byte == ESCAPE_CHARACTER)
        command[location++] = byte;
    byte = (red >> 4) & 0xFF;
    command[location++] = byte;
    if(byte == ESCAPE_CHARACTER)
        command[location++] = byte;
    byte = ((red << 4) & 0xF0) | ((green >> 8) & 0x0F);
    command[location++] = byte;
    if(byte == ESCAPE_CHARACTER)
        command[location++] = byte;
    byte = (green & 0xFF);
    command[location++] = byte;
    if(byte == ESCAPE_CHARACTER)
        command[location++] = byte;
    byte = (blue >> 4) & 0xFF;
    command[location++] = byte;
    if(byte == ESCAPE_CHARACTER)
        command[location++] = byte;
    byte = (blue << 4) & 0xF0;
    command[location++] = byte;
    if(byte == ESCAPE_CHARACTER)
        command[location++] = byte;
    command[location++] = ESCAPE_CHARACTER;
    command[location++] = kControlCharacterFrameEnd;
    
	// Send command
	if(deviceWrite((char*)command, location) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write setRGBLEDColor command.  errno = " << errno << endl;
	}
	
	if(verbose_ >= 3)
		cout << "Setting RGB LED color for device " << device << ", led " << led << endl;
        
	// Return value depends on ACK or NAK received
	return true; //checkForAck(20);
}

// Turn off all RGB LEDs on a given board
bool TouchkeyDevice::internalRGBLEDAllOff() {
	if(!isOpen())
		return false;
    if(!deviceHasRGBLEDs_)
        return false;
    
    unsigned char command[5];
    
    command[0] = ESCAPE_CHARACTER;
    command[1] = kControlCharacterFrameBegin;
    command[2] = kFrameTypeRGBLEDAllOff;
    command[3] = ESCAPE_CHARACTER;
    command[4] = kControlCharacterFrameEnd;
	
	// Send command
	if(deviceWrite((char*)command, 5) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write setRGBLEDAllOff command.  errno = " << errno << endl;
	}
    
	if(verbose_ >= 3)
		cout << "Turning off all RGB LEDs" << endl;
    
	// Return value depends on ACK or NAK received
	return true; //checkForAck(20);
}

// Get board number for MIDI note
int TouchkeyDevice::internalRGBLEDMIDIToBoardNumber(const int midiNote) {
    // lowestMidiNote_ holds the very bottom LED on the bottom board. The boards
    // go up by two-octave sets from there. The top board has one extra LED (high C).
    
    if(midiNote > lowestMidiNote_ + 96)
        return -1;
    if(midiNote >= lowestMidiNote_ + 72)
        return 3;
    else if(midiNote >= lowestMidiNote_ + 48)
        return 2;
    else if(midiNote >= lowestMidiNote_ + 24)
        return 1;
    else if(midiNote >= lowestMidiNote_)
        return 0;
    return -1;
}

// Get LED number for MIDI note (within a board)
int TouchkeyDevice::internalRGBLEDMIDIToLEDNumber(const int midiNote) {
    // Take the note number relative to the lowest note of the whole device.
    // Once it's located within a board (2-octaves each), the offset gives us the LED number.
    // However, the lowest board works differently as it only has 15 LEDs which start at A,
    // not at C.
    const int midiNoteOffset = midiNote - lowestMidiNote_;
    
    if(midiNoteOffset < 9) // Below the bottom A, hence invalid
        return -1;
    if(midiNoteOffset < 24) {
        // Within 2 octaves of bottom --> lowest board --> adjust for 15 LEDs on board
        return midiNoteOffset - 9;
    }
    else if(midiNoteOffset < 48) {
        // Board 1
        return midiNoteOffset - 24;
    }
    else if(midiNoteOffset < 72) {
        // Board 2
        return midiNoteOffset - 48;
    }
    else if(midiNoteOffset < 97) {
        // Board 3 includes a top C (index 24)
        return midiNoteOffset - 72;
    }
    return -1;
}

// ***** Calibration Methods *****

// Start calibrating selected keys and pedals. If argument is NULL, assume it applies to all keys.
void TouchkeyDevice::calibrationStart(std::vector<int>* keysToCalibrate) {
	if(keysToCalibrate == 0) {
		for(int i = 0; i < keyCalibratorsLength_; i++)
			keyCalibrators_[i]->calibrationStart();
	}
	else {
		std::vector<int>::iterator it;
		for(it = keysToCalibrate->begin(); it != keysToCalibrate->end(); it++) {
			if(*it >= 0 && *it < keyCalibratorsLength_)
				keyCalibrators_[*it]->calibrationStart();
		}
	}
	
	calibrationInProgress_ = true;
}

// Finish the current calibration in progress.  Pass it on to all Calibrators, and the ones that weren't
// calibrating will just ignore it.
void TouchkeyDevice::calibrationFinish() {
    bool calibratedAtLeastOneKey = false;
    
	for(int i = 0; i < keyCalibratorsLength_; i++) {
        // Check if calibration was successful
		if(keyCalibrators_[i]->calibrationFinish()) {
            calibratedAtLeastOneKey = true;
            // Update the display if available
            if(keyboard_.gui() != 0) {
                keyboard_.gui()->setAnalogCalibrationStatusForKey(i + lowestMidiNote_, true);
            }
        }
    }
	
	calibrationInProgress_ = false;
	isCalibrated_ = calibratedAtLeastOneKey;
}

// Abort a calibration in progress, without saving its results. Pass it on to all Calibrators.
void TouchkeyDevice::calibrationAbort() {
	for(int i = 0; i < keyCalibratorsLength_; i++)
		keyCalibrators_[i]->calibrationAbort();
	
	calibrationInProgress_ = false;
}

// Clear the existing calibration, reverting to an uncalibrated state.
void TouchkeyDevice::calibrationClear() {
	for(int i = 0; i < keyCalibratorsLength_; i++) {
		keyCalibrators_[i]->calibrationClear();
        if(keyboard_.gui() != 0) {
            keyboard_.gui()->setAnalogCalibrationStatusForKey(i + lowestMidiNote_, false);
        }
    }

	calibrationInProgress_ = false;
	isCalibrated_ = false;
}

// Save calibration data to a file
bool TouchkeyDevice::calibrationSaveToFile(std::string const& filename) {
	int i;
	
	if(!isCalibrated()) {
		std::cerr << "TouchKeys not calibrated, so can't save calibration data.\n";
		return false;
	}
	
	// Create an XML structure and save it to file.
	try {
		XmlElement baseElement("TouchkeyDeviceCalibration");
		bool savedValidData = false;
		
		for(i = 0; i < keyCalibratorsLength_; i++) {
            XmlElement *calibrationElement = baseElement.createNewChildElement("Key");
            if(calibrationElement == 0)
                continue;
            
			calibrationElement->setAttribute("id", i);
			
			// Tell each individual calibrator to add its data to the XML tree
			if(keyCalibrators_[i]->saveToXml(*calibrationElement)) {
				savedValidData = true;
			}
		}
		
		if(!savedValidData) {
			std::cerr << "TouchkeyDevice: unable to find valid calibration data to save.\n";
			throw 1;
		}
		
		// Now save the generated tree to a file
        
		if(!baseElement.writeToFile(File(filename.c_str()), "")) {
			std::cerr << "TouchkeyDevice: could not write calibration file " << filename << "\n";
			throw 1;
		}
		
		//lastCalibrationFile_ = filename;
	}
	catch(...) {
		return false;
	}
	
	return true;
}

// Load calibration from a file
bool TouchkeyDevice::calibrationLoadFromFile(std::string const& filename) {
	//int i, j;
    
	calibrationClear();
	
	// Open the file and read the new values
	try {
        XmlDocument doc(File(filename.c_str()));
		XmlElement *baseElement = doc.getDocumentElement();
        XmlElement *deviceCalibrationElement, *calibratorElement;
		
		if(baseElement == 0) {
			std::cerr << "TouchkeyDevice: unable to load patch table file: \"" << filename << "\". Error was:\n";
			std::cerr << doc.getLastParseError() << std::endl;
			throw 1;
		}
		
		// All calibration data is encapsulated within the root element <PianoBarCalibration>
		deviceCalibrationElement = baseElement->getChildByName("TouchkeyDeviceCalibration");
		if(deviceCalibrationElement == 0) {
			std::cerr << "TouchkeyDevice: malformed calibration file, aborting.\n";
            delete baseElement;
			throw 1;
		}
		
		// Go through and find each key's calibration information
		calibratorElement = deviceCalibrationElement->getChildByName("Key");
		if(calibratorElement == 0) {
			std::cerr << "TouchkeyDevice: warning: no keys found\n";
		}
		else {
			while(calibratorElement != 0) {
				int keyId;
                
                if(calibratorElement->hasAttribute("id")) {
                    keyId = calibratorElement->getIntAttribute("id");
					if(keyId >= 0 && keyId < keyCalibratorsLength_)
						keyCalibrators_[keyId]->loadFromXml(*calibratorElement);
                }

				calibratorElement = calibratorElement->getNextElementWithTagName("Key");
			}
		}
        
        calibrationInProgress_ = false;
        isCalibrated_ = true;
        if(keyboard_.gui() != 0) {
            for(int i = lowestMidiNote_; i <  lowestMidiNote_ + 12*numOctaves_; i++) {
                keyboard_.gui()->setAnalogCalibrationStatusForKey(i, true);
            }
        }
		//lastCalibrationFile_ = filename;
        
        delete baseElement;
	}
	catch(...) {
		return false;
	}
	
	// TODO: reset key states?
	
	return true;
}

// Initialize the calibrators
void TouchkeyDevice::calibrationInit(int numberOfCalibrators) {
    if(keyCalibrators_ != 0)
        calibrationDeinit();
    if(numberOfCalibrators <= 0)
        return;
    keyCalibratorsLength_ = numberOfCalibrators;
    
    // Initialize the calibrator array
    keyCalibrators_ = (PianoKeyCalibrator **)malloc(keyCalibratorsLength_ * sizeof(PianoKeyCalibrator*));
    
    for(int i = 0; i < keyCalibratorsLength_; i++) {
		keyCalibrators_[i] = new PianoKeyCalibrator(true, 0);
	}
    
    calibrationClear();
}

// Free the initialized calibrators
void TouchkeyDevice::calibrationDeinit() {
    if(keyCalibrators_ == 0)
        return;
    
	for(int i = 0; i < keyCalibratorsLength_; i++) {
        if(keyCalibrators_[i] != 0)
            delete keyCalibrators_[i];
        keyCalibrators_[i] = 0;
    }
    free(keyCalibrators_);
    
    keyCalibratorsLength_ = 0;
    isCalibrated_ = calibrationInProgress_ = false;
}

// Update the lowest MIDI note of the TouchKeys device
void TouchkeyDevice::setLowestMidiNote(int note) {
    // If running, save the value in a temporary holding place until
    // the data gathering thread makes the update. Otherwise update right away.
    // This avoids things changing during data processing and other threading problems.
    if(isAutoGathering())
        updatedLowestMidiNote_ = note;
    else {
        lowestKeyPresentMidiNote_ += (note - lowestMidiNote_);
        lowestMidiNote_ = updatedLowestMidiNote_ = note;
        if(isOpen())
            keyboard_.setKeyboardGUIRange(lowestMidiNote_, lowestMidiNote_ + 12*numOctaves_);
    }
}

// Loop for sending LED updates to the device, which must happen
// in a separate thread from data collection so the device's capacity
// to process incoming data doesn't gate its transmission of sensor data
void TouchkeyDevice::ledUpdateLoop(DeviceThread *thread) {
    
    // Run until told to stop, looking for updates to send to the board
    while(!shouldStop_ && !ledShouldStop_ && !thread->threadShouldExit()) {
        while(!ledUpdateQueue_.empty()) {
            // Get the update
            RGBLEDUpdate& updateStructure = ledUpdateQueue_.back();
            
            if(updateStructure.allLedsOff) {
                internalRGBLEDAllOff();
            }
            else {
                // Convert MIDI note number to board/LED pair. If valid, send to device.
                int board = internalRGBLEDMIDIToBoardNumber(updateStructure.midiNote);
                int led = internalRGBLEDMIDIToLEDNumber(updateStructure.midiNote);
                
                if(board >= 0 && board <= 3 && led >= 0)
                    internalRGBLEDSetColor(board, led, updateStructure.red, updateStructure.green, updateStructure.blue);
            }
            
            // Remove the update we just transmitted
            ledUpdateQueue_.pop_back();
        }
        
        Thread::sleep(20);
    }
}

// Main run loop, which runs in its own thread
void TouchkeyDevice::runLoop(DeviceThread *thread) {
	unsigned char buffer[1024];							// Raw data from device
	unsigned char frame[TOUCHKEY_MAX_FRAME_LENGTH];		// Accumulated frame of data
	int frameLength;
	bool controlSeq = false, inFrame = false, frameError = false;

   /* struct timeval currentTime;
    unsigned long long currentTicks = 0, lastTicks = 0;
    int currentNote = 21;*/

	// Continuously read from the input device.  Read as much data as is available, up to
	// 1024 bytes at a time.  If no data is available, wait 0.5ms before trying again.  USB
	// data comes in every 1ms, so this guarantees no more than a 1ms wait for data, and often less.
	
	while(!shouldStop_ && !thread->threadShouldExit()) {
        
/*
            // This code for RGBLED testing
            gettimeofday(&currentTime, 0);
            
            currentTicks = currentTime.tv_sec * 1000000ULL + currentTime.tv_usec;
            if(currentTicks - lastTicks > 50000ULL) {
                lastTicks = currentTicks;
                rgbledSetColor(currentNote, 0, 0, 0);
                currentNote++;
                if(currentNote > highestMidiNote()) {
                    rgbledAllOff();
                    currentNote = 21;
                }
                rgbledSetColorHSV(currentNote, (float)(currentNote - 21)/(float)(highestMidiNote() - 21), 1.0, 1.0);
            }
*/        
 		long count = deviceRead((char *)buffer, 1024);

		if(count == 0) {
#ifdef _MSC_VER
            Thread::sleep(1);
#else
			usleep(500);
#endif
			continue;
		}
		if(count < 0) {
			if(errno != EAGAIN) {	// EAGAIN just means no data was available
                if(verbose_ >= 1)
                    cout << "Unable to read from device (error " << errno << ").  Aborting.\n";
                stopAutoGathering(false);
				//shouldStop_ = true;
			}
			
#ifdef _MSC_VER
			Thread::sleep(1);
#else
			usleep(500);
#endif
			continue;
		}	
		
		// Process the received data
		
		for(int i = 0; i < count; i++) {
			unsigned char ch = buffer[i];
		
			if(inFrame) {
				// Receiving a frame
				
				if(controlSeq) {
					controlSeq = false;
					if(ch == kControlCharacterFrameEnd)	{		// frame finished?
						inFrame = false;
						processFrame(frame, frameLength);
					}
					else if(ch == kControlCharacterFrameError) { // device telling us about an internal comm error
						if(verbose_ >= 1)
							cout << "Warning: received frame error, continuing anyway.\n";
						frameError = true;
					}
					else if(ch == ESCAPE_CHARACTER) {			// double-escape means a literal escape character
						frame[frameLength++] = ch;
						if(frameLength >= TOUCHKEY_MAX_FRAME_LENGTH) {
							inFrame = false;
							if(verbose_ >= 1)
								cout << "Warning: ignoring frame exceeding length limit " << (int)TOUCHKEY_MAX_FRAME_LENGTH << endl;
						}				
					}
					else if(ch == kControlCharacterNak && verbose_ >= 1) {
                        // TODO: pass this on to a checkForAck() call
						cout << "Warning: received NAK (while receiving frame)\n";
					}			
				}
				else {
					if(ch == ESCAPE_CHARACTER)
						controlSeq = true;
					else {
						frame[frameLength++] = ch;
						if(frameLength >= TOUCHKEY_MAX_FRAME_LENGTH) {
							inFrame = false;
							if(verbose_ >= 1)
								cout << "Warning: ignoring frame exceeding length limit " << (int)TOUCHKEY_MAX_FRAME_LENGTH << endl;
						}
					}
				}				
			}
			else {
				// Waiting for a frame beginning control sequence
				
				if(controlSeq) {
					controlSeq = false;
					if(ch == kControlCharacterFrameBegin) {
						inFrame = true;
						frameLength = 0;
						frameError = false;
					}
					else if(ch == kControlCharacterNak && verbose_ >= 1) {
                        // TODO: pass this on to a checkForAck() call
						cout << "Warning: received NAK (while waiting for frame)\n";
					}
				}
				else {
					if(ch == ESCAPE_CHARACTER)
						controlSeq = true;
				}
			}
		}
	}
}

// Main run loop for gathering raw data from a particular key, used for debugging
// and testing purposes
void TouchkeyDevice::rawDataRunLoop(DeviceThread *thread) {
	unsigned char buffer[1024];							// Raw data from device
	unsigned char frame[TOUCHKEY_MAX_FRAME_LENGTH];		// Accumulated frame of data
	int frameLength;
	bool controlSeq = false, inFrame = false, frameError = false;
    
    unsigned char gatherDataCommand[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin,
        kFrameTypeSendI2CCommand, (unsigned char)rawDataCurrentOctave_, (unsigned char)rawDataCurrentKey_,
        0 /* xmit */, 26 /* response */, 
        ESCAPE_CHARACTER, kControlCharacterFrameEnd};
    
    //struct timeval currentTime;
    double currentTime = 0, lastTime = 0;
    //unsigned long long currentTicks = 0, lastTicks = 0;

	// Continuously read from the input device.  Read as much data as is available, up to
	// 1024 bytes at a time.  If no data is available, wait 0.5ms before trying again.  USB
	// data comes in every 1ms, so this guarantees no more than a 1ms wait for data, and often less.
	
	while(!shouldStop_ && !thread->threadShouldExit()) {
        // Every 100ms, request raw data from the active key
        currentTime = Time::getMillisecondCounterHiRes();
        
        if(currentTime - lastTime > 50.0) {
            lastTime = currentTime;
            
            // Check if we need to choose a new key or mode
            if(rawDataShouldChangeMode_) {
                // Prepare the key and update the command
                rawDataPrepareCollection(rawDataCurrentOctave_, rawDataCurrentKey_, rawDataCurrentMode_, rawDataCurrentScaler_);
                gatherDataCommand[3] = rawDataCurrentOctave_;
                gatherDataCommand[4] = rawDataCurrentKey_;
            }
            
            // Request data
            if(deviceWrite((char*)gatherDataCommand, 9) < 0) {
                if(verbose_ >= 1)
                    cout << "ERROR: unable to write gather data command.  errno = " << errno << endl;
            }
        }
      
 		long count = deviceRead((char *)buffer, 1024);

		if(count == 0) {
#ifdef _MSC_VER
			Thread::sleep(1);
#else
			usleep(500);
#endif
			continue;
		}
		if(count < 0) {
			if(errno != EAGAIN) {	// EAGAIN just means no data was available
                if(verbose_ >= 1)
                    cout << "Unable to read from device (error " << errno << ").  Aborting.\n";
                stopAutoGathering(false);
				//shouldStop_ = true;
			}
			
#ifdef _MSC_VER
			Thread::sleep(1);
#else
			usleep(500);
#endif
			continue;
		}
		
		// Process the received data
		
		for(int i = 0; i < count; i++) {
			unsigned char ch = buffer[i];
            
			if(inFrame) {
				// Receiving a frame
				
				if(controlSeq) {
					controlSeq = false;
					if(ch == kControlCharacterFrameEnd)	{		// frame finished?
						inFrame = false;
						processFrame(frame, frameLength);
					}
					else if(ch == kControlCharacterFrameError) { // device telling us about an internal comm error
						if(verbose_ >= 1)
							cout << "Warning: received frame error, continuing anyway.\n";
						frameError = true;
					}
					else if(ch == ESCAPE_CHARACTER) {			// double-escape means a literal escape character
						frame[frameLength++] = ch;
						if(frameLength >= TOUCHKEY_MAX_FRAME_LENGTH) {
							inFrame = false;
							if(verbose_ >= 1)
								cout << "Warning: ignoring frame exceeding length limit " << (int)TOUCHKEY_MAX_FRAME_LENGTH << endl;
						}
					}
					else if(ch == kControlCharacterNak && verbose_ >= 1) {
                        // TODO: pass this on to a checkForAck() call
						cout << "Warning: received NAK (while receiving frame)\n";
					}
				}
				else {
					if(ch == ESCAPE_CHARACTER)
						controlSeq = true;
					else {
						frame[frameLength++] = ch;
						if(frameLength >= TOUCHKEY_MAX_FRAME_LENGTH) {
							inFrame = false;
							if(verbose_ >= 1)
								cout << "Warning: ignoring frame exceeding length limit " << (int)TOUCHKEY_MAX_FRAME_LENGTH << endl;
						}
					}
				}
			}
			else {
				// Waiting for a frame beginning control sequence
				
				if(controlSeq) {
					controlSeq = false;
					if(ch == kControlCharacterFrameBegin) {
						inFrame = true;
						frameLength = 0;
						frameError = false;
					}
					else if(ch == kControlCharacterNak && verbose_ >= 1) {
                        // TODO: pass this on to a checkForAck() call
						cout << "Warning: received NAK (while waiting for frame)\n";
					}
				}
				else {
					if(ch == ESCAPE_CHARACTER)
						controlSeq = true;
				}
			}
		}
	}
}

// Process the contents of a frame that has been received from the device
void TouchkeyDevice::processFrame(unsigned char * const frame, int length) {
	if(length == 0)	// Empty frame --> nothing to do here
		return;
	
	switch(frame[0]) { // First character gives frame type
		case kFrameTypeCentroid:
			if(verbose_ >= 3)
				cout << "Received centroid data\n";
			processCentroidFrame(&frame[1], length - 1);
			break;
		case kFrameTypeRawKeyData:
			if(verbose_ >= 3)
				cout << "Received raw key data\n";
			processRawDataFrame(&frame[1], length - 1);
			break;
        case kFrameTypeAnalog:
			if(verbose_ >= 3)
				cout << "Received analog data\n";
            processAnalogFrame(&frame[1], length - 1);
            break;
        case kFrameTypeErrorMessage:
            if(verbose_ >= 3)
				cout << "Received error data\n";
            processErrorMessageFrame(&frame[1], length-1);
            break;
        case kFrameTypeI2CResponse:
            if(verbose_ >= 3)
                cout << "Received I2C response\n";
            processI2CResponseFrame(&frame[1], length - 1);
            break;
		case kFrameTypeStatus:
		default:
			if(verbose_ >= 3)
				cout << "Received frame type " << (int)frame[0] << endl;			
			break;
	}	
}

// Process a frame of data containing centroid values (the default mode of scanning)
void TouchkeyDevice::processCentroidFrame(unsigned char * const buffer, const int bufferLength) {
    int frame, octave, bufferIndex;
    
    // Old and new generation devices structure the frame differently. 
	if((deviceSoftwareVersion_ <= 0 && bufferLength < 3) || (deviceSoftwareVersion_ > 0 && bufferLength < 5)) {
		if(verbose_ >= 1)
			cout << "Warning: ignoring malformed centroid frame of " << bufferLength << " bytes, less than minimum 3\n";
		if(verbose_ >= 2) {
			cout << "  Contents: ";
			hexDump(cout, buffer, bufferLength);
			cout << endl;
		}
		return;
	}
	
	if(verbose_ >= 4) {
		cout << "Centroid frame contents:  ";
		hexDump(cout, buffer, bufferLength);
		cout << endl;		
	}
	
    // Parse the octave and timestamp differently depending on hardware version
    if(deviceSoftwareVersion_ > 0) {
        octave = buffer[0]; // First byte is octave
        
        // Frame is stored as 32-bit little endian value
        frame = buffer[1] + ((int)buffer[2] << 8) + ((int)buffer[3] << 16) + ((int)buffer[4] << 24);
        bufferIndex = 5;
        
        if(verbose_ >= 3)
            cout << "Centroid frame octave " << octave << " timestamp " << frame << endl;
    }
    else {
        frame = (buffer[0] << 8) + buffer[1];	// First two bytes give us the timestamp in milliseconds (mod 2^16)
        octave = buffer[2];	// Third byte tells us which octave of keys is being addressed
        bufferIndex = 3;
	}
    
	// Convert from device frame number (expressed in USB 1ms SOF intervals) to a system
	// timestamp that can be synchronized with other data streams
	lastTimestamp_ = timestampSynchronizer_.synchronizedTimestamp(frame);
	
	//ioMutex_.enter();
	
	while(bufferIndex < bufferLength) {
		// First byte tells us the number of the key (0-12); next bytes hold the data frame
		int key = (int)buffer[bufferIndex++];
		int bytesParsed = processKeyCentroid(frame,octave, key, lastTimestamp_, &buffer[bufferIndex], bufferLength - bufferIndex);
		
		if(bytesParsed < 0)  {
			if(verbose_ >= 1)
				cout << "Warning: malformed data frame (parsing key " << key << " at byte " << bufferIndex << ")\n";
			
			if(verbose_ >= 2) {
				cout << "--> Data: ";
				hexDump(cout, buffer, bufferLength);
				cout << endl;
			}
			
			break;
		}
		
		bufferIndex += bytesParsed;
	}
    
    if(updatedLowestMidiNote_ != lowestMidiNote_) {
        int keyPresentDifference = (lowestKeyPresentMidiNote_ - lowestMidiNote_);
        
		lowestMidiNote_ = updatedLowestMidiNote_;
        lowestKeyPresentMidiNote_ = lowestMidiNote_ + keyPresentDifference;

        // Turn off all existing touches before changing the octave
        // so we don't end up with orphan touches when the "off" message is
        // sent to a different octave than the "on"
        for(int i = 0; i <= 127; i++)
            if(keyboard_.key(i) != 0)
                if(keyboard_.key(i)->touchIsActive())
                    keyboard_.key(i)->touchOff(lastTimestamp_);
        
        keyboard_.setKeyboardGUIRange(lowestMidiNote_, lowestMidiNote_ + 12*numOctaves_);
    }
	
	//ioMutex_.exit();
}

// Process a frame containing raw key data, whose configuration was set with startRawDataCollection()
// First byte holds the octave that the data came from.

void TouchkeyDevice::processRawDataFrame(unsigned char * const buffer, const int bufferLength) {
	int octave = buffer[0];
	
	if(verbose_ >= 3)
		cout << "Raw data frame from octave " << octave << " contains " << bufferLength - 1 << " samples\n";
	
	if(verbose_ >= 4) {
		cout << "  ";
		hexDump(cout, &buffer[1], bufferLength - 1);
		cout << endl;		
	}
    
    // Change the first byte to contain the note number this data is expected to have come
    // from (based on which key we are presently querying)
    buffer[0] = lowestMidiNote_ + (rawDataCurrentOctave_ * 12 + rawDataCurrentKey_);

	// Send raw data as an OSC blob
	lo_blob b = lo_blob_new(bufferLength, buffer);
	keyboard_.sendMessage("/touchkeys/rawbytes", "b", b, LO_ARGS_END);
	lo_blob_free(b);	
}

// Extract the floating-point centroid data for a key from packed character input.
// Send OSC features as appropriate

int TouchkeyDevice::processKeyCentroid(int frame, int octave, int key, timestamp_type timestamp, unsigned char * buffer, int maxLength) {
	int touchCount = 0;
	
	float sliderPosition[3];
	float sliderPositionH;
	float sliderSize[3];
	
	int bytesParsed;
	
	if(key < 0 || key > 12 || maxLength < 1)
		return -1;
	
	int white = (kKeyColor[key] == kKeyColorWhite);
	int midiNote = octaveKeyToMidi(octave, key);
	
	// Check that the received data is actually valid and not left over from a previous scan (which
	// can happen when the scan rate is too high).  0x88 is a special "warning" marker for this case
	// since it will never be part of a valid centroid.
	
	if(buffer[0] == 0x88) {
        if(verbose_ >= 1)
            cout << "Warning: octave " << octave << " key " << key << " data is not ready.  Check scan rate.\n";
        if(deviceSoftwareVersion_ >= 1)
            return white ? expectedLengthWhite_ : expectedLengthBlack_;
        else
            return 1;
	}
	
	// A value of 0xFF means that no touch is active, and no further data will be present on this key.
	
	if(buffer[0] == 0xFF && deviceSoftwareVersion_ <= 0) {
		bytesParsed = 1;
		sliderPosition[0] = sliderPosition[1] = sliderPosition[2] = -1.0;
		sliderSize[0] = sliderSize[1] = sliderSize[2] = 0.0;
		sliderPositionH = -1.0;
		
		if(verbose_ >= 4) {
			cout << "Octave " << octave << " Key " << key << " (TS " << timestamp << "): ff\n";
		}			
	}
	else {		
		bytesParsed = white ? expectedLengthWhite_ : expectedLengthBlack_;
		
		if(bytesParsed > maxLength)	// Make sure there's enough buffer left to process this key
			return -1;
		
		int rawSliderPosition[3];
		int rawSliderPositionH;		
		
		rawSliderPosition[0] = (((buffer[0] & 0xF0) << 4) + buffer[1]);
		rawSliderPosition[1] = (((buffer[0] & 0x0F) << 8) + buffer[2]);
		rawSliderPosition[2] = (((buffer[3] & 0xF0) << 4) + buffer[4]);
		
        if(deviceHardwareVersion_ >= 2)
        {
            // Always an H value with version 2 sensor hardware
            rawSliderPositionH = (((buffer[3] & 0x0F) << 8) + buffer[5]);
            
            if(white) {
                for(int i = 0; i < 3; i++) {
                    if(rawSliderPosition[i] != 0x0FFF) {	// 0x0FFF means no touch
                        sliderPosition[i] = (float)rawSliderPosition[i] / whiteMaxY_;
                        sliderSize[i] = (float)buffer[i + 6] / kSizeMaxValue;
                        touchCount++;
                    }
                    else {
                        sliderPosition[i] = -1.0;
                        sliderSize[i] = 0.0;
                    }
                }
            }
            else {
                for(int i = 0; i < 3; i++) {
                    if(rawSliderPosition[i] != 0x0FFF) {	// 0x0FFF means no touch
                        sliderPosition[i] = (float)rawSliderPosition[i] / blackMaxY_;
                        sliderSize[i] = (float)buffer[i + 6] / kSizeMaxValue;
                        touchCount++;
                    }
                    else {
                        sliderPosition[i] = -1.0;
                        sliderSize[i] = 0.0;
                    }
                }
            }
        }
        else
        {
            // H value only on white keys with version 0-1 sensor hardware
            
            if(white) {
                rawSliderPositionH = (((buffer[3] & 0x0F) << 8) + buffer[5]);
                
                for(int i = 0; i < 3; i++) {
                    if(rawSliderPosition[i] != 0x0FFF) {	// 0x0FFF means no touch
                        sliderPosition[i] = (float)rawSliderPosition[i] / whiteMaxY_;
                        sliderSize[i] = (float)buffer[i + 6] / kSizeMaxValue;
                        touchCount++;
                    }
                    else {
                        sliderPosition[i] = -1.0;
                        sliderSize[i] = 0.0;
                    }
                }
            }
            else {
                rawSliderPositionH = 0x0FFF;
                
                for(int i = 0; i < 3; i++) {
                    if(rawSliderPosition[i] != 0x0FFF) {	// 0x0FFF means no touch
                        sliderPosition[i] = (float)rawSliderPosition[i] / blackMaxY_;
                        sliderSize[i] = (float)buffer[i + 5] / kSizeMaxValue;
                        touchCount++;
                    }
                    else {
                        sliderPosition[i] = -1.0;
                        sliderSize[i] = 0.0;
                    }
                }		
            }
        }
		
		if(rawSliderPositionH != 0x0FFF) {
			sliderPositionH = (float)rawSliderPositionH / whiteMaxX_ ;
		}
		else
			sliderPositionH = -1.0;
		
		if(verbose_ >= 4) {
			cout << "Octave " << octave << " Key " << key << ": ";
			hexDump(cout, buffer, white ? expectedLengthWhite_ : expectedLengthBlack_);
			cout << endl;
		}	
	}
	
	// Sanity check: do we have the PianoKey structure available to receive this data?
	// If not, no need to proceed further.
	if(keyboard_.key(midiNote) == 0) {
        if(verbose_ >= 1)
            cout << "Warning: No PianoKey available for touchkey MIDI note " << midiNote << endl;
		return bytesParsed;
	}
    
    // From here on out, grab the performance data mutex so no MIDI events can show up in the middle
    ScopedLock ksl(keyboard_.performanceDataMutex_);

	// Turn off touch activity on this key if there's no active touches
	if(touchCount == 0) {
		if(keyboard_.key(midiNote)->touchIsActive())
        {
			keyboard_.key(midiNote)->touchOff(timestamp);
            KeyTouchFrame newFrame(0, sliderPosition, sliderSize, sliderPositionH, white);
            
            if (loggingActive_)
            {
                ////////////////////////////////////////////////////////
                ////////////////////////////////////////////////////////
                //////////////////// BEGIN LOGGING /////////////////////
                
                keyTouchLog_.write((char*)&timestamp, sizeof(timestamp_type));
                keyTouchLog_.write((char*)&frame, sizeof(int));
                keyTouchLog_.write((char*)&midiNote, sizeof(int));
                keyTouchLog_.write((char*)&newFrame, sizeof(KeyTouchFrame));
                
                ///////////////////// END LOGGING //////////////////////
                ////////////////////////////////////////////////////////
                ////////////////////////////////////////////////////////
            }
            
            // Send raw OSC message if enabled
            if(sendRawOscMessages_) {
                keyboard_.sendMessage("/touchkeys/raw-off", "iii",
                                      octave, key, frame,
                                      LO_ARGS_END );
            }
            
        }
        
        return bytesParsed;
	}
	
	// At this point, construct a new frame with this data and pass it to the PianoKey for
	// further processing.  Leave the ID fields empty; these are state-dependent and will
	// be worked out based on the previous frames.
	
	KeyTouchFrame newFrame(touchCount, sliderPosition, sliderSize, sliderPositionH, white);
	
	keyboard_.key(midiNote)->touchInsertFrame(newFrame, timestamp);
    
    
    if (loggingActive_)
    {
        ////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////
        //////////////////// BEGIN LOGGING /////////////////////
        
        keyTouchLog_.write((char*)&timestamp, sizeof(timestamp_type));
        keyTouchLog_.write((char*)&frame, sizeof(int));
        keyTouchLog_.write((char*)&midiNote, sizeof(int));
        keyTouchLog_.write((char*)&newFrame, sizeof(KeyTouchFrame));
        
        ///////////////////// END LOGGING //////////////////////
        ////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////
    }
	
	// Send raw OSC message if enabled
	if(sendRawOscMessages_) {
		keyboard_.sendMessage("/touchkeys/raw", "iiifffffff",
									 octave, key, frame,
									 sliderPosition[0],
									 sliderSize[0],
									 sliderPosition[1],
									 sliderSize[1],									 
									 sliderPosition[2],
									 sliderSize[2],
									 sliderPositionH,
									 LO_ARGS_END );
	}
	
	// Verbose logging of key info
	if(verbose_ >= 3) {
		cout << "Octave " << octave << " Key " << key << " (TS " << timestamp << "): ";
		cout << sliderPositionH << " ";
		cout << sliderPosition[0] << " " << sliderPosition[1] << " " << sliderPosition[2] << " ";
		cout << sliderSize[0] << " " << sliderSize[1] << " " << sliderSize[2] << endl;
	}	
	
	return bytesParsed;
}

// Process a frame of data containing analog values (i.e. key angle, Z-axis). These
// always come as a group for a whole board, and should be parsed apart into individual keys
void TouchkeyDevice::processAnalogFrame(unsigned char * const buffer, const int bufferLength) {
    // Format: [Octave] [TS0] [TS1] [TS2] [TS3] [Key0L] [Key0H] [Key1L] [Key1H] ... [Key24L] [Key24H]
    //                   ... (more frames)
    //                  [TS0] [TS1] [TS2] [TS3] [Key0L] [Key0H] [Key1L] [Key1H] ... [Key24L] [Key24H]
    
    if(bufferLength < 1) {
		if(verbose_ >= 1)
			cout << "Warning: ignoring malformed analog frame of " << bufferLength << " bytes, less than minimum 1\n";
        return;
    }
    
    int octave = buffer[0];
    int board = octave / 2;
    int frame;
    int bufferIndex = 1;
    int midiNote, value;
    
    // Parse the buffer one frame at a time
    while(bufferIndex < bufferLength) {
        if(bufferLength - bufferIndex < 54) {
            // This condition indicates a malformed analog frame (not enough data)
            if(verbose_ >= 1)
                cout << "Warning: ignoring extra analog data of " << bufferLength - bufferIndex << " bytes, less than full frame 54 (total " << bufferLength << ")\n";
            break;
        }
        
        // Find the timestamp (i.e. frame ID generated by the device). 32-bit little-endian.
        frame = buffer[bufferIndex] + ((int)buffer[bufferIndex+1] << 8) +
                ((int)buffer[bufferIndex+2] << 16) + ((int)buffer[bufferIndex+3] << 24);
        
        // Check the timestamp against the last frame from this board to see if any frames have been dropped
        if(frame > analogLastFrame_[board] + 1) {
            if(verbose_ >= 1)
                cout << "WARNING: dropped frame(s) on board " << board << " at " << frame << " (last was " << analogLastFrame_[board] << ")" << endl;
        }
        else if(frame < analogLastFrame_[board] + 1) {
            if(verbose_ >= 1)
                cout << "WARNING: repeat frame(s) on board " << board << " at " << frame << " (last was " << analogLastFrame_[board] << ")" << endl;
        }
        analogLastFrame_[board] = frame;
        
        // TESTING
        /*if(verbose_ >= 3 || (frame % 500 == 0))
            cout << "Analog frame octave " << octave << " timestamp " << frame << endl;
        if(verbose_ >= 4 || (frame % 500 == 0)) {
            cout << "Values: ";
            for(int i = 0; i < 25; i++) {
                cout << std::setw(5) << (((signed char)buffer[i*2 + 6])*256 + buffer[i*2 + 5]) << " ";
            }
            cout << endl;
        }*/
        
        // Process key values individually and add them to the keyboard data structure
        for(int key = 0; key < 25; key++) {
            // Every analog frame contains 25 values, however only the top board actually uses all 25
            // sensors. There are several "high C" values in the lower boards (i.e. key == 24) which
            // do not correspond to real sensors. These should be ignored.
            if(key == 24 && octave != numberOfOctaves() - 2)
                continue;
            
            midiNote = octaveKeyToMidi(octave, key);
            
            // Check that this note is in range to the available calibrators and keys.
            if(keyboard_.key(midiNote) == 0 || (octave*12 + key) >= keyCalibratorsLength_ || midiNote < 21)
                continue;
            
            // Pull the value out from the packed buffer (little endian 16 bit)
            value = (((signed char)buffer[key*2 + 6])*256 + buffer[key*2 + 5]);
            
            // Calibrate the value, assuming the calibrator is ready and running
            key_position calibratedPosition = keyCalibrators_[octave*12 + key]->evaluate(value);
            if(!missing_value<key_position>::isMissing(calibratedPosition)) {
                timestamp_type timestamp = timestampSynchronizer_.synchronizedTimestamp(frame);
                keyboard_.key(midiNote)->insertSample(calibratedPosition, timestamp);
            }
            else if(keyboard_.gui() != 0){
                
                //keyboard_.key(midiNote)->insertSample((float)value / 4096.0, timestampSynchronizer_.synchronizedTimestamp(frame));
                
                // Update the GUI but don't actually save the value since it's uncalibrated
                keyboard_.gui()->setAnalogValueForKey(midiNote, (float)value / kTouchkeyAnalogValueMax);
                
                if(keyCalibrators_[octave*12 + key]->calibrationStatus() == kPianoKeyCalibrated) {
                    if(verbose_ >= 1)
                        cout << "key " << midiNote << " calibrated but missing (raw value " << value << ")\n";
                }
            }
        }
        
        if(loggingActive_) {
            analogLog_.write((char*)&buffer[0], 1); // Octave number
            analogLog_.write((char*)&buffer[bufferIndex], 54);
        }
        
        // Skip to next frame
        bufferIndex += 54;
    }
}

// Process a frame containing a human-readable (and machine-coded) error message generated
// internally by the device
void TouchkeyDevice::processErrorMessageFrame(unsigned char * const buffer, const int bufferLength) {
    char msg[256];
    int len = bufferLength - 5;
    
    // Error on error message frame!
    if(bufferLength < 5) {
        if(verbose_ >= 1)
            cout << "Warning: received error message frame of " << bufferLength << " bytes, less than minimum 5\n";
        return;
    }
    
    // Limit length of string for safety reasons
    if(len > 256)
        len = 256;
    memcpy(msg, &buffer[5], len * sizeof(char));
    msg[len - 1] = '\0';
    
    // Print the error
    if(verbose_ >= 1)
        cout << "Error frame received: " << msg << endl;
    
    // Dump the buffer containing error coding information
    if(verbose_ >= 2) {
        cout << "Contents: ";
        hexDump(cout, buffer, 5);
        cout << endl;
    }
}

// Process a frame containing a response to an I2C command. We can use this to gather
// raw information from the key.
void TouchkeyDevice::processI2CResponseFrame(unsigned char * const buffer, const int bufferLength) {
    // Format: [octave] [key] [length] <data>
    
    if(bufferLength < 3) {
        if(verbose_ >= 1)
            cout << "Warning: received I2C response frame of " << bufferLength << " bytes, less than minimum 3\n";
        return;
    }
    
    int octave = buffer[0];
    int key = buffer[1];
    int responseLength = buffer[2];
    
    if(bufferLength < responseLength + 3) {
        if(verbose_ >= 1) {
            cout << "Warning: received malformed I2C response (octave " << octave << ", key " << key << ", length " << responseLength;
            cout << ") but only " << bufferLength - 3 << " bytes of data\n";
        }
        if(verbose_ >= 4) {
            cout << "  ";
            hexDump(cout, &buffer[3], bufferLength - 3);
            cout << endl;
        }
        
        responseLength = bufferLength - 3;
    }
    else {
        if(verbose_ >= 3) {
            cout << "I2C response from octave " << octave << ", key " << key << ", length " << responseLength << endl;
        }
        if(verbose_ >= 4) {
            cout << "  ";
            hexDump(cout, &buffer[3], responseLength);
            cout << endl;
        }
    }
    
    if(sensorDisplay_ != 0) {
        // Copy response data to display
        vector<int> data;
        
        for(int i = 3; i < responseLength + 3; i++) {
            data.push_back(buffer[i]);
        }
        sensorDisplay_->setDisplayData(data);
    }
    
    // Change the first byte to contain the note number this data is expected to have come
    // from (based on which key we are presently querying)
    buffer[2] = lowestMidiNote_ + (octave * 12 + key);
    
	// Send raw data as an OSC blob
	lo_blob b = lo_blob_new(responseLength + 1, &buffer[2]);
	keyboard_.sendMessage("/touchkeys/rawbytes", "b", b, LO_ARGS_END);
	lo_blob_free(b);
}

// Parse raw data from a status request.  Buffer should start immediately after the
// frame type byte, and processing will finish either at the end of the expected buffer,
// or at the given length, whichever comes first.  Returns true if a status buffer was
// successfully received.

bool TouchkeyDevice::processStatusFrame(unsigned char * buffer, int maxLength, TouchkeyDevice::ControllerStatus *status) {
	if((status == 0 || maxLength < 5) && verbose_ >= 1) {
		cout << "Invalid status frame: ";
		hexDump(cout, buffer, maxLength);
		cout << endl;
		return false;
	}
	
	status->hardwareVersion = buffer[0];
	status->softwareVersionMajor = buffer[1];
	status->softwareVersionMinor = buffer[2];
	status->running = ((buffer[3] & kStatusFlagRunning) != 0);
	status->octaves = buffer[4];
	status->connectedKeys = (unsigned int *)malloc(2*status->octaves*sizeof(unsigned int));
	
	int i, oct = 0;				// Get connected key information
    if(status->softwareVersionMajor >= 2) {
        // One extra byte holds lowest physical sensor
        status->lowestHardwareNote = buffer[5];
        status->hasTouchSensors = ((buffer[3] & kStatusFlagHasI2C) != 0);
        status->hasAnalogSensors = ((buffer[3] & kStatusFlagHasAnalog) != 0);
        status->hasRGBLEDs = ((buffer[3] & kStatusFlagHasRGBLED) != 0);
        i = 6;
    }
    else {
        status->lowestHardwareNote = 0;
        status->hasTouchSensors = true;
        status->hasAnalogSensors = true;
        status->hasRGBLEDs = true;
        i = 5;
    }

	while(i+1 < maxLength) {
		status->connectedKeys[oct] = 256*buffer[i] + buffer[i+1];
		i += 2;
		oct++;
	}
	
	if(oct < status->octaves && verbose_ >= 1) {
		cout << "Invalid status frame: ";
		hexDump(cout, buffer, maxLength);	
		cout << endl;
		return false;
	}
	
	return true;
}

// Prepare the indicated key for raw data collection
void TouchkeyDevice::rawDataPrepareCollection(int octave, int key, int mode, int scaler) {
    Thread::sleep(10);
    
    // Command to set the mode of the key
    unsigned char commandSetMode[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin,
        kFrameTypeSendI2CCommand, (unsigned char)octave, (unsigned char)key,
        3 /* xmit */, 0 /* response */, 0 /* command offset */, 1 /* mode */, (unsigned char)mode,
        ESCAPE_CHARACTER, kControlCharacterFrameEnd};
	
	if(deviceWrite((char*)commandSetMode, 12) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write setMode command.  errno = " << errno << endl;
	}

    Thread::sleep(10);
    
    // Command to set the scaler of the key
    unsigned char commandSetScaler[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin,
        kFrameTypeSendI2CCommand, (unsigned char)octave, (unsigned char)key,
        3 /* xmit */, 0 /* response */, 0 /* command offset */, 3 /* raw scaler */, (unsigned char)scaler,
        ESCAPE_CHARACTER, kControlCharacterFrameEnd};
	
	if(deviceWrite((char*)commandSetScaler, 12) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write setMode command.  errno = " << errno << endl;
	}
    
    Thread::sleep(10);
    
    unsigned char commandPrepareRead[] = {ESCAPE_CHARACTER, kControlCharacterFrameBegin,
        kFrameTypeSendI2CCommand, (unsigned char)octave, (unsigned char)key,
        1 /* xmit */, 0 /* response */, 6 /* data offset */,
        ESCAPE_CHARACTER, kControlCharacterFrameEnd};
    
	if(deviceWrite((char*)commandPrepareRead, 10) < 0) {
        if(verbose_ >= 1)
            cout << "ERROR: unable to write prepareRead command.  errno = " << errno << endl;
	}

   Thread::sleep(10);
    
    rawDataShouldChangeMode_ = false;
}

// Check for an ACK response from the device.  Returns true if found.  Returns
// false if NAK received, or if a timeout occurs.
// TODO: this implementation needs to change to not chew up other data coming in.

bool TouchkeyDevice::checkForAck(int timeoutMilliseconds) {
	//struct timeval startTime, currentTime;
	bool controlSeq = false;
	unsigned char ch;
	
    double startTime = Time::getMillisecondCounterHiRes();
    double currentTime = startTime;
    
	//gettimeofday(&startTime, 0);
	//gettimeofday(&currentTime, 0);
	
	while(currentTime - startTime < (double)timeoutMilliseconds) {
		long count = deviceRead((char *)&ch, 1);

		if(count < 0) {				// Check if an error occurred on read
			if(errno != EAGAIN) {
                if(verbose_ >= 1)
                    cout << "Unable to read from device while waiting for ACK (error " << errno << ").  Aborting.\n";
				return false;
			}
		}
		else if(count > 0) {		// Data received
			// Wait for a sequence {ESCAPE_CHARACTER, ACK} or {ESCAPE_CHARACTER, NAK}
			if(controlSeq) {
				controlSeq = false;
				if(ch == kControlCharacterAck) {
					if(verbose_ >= 2)
						cout << "Received ACK\n";
					return true;
				}
				else if(ch == kControlCharacterNak) {
					if(verbose_ >= 1)
						cout << "Warning: received NAK\n";
					return false;
				}
			}
			else if(ch == ESCAPE_CHARACTER)
				controlSeq = true;
		}
		
		currentTime = Time::getMillisecondCounterHiRes();
	}
	
    if(verbose_ >= 1)
        cout << "Error: timeout waiting for ACK\n";
	return false;
}

// Convenience method to dump hexadecimal output
void TouchkeyDevice::hexDump(ostream& str, unsigned char * buffer, int length) {
	if(length <= 0)
		return;
	str << std::hex << (int)buffer[0];
	for(int i = 1; i < length; i++) {
		str << " " << (int)buffer[i];
	}
	str << std::dec;
}

// Read from the TouchKeys device
long TouchkeyDevice::deviceRead(char *buffer, unsigned int count) {
#ifdef _MSC_VER
	int n;

	if(!ReadFile(serialHandle_, buffer, count, (LPDWORD)((void *)&n), NULL))
		return -1;
	return n;
#else
    return read(device_, buffer, count);
#endif
}

// Write to the TouchKeys device
int TouchkeyDevice::deviceWrite(char *buffer, unsigned int count) {
    int result;
    
#ifdef _MSC_VER
    if(!WriteFile(serialHandle_, buffer, count, (LPDWORD)((void *)&result), NULL))
		return -1;
#else
    result = write(device_, buffer, count);
#endif
    deviceDrainOutput();
    return result;
}

// Flush (discard) the TouchKeys device input
void TouchkeyDevice::deviceFlush(bool bothDirections) {
#ifdef _MSC_VER
	// WINDOWS_TODO (?)
#else
    if(bothDirections)
        tcflush(device_, TCIOFLUSH);
    else
        tcflush(device_, TCIFLUSH);							// Flush device input
#endif
}

// Flush the TouchKeys device output
void TouchkeyDevice::deviceDrainOutput() {
#ifdef _MSC_VER
    FlushFileBuffers(serialHandle_);
#else
    tcdrain(device_);
#endif
}


TouchkeyDevice::~TouchkeyDevice() {
    if (logFileCreated_)
    {
        keyTouchLog_.close();
        analogLog_.close();
    }
    
	closeDevice();
    calibrationDeinit();
}