giuliomoro@135: #include "VirtualClock.h"
giuliomoro@135: void VirtualClock::init(){
giuliomoro@135: 	firstRun=true;
giuliomoro@135: 	movingAverage.setLength(31); //TODO: a better filtering algorithm ( Did you say Kalman?)
giuliomoro@135: 	period=-1;
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@135: void VirtualClock::sync(double count){
giuliomoro@135: 	myClock_t currentTime=Clock::getTimeUs();
giuliomoro@135: 	if(firstRun==true){
giuliomoro@135: 		firstRun=false;
giuliomoro@135: 		startTime=currentTime;
giuliomoro@135: 	} else {
giuliomoro@135: 		period=movingAverage.add((currentTime-lastSync)/count); //TODO: replace with Kalman filter
giuliomoro@135: 	}
giuliomoro@135: 	lastSync=currentTime;
giuliomoro@135: }
giuliomoro@135: 
giuliomoro@135: double VirtualClock::getNow(){
giuliomoro@135: 	myClock_t now=Clock::getTimeUs();
giuliomoro@135: 	if(period<=0){
giuliomoro@135: 		return now;
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@135: 	myClock_t elapsed=(now-startTime);
giuliomoro@135: 	double frac=elapsed/(double)period;
giuliomoro@135: 	//  printf("now=%lld; beginningOfPeriod=%f; lastSync=%lld; period=%lld; frac=%f\n", now, beginningOfPeriod, lastSync, period, frac);
giuliomoro@135: 	return frac;
giuliomoro@135: }
giuliomoro@135: 
giuliomoro@135: double VirtualClock::getPeriod(){
giuliomoro@135: 	return period;
giuliomoro@135: }