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:   TimestampSynchronizer.cpp: handles aligning timestamps between multiple
andrewm@0:   asynchronous sources, while reducing the jitter that occurs when using
andrewm@0:   system clock time for every received sample.
andrewm@0: */
andrewm@0: 
andrewm@0: #include "TimestampSynchronizer.h"
andrewm@0: 
andrewm@0: // Constructor
andrewm@0: TimestampSynchronizer::TimestampSynchronizer()
andrewm@0: : history_(kTimestampSynchronizerHistoryLength), nominalSampleInterval_(0), currentSampleInterval_(0),
andrewm@0: frameModulus_(0), startingClockTimeMilliseconds_(0), startingTimestamp_(0), 
andrewm@0: bufferLengthCounter_(0)
andrewm@0: {
andrewm@0: }
andrewm@0: 
andrewm@0: // Clear the accumulated timestamp history and reset the current
andrewm@0: // value to its nominal "expected" value.  Also (re-)establish
andrewm@0: // the relationship between system clock time and output timestamp.
andrewm@0: // If multiple streams are to be synchronized, they should be
andrewm@0: // initialized with the same values
andrewm@0: 
andrewm@0: void TimestampSynchronizer::initialize(double clockTimeMilliseconds,
andrewm@0: 									   timestamp_type startingTimestamp) {
andrewm@0: 	history_.clear();
andrewm@0: 	currentSampleInterval_ = nominalSampleInterval_;
andrewm@0: 	startingClockTimeMilliseconds_ = clockTimeMilliseconds;
andrewm@0: 	startingTimestamp_ = startingTimestamp;
andrewm@0: 	
andrewm@0: 	//cout << "initialize(): startingTimestamp = " << startingTimestamp_ << ", interval = " << nominalSampleInterval_ << endl;
andrewm@0: }
andrewm@0: 
andrewm@0: // Given a frame number, calculate a current timestamp
andrewm@0: timestamp_type TimestampSynchronizer::synchronizedTimestamp(int rawFrameNumber) {
andrewm@0: 	// Calculate the current system clock-related timestamp
andrewm@0: 	timestamp_type clockTime = startingTimestamp_ + milliseconds_to_timestamp(Time::getMillisecondCounterHiRes() - startingClockTimeMilliseconds_);
andrewm@0: 	timestamp_type frameTime;
andrewm@0: 
andrewm@0: 	// Retrieve the timestamp of the previous frame
andrewm@0: 	// Need at least 2 samples in the buffer for the calculations that follow
andrewm@0: 	if(history_.empty()) {
andrewm@0: 		frameTime = clockTime;
andrewm@0: 	}
andrewm@0: 	else if(history_.size() < 2) {
andrewm@0: 		// One sample in buffer: make sure the new sample is new before
andrewm@0: 		// storing it in the buffer.
andrewm@0: 		
andrewm@0: 		int lastFrame = history_.latest().first;
andrewm@0: 		
andrewm@0: 		frameTime = clockTime;
andrewm@0: 		
andrewm@0: 		if(lastFrame == rawFrameNumber) // Don't reprocess identical frames
andrewm@0: 			return frameTime;		
andrewm@0: 	}
andrewm@0: 	else {
andrewm@0: 		int totalHistoryFrames;		
andrewm@0: 		int lastFrame = history_.latest().first;
andrewm@0: 		frameTime = history_.latest().second;
andrewm@0: 		
andrewm@0: 		if(lastFrame == rawFrameNumber) // Don't reprocess identical frames
andrewm@0: 			return frameTime;
andrewm@0: 			
andrewm@0: 		if(frameModulus_ == 0) {
andrewm@0: 			// No modulus, just compare the raw frame number to the last frame number
andrewm@0: 			frameTime += currentSampleInterval_ * (timestamp_type)(rawFrameNumber - lastFrame);
andrewm@0: 			
andrewm@0: 			totalHistoryFrames = (history_.latest().first - history_.earliest().first);
andrewm@0: 			if(totalHistoryFrames <= 0) {
andrewm@0: 				//cout << "Warning: TimestampSynchronizer history buffer has a difference of " << totalHistoryFrames << " frames.\n";
andrewm@0: 				//cout << "Size = " << history_.size() << " first = " << history_.earliest().first << " last = " << history_.latest().first << endl;
andrewm@0: 				totalHistoryFrames = 1;
andrewm@0: 			}
andrewm@0: 		}
andrewm@0: 		else {
andrewm@0: 			// Use mod arithmetic to handle wraparounds in the frame number
andrewm@0: 			frameTime += currentSampleInterval_ * (timestamp_type)((rawFrameNumber + frameModulus_ - lastFrame) % frameModulus_);
andrewm@0: 			
andrewm@0: 			totalHistoryFrames = (history_.latest().first - history_.earliest().first + frameModulus_) % frameModulus_;
andrewm@0: 			if(totalHistoryFrames <= 0) {
andrewm@0: 				//cout << "Warning: TimestampSynchronizer history buffer has a difference of " << totalHistoryFrames << " frames.\n";
andrewm@0: 				//cout << "Size = " << history_.size() << " first = " << history_.earliest().first << " last = " << history_.latest().first << endl;
andrewm@0: 
andrewm@0: 				totalHistoryFrames = 1;
andrewm@0: 			}			
andrewm@0: 		}
andrewm@0: 		
andrewm@0: 		// Recalculate the nominal sample interval by examining the difference in times
andrewm@0: 		// between first and last frames in the buffer.
andrewm@0: 		
andrewm@0: 		currentSampleInterval_ = (history_.latestTimestamp() - history_.earliestTimestamp()) / (timestamp_diff_type)totalHistoryFrames;
andrewm@0: 		
andrewm@0: 		// The frame time was just incremented by the current sample period.  Check whether
andrewm@0: 		// this puts the frame time ahead of the clock time.  Don't allow the frame time to get
andrewm@0: 		// ahead of the system clock (this will also push future frame timestamps back).
andrewm@0: 		
andrewm@0: 		if(frameTime > clockTime) {
andrewm@0: 			//cout << "CLIP " << 100.0 * (frameTime - clockTime) / currentSampleInterval_ << "%: frame=" << frameTime << " to clock=" << clockTime << endl;
andrewm@0: 			frameTime = clockTime;			
andrewm@0: 		}
andrewm@0: 		
andrewm@0: 		bufferLengthCounter_++;
andrewm@0: 		
andrewm@0: 		if(bufferLengthCounter_ >= kTimestampSynchronizerHistoryLength) {
andrewm@0: 			//timestamp_diff_type currentLatency = clockTime - frameTime;
andrewm@0: 			timestamp_diff_type maxLatency = 0, minLatency = 1000000.0;
andrewm@0: 			
andrewm@0: 			Node<pair<int, timestamp_type> >::iterator it;
andrewm@0: 			
andrewm@0: 			for(it = history_.begin(); it != history_.end(); ++it) {
andrewm@0: 				timestamp_diff_type l = (it.timestamp() - it->second);
andrewm@0: 				if(l > maxLatency)
andrewm@0: 					maxLatency = l;
andrewm@0: 				if(l < minLatency)
andrewm@0: 					minLatency = l;
andrewm@0: 			}
andrewm@0: 			
andrewm@0: 			//cout << "frame " << rawFrameNumber << ": rate = " << currentSampleInterval_ << " clock = " << clockTime << " frame = " << frameTime << " latency = " 
andrewm@0: 			//	<< currentLatency << " max = " << maxLatency << " min = " << minLatency << endl;
andrewm@0: 			
andrewm@0: 			//timestamp_diff_type targetMinLatency = (maxLatency - minLatency) * 2.0 / sqrt(kTimestampSynchronizerHistoryLength);
andrewm@0: 			
andrewm@0: 			/*if(minLatency > targetMinLatency) {
andrewm@0: 				cout << "ADDING " << 50.0 * (minLatency - targetMinLatency) / (currentSampleInterval_) << "%: (target " << targetMinLatency << ")\n";
andrewm@0: 				frameTime += (minLatency - targetMinLatency) / 2.0;
andrewm@0: 			}*/
andrewm@0: 			//frameTime += minLatency / 4.0;
andrewm@0: 			
andrewm@0: 			bufferLengthCounter_ = 0;
andrewm@0: 		}
andrewm@0: 	}
andrewm@0: 	
andrewm@0: 	// Insert the new frame time and clock times into the buffer
andrewm@0: 	history_.insert(pair<int, timestamp_type>(rawFrameNumber, frameTime), clockTime);
andrewm@0: 
andrewm@0: 	// The timestamp we return is associated with the frame, not the clock (which is potentially much
andrewm@0: 	// higher jitter)
andrewm@0: 	return frameTime;
andrewm@0: }