giuliomoro@135: #include "VirtualClock.h"
giuliomoro@135: void VirtualClock::init(){
giuliomoro@135: 	firstRun=true;
giuliomoro@141: 	movingAverage.setLength(101); //TODO: a better filtering algorithm ( Did you say Kalman?)
giuliomoro@135: 	period=-1;
giuliomoro@141: 	elapsedPeriods=0;
giuliomoro@141: 	startTime=0;
giuliomoro@141: 	startTimeOffset=0;
giuliomoro@141: 	elapsedPeriodsOffset=0;
giuliomoro@135: }
giuliomoro@135: 	
giuliomoro@135: VirtualClock::VirtualClock(){
giuliomoro@135: 	init();
giuliomoro@135: }
giuliomoro@135: void VirtualClock::sync(){
giuliomoro@135: 	sync(1);
giuliomoro@135: }
giuliomoro@141: void VirtualClock::sync(double numPeriods){
giuliomoro@135: 	myClock_t currentTime=Clock::getTimeUs();
giuliomoro@141: 	static int calls=0;
giuliomoro@141: 	elapsedPeriods+=numPeriods;
giuliomoro@141: 	if(calls==50){ //TODO: this is dangerous as the clock might jump suddenly if currentTime is not precise
giuliomoro@141: 		calls=0;
giuliomoro@141: 		startTimeOffset=startTime;
giuliomoro@141: 		startTime=currentTime;
giuliomoro@141: 		elapsedPeriodsOffset=elapsedPeriods;
giuliomoro@141: 	}
giuliomoro@141: 	calls++;
giuliomoro@135: 	if(firstRun==true){
giuliomoro@135: 		firstRun=false;
giuliomoro@135: 		startTime=currentTime;
giuliomoro@135: 	} else {
giuliomoro@141: 		double newPeriod=(currentTime-lastSync)/numPeriods;
giuliomoro@141: 		double expectedPeriod=22.67;
giuliomoro@141: 		double maxPeriodDeviation=10;
giuliomoro@141: 		if(fabs(newPeriod-expectedPeriod)<maxPeriodDeviation){ // filtering outliers
giuliomoro@141: 			period=movingAverage.add(newPeriod); //TODO: replace with Kalman filter
giuliomoro@141: 		} else {
giuliomoro@141: 			printf("period out of range: %f\n", newPeriod);
giuliomoro@141: 		}
giuliomoro@135: 	}
giuliomoro@135: 	lastSync=currentTime;
giuliomoro@139: //	printf("lastSync: %lld\n",lastSync-startTime);
giuliomoro@135: }
giuliomoro@135: 
giuliomoro@135: double VirtualClock::getNow(){
giuliomoro@139: 	myClock_t currentSystemTime=Clock::getTimeUs();
giuliomoro@135: 	if(period<=0){
giuliomoro@141: 		return currentSystemTime; // TODO: this is not very meaningful.
giuliomoro@135: 	}
giuliomoro@135: 	//  double beginningOfPeriod=lastSync; // TODO: if sync() does not get called every time (but e.g. only every so often),
giuliomoro@135: 													 // then this line (and the class) needs editing
giuliomoro@139: 	myClock_t elapsed=(currentSystemTime-startTime);
giuliomoro@141: 	double now=elapsedPeriodsOffset + elapsed/(double)period;
giuliomoro@139: //	printf("elapsed=%lld;  sincelastSync=%lld; period=%f; now=%f\n", elapsed, currentSystemTime-lastSync, period, now);
giuliomoro@139: 	return now;
giuliomoro@135: }
giuliomoro@135: 
giuliomoro@141: void VirtualClock::addOffset(double periodOffset){
giuliomoro@141: 	elapsedPeriods-=periodOffset;
giuliomoro@141: 	if(elapsedPeriods<0){
giuliomoro@141: 		printf("ERROR: periodOffset adjustment of %f resulted in elapsedPeriods=%f\n", periodOffset, elapsedPeriods);
giuliomoro@141: 		exit(1);
giuliomoro@141: 	}
giuliomoro@141: 	movingAverage.reset();
giuliomoro@141: }
giuliomoro@135: double VirtualClock::getPeriod(){
giuliomoro@135: 	return period;
giuliomoro@135: }