giuliomoro@135: #include "VirtualClock.h"
giuliomoro@149: void VirtualClock::init(float initialValueUs){
giuliomoro@135: 	firstRun=true;
giuliomoro@141: 	movingAverage.setLength(101); //TODO: a better filtering algorithm ( Did you say Kalman?)
giuliomoro@149: 	blockPeriod=-1;
giuliomoro@141: 	elapsedPeriods=0;
giuliomoro@141: 	startTime=0;
giuliomoro@149: 	elapsedPeriodsOffset = 0;
giuliomoro@149: 	lastSyncEstimatedTime = 0;
giuliomoro@149: 	double coefficients[IIR_FILTER_STAGE_COEFFICIENTS] = {
giuliomoro@149: 			0.000241359049041961, 0.000482718098083923, 0.000241359049041961, -1.95557824031504, 0.956543676511203
giuliomoro@149: 	};
giuliomoro@149: 	iir.setNumberOfStages(4);
giuliomoro@149: 	iir.setCoefficients(coefficients);
giuliomoro@149: 	printf("kalmanInit=%f\n",initialValueUs);
giuliomoro@149: 	kalman.init(0.01, 158575.715009816 /*measured var() */,
giuliomoro@149: 			initialValueUs
giuliomoro@149: //#ifdef USE_JUCE
giuliomoro@149: //			5804.98866213152 /*blockSize/Fs*1e6*/
giuliomoro@149: //#else
giuliomoro@149: //			5804.98866213152 /*blockSize/Fs*1e6*/
giuliomoro@149: //#endif
giuliomoro@149: 			);
giuliomoro@135: }
giuliomoro@135: 	
giuliomoro@149: VirtualClock::VirtualClock(){}
giuliomoro@135: void VirtualClock::sync(){
giuliomoro@135: 	sync(1);
giuliomoro@135: }
giuliomoro@141: void VirtualClock::sync(double numPeriods){
giuliomoro@149: 	myClock_t currentTime = Clock::getTimeUs();
giuliomoro@149: 	elapsedPeriods += numPeriods;
giuliomoro@149: 	static int calls = 0;
giuliomoro@149: 	if(calls == 0){
giuliomoro@149: 		startTime = currentTime;
giuliomoro@149: //AA		lastSyncEstimatedTime = startTime;
giuliomoro@149: 		lastSyncEstimatedTime = 0;
giuliomoro@149: 	} else {
giuliomoro@149: 		double newBlockPeriod = (currentTime - lastSyncTime);
giuliomoro@149: //		period = iir.process(movingAverage.add(newPeriod)); //TODO: replace with Kalman filter
giuliomoro@149: 		blockPeriod = iir.process(kalman.process(newBlockPeriod));
giuliomoro@149: //		period = 22.6760958210091;
giuliomoro@149: 		period = blockPeriod / numPeriods;
giuliomoro@149: 		lastSyncEstimatedTime += (period * numPeriods);
giuliomoro@149: //		if(calls == 800)
giuliomoro@149: //			lastSyncEstimatedTime = lastSyncTime;
giuliomoro@149: //		printf("%lld, %lld\n", lastSyncTime, lastSyncEstimatedTime);
giuliomoro@141: 	}
giuliomoro@149: 	lastSyncTime = currentTime;
giuliomoro@149: //	printf("%lld\n", lastSyncTime);
giuliomoro@141: 	calls++;
giuliomoro@135: }
giuliomoro@135: 
giuliomoro@135: double VirtualClock::getNow(){
giuliomoro@139: 	myClock_t currentSystemTime=Clock::getTimeUs();
giuliomoro@149: 	if(blockPeriod<=0){
giuliomoro@141: 		return currentSystemTime; // TODO: this is not very meaningful.
giuliomoro@135: 	}
giuliomoro@149: 	double elapsed = (currentSystemTime - startTime) - lastSyncEstimatedTime;
giuliomoro@149: 	double now = elapsedPeriods + elapsed / (double)period;
giuliomoro@149: //	if(now>currentSystemTime+10*1e6)
giuliomoro@149: //		now=0;
giuliomoro@149: //	static long long int pastSy=0;
giuliomoro@149: //	printf("%lld\n", currentSystemTime-pastSy);
giuliomoro@149: //	pastSy=currentSystemTime;
giuliomoro@149: 	static int count=0;
giuliomoro@149: 	count++;
giuliomoro@149: //	if(count&1)
giuliomoro@149: #ifdef USE_JUCE
giuliomoro@149: #else
giuliomoro@149: //	printf("%f %f %f\n", (currentSystemTime - startTime)/1e6*44100.0, blockPeriod, now);
giuliomoro@149: #endif
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@149: 	printf("this is not called\n");
giuliomoro@141: 	movingAverage.reset();
giuliomoro@141: }
giuliomoro@135: double VirtualClock::getPeriod(){
giuliomoro@149: 	return blockPeriod;
giuliomoro@135: }