giuliomoro@135: #include "ClockSync.h"
giuliomoro@135: 
giuliomoro@135: void ClockSync::setVirtualClock(VirtualClock &aVirtualClock){
giuliomoro@135: 	virtualClock=&aVirtualClock;
giuliomoro@135: }
giuliomoro@152: Pid* gClockSyncPid;
giuliomoro@135: void ClockSync::init(bool thisIsSlave, int aPort, VirtualClock &aVirtualClock){
giuliomoro@135: 	setVirtualClock(aVirtualClock);
giuliomoro@135: 	slave=thisIsSlave;
giuliomoro@135: 	setPort(aPort);
giuliomoro@135: 	//  isSlave() ? client.setServer("127.0.0.1") : client.setServer("127.0.0.1");
giuliomoro@135: 	isSlave() ? client.setServer("192.168.7.1") : client.setServer("192.168.7.2");
giuliomoro@135: 	bufferLength=kSyncMessageLength;
giuliomoro@141: 	resetTs();
giuliomoro@141: 	receiveLoopSleepUs=100;
giuliomoro@141: 	receiveLoopTimeout=1e5;
giuliomoro@141: 	movingAverage.setLength(31);
giuliomoro@141: 	expectedClockSyncType=isSlave() ? kSync : kNone;
giuliomoro@152: 	gClockSyncPid = &pid;
giuliomoro@141: }
giuliomoro@141: void ClockSync::resetTs(){
giuliomoro@135: 	T1=-1;
giuliomoro@135: 	T1p=-1;
giuliomoro@135: 	T2=-1;
giuliomoro@135: 	T2p=-1;
giuliomoro@141: }
giuliomoro@141: bool ClockSync::areTsValid(){
giuliomoro@141: 	return T1>0 && T1p>0 && T2>0 && T2p>0;
giuliomoro@135: }
giuliomoro@135: ClockSync::ClockSync(bool thisIsSlave, int aPort, VirtualClock &aVirtualClock){
giuliomoro@135: 	init(thisIsSlave, aPort, aVirtualClock);
giuliomoro@135: }
giuliomoro@135: void* ClockSync::getBuffer(){
giuliomoro@135: 	return buffer;
giuliomoro@135: }
giuliomoro@135: bool ClockSync::isSlave(){
giuliomoro@135: 	return slave;
giuliomoro@135: }
giuliomoro@135: bool ClockSync::isMaster(){
giuliomoro@135: 	return !slave;
giuliomoro@135: }
giuliomoro@135: int ClockSync::getType(){
giuliomoro@135: 	return ((int*)buffer)[0];
giuliomoro@135: }
giuliomoro@135: myClock_t ClockSync::getTimestamp(){
giuliomoro@135: 	return *((myClock_t*)((char*)buffer+sizeof(int)));
giuliomoro@135: }
giuliomoro@135: void ClockSync::setType(int clockSyncType){
giuliomoro@135: 	((int*)buffer)[0]=clockSyncType;
giuliomoro@135: }
giuliomoro@135: void ClockSync::setTimestamp(myClock_t timestamp){
giuliomoro@139: //	printf("setting timestamp: %lld\n", timestamp);
giuliomoro@135: 	((myClock_t*)((char*)buffer+sizeof(int)))[0]=timestamp;
giuliomoro@135: }
giuliomoro@135: void ClockSync::print(){
giuliomoro@135: 	 //printf("type: %d, timestamp: %lld\n",*((int*)buffer),*((myClock_t*)(((int*)buffer)+1)));
giuliomoro@135: }
giuliomoro@135: void ClockSync::setPort(int aPort){
giuliomoro@135: 	if(aPort>0){
giuliomoro@135: 		int inPort=isSlave() ? aPort : aPort+1;
giuliomoro@135: 		int outPort=isSlave() ? aPort+1: aPort;
giuliomoro@135: 		server.bindToPort(inPort);
giuliomoro@135: 		client.setPort(outPort);
giuliomoro@135: 		//printf("Receiving on port %d\n", inPort);
giuliomoro@135: 		//printf("Sending to port %d\n", outPort);
giuliomoro@135: 	}
giuliomoro@135: }
giuliomoro@135: /** 
giuliomoro@135:  * sends a clockSync without blocking, checks results and returns the timestamp
giuliomoro@135:  * immediately after the clockSync has been sent or -1 if there was an error or timeout expired.
giuliomoro@135: */
giuliomoro@135: myClock_t ClockSync::send(){
giuliomoro@135: 	//  print();
giuliomoro@135: 	int ret;
giuliomoro@149: 	ret=client.waitUntilReady(false, isSlave() ? 110 : 5);
giuliomoro@135: 	if(ret<=0){ //don't retry
giuliomoro@135: 		return -1;
giuliomoro@135: 	}
giuliomoro@135: 	ret=client.send(buffer, bufferLength);
giuliomoro@135: 	myClock_t timestamp=(myClock_t)virtualClock->getNow();
giuliomoro@135: 	if(ret<0){
giuliomoro@135: 	//if there was an error while sending, don't retry
giuliomoro@135: 		return -1;
giuliomoro@135: 	}
giuliomoro@135: 	return timestamp; //get the accurate time *after* you sent the sync clockSync
giuliomoro@135: }
giuliomoro@135: /** 
giuliomoro@135:  * receives a clockSync without blocking, checks results and returns the timestamp
giuliomoro@135:  * immediately after the clockSync has been received, or -1 if there was an error 
giuliomoro@135:  * or 0 if timeout expired.
giuliomoro@135: */
giuliomoro@135: myClock_t ClockSync::receive(){
giuliomoro@135: 	int ret;
giuliomoro@135: 	ret=server.waitUntilReady(true, 0);
giuliomoro@135: 	if(ret<=0){ //don't retry
giuliomoro@135: 		return 0;
giuliomoro@135: 	}
giuliomoro@135: 	ret=server.read(buffer, bufferLength, false);
giuliomoro@135: 	myClock_t timestamp=(myClock_t)virtualClock->getNow();
giuliomoro@135: 	if(timestamp==0){
giuliomoro@135: 		//printf("The virtualClock period is <=0\n");
giuliomoro@135: 		return -3;
giuliomoro@135: 	}
giuliomoro@135: 	if(ret==-1){
giuliomoro@135: 		//if there was an error while receiving, don't retry
giuliomoro@135: 		return -1;
giuliomoro@135: 	}
giuliomoro@135: 	if(ret!=bufferLength){
giuliomoro@135: 		//printf("Received a clockSync of the wrong size: %d\n", ret);
giuliomoro@135: 		return -2;
giuliomoro@135: 	}
giuliomoro@135: 	//  print();
giuliomoro@135: 	return timestamp; //get the accurate time *after* you sent the sync clockSync
giuliomoro@135: }
giuliomoro@135: 
giuliomoro@135: int ClockSync::masterSendSync(){
giuliomoro@135: 	//let's send a sync clockSync!
giuliomoro@135: 	//printf("Sending a sync clockSync\n");
giuliomoro@135: 	setType(kSync);
giuliomoro@135: 	setTimestamp(-1);//do not care about sending the timestamp, a more accurate one will be sent in the follow up
giuliomoro@135: 	localTimestamp=send();
giuliomoro@135: 	if(localTimestamp<0){
giuliomoro@135: 		//printf("Could not send sync clockSync\n");
giuliomoro@135: 		return -1; //error, don't retry, start over
giuliomoro@135: 	}
giuliomoro@135: 	//let's send a followUp
giuliomoro@135: 	//printf("sent a sync clockSync\n");
giuliomoro@135: 	setType(kFollowUp);
giuliomoro@135: 	setTimestamp(localTimestamp);
giuliomoro@135: 	if(localTimestamp<0){
giuliomoro@135: 		//printf("Could not send followup clockSync\n");
giuliomoro@135: 		return -2; //error, don't retry, start over
giuliomoro@135: 	}
giuliomoro@135: 	int ret=send();
giuliomoro@135: 	if(ret<0){
giuliomoro@135: 		//printf("Error while sending followup\n");
giuliomoro@135: 		return -3;
giuliomoro@135: 	}
giuliomoro@135: 	//printf("sent a followUp clockSync\n");
giuliomoro@135: 	expectedClockSyncType=kDelayReq;
giuliomoro@135: 	return 1;
giuliomoro@135: }
giuliomoro@135: #ifdef USE_JUCE
giuliomoro@135: #define NOTGSHOULDSTOP 1
giuliomoro@135: #else
giuliomoro@135: extern bool gShouldStop;
giuliomoro@135: #define NOTGSHOULDSTOP (!gShouldStop)
giuliomoro@135: #endif /* USE_JUCE */
giuliomoro@135: int ClockSync::receiveLoop(){
giuliomoro@135: 	int receiveLoopElapsed=0;
giuliomoro@135: 	while( NOTGSHOULDSTOP && (isSlave() || (receiveLoopElapsed<receiveLoopTimeout))){ //when slave, does not timeout!
giuliomoro@135: 		receiveLoopElapsed+=receiveLoopSleepUs;
giuliomoro@135: 		usleep(receiveLoopSleepUs); //how often to check for new clockSyncs;
giuliomoro@135: 		//   //printf("waiting for clockSyncs\n");
giuliomoro@135: 		localTimestamp=receive();
giuliomoro@135: 		if(localTimestamp<=0){
giuliomoro@135: 			if(localTimestamp==0){
giuliomoro@139: //				  printf("Socket not ready to be read: %lld\n", localTimestamp);
giuliomoro@135: 			}
giuliomoro@135: 			else if(localTimestamp==-1){
giuliomoro@139: 				printf("Error while receiving: %lld\n", localTimestamp);
giuliomoro@135: 			}
giuliomoro@135: 			else if(localTimestamp==-2){
giuliomoro@139: 				  printf("Wrong size of the received clockSync: %lld\n", localTimestamp);
giuliomoro@135: 			}
giuliomoro@135: 			continue ; //keep waiting
giuliomoro@135: 		}
giuliomoro@135: 		clockSyncType=getType();
giuliomoro@135: 		clockSyncTimestamp=getTimestamp();
giuliomoro@135: 		if(clockSyncType!=expectedClockSyncType){
giuliomoro@135: 			//printf("Wrong clockSync type: %d, expected: %d\n",clockSyncType, expectedClockSyncType);
giuliomoro@135: 			return -2; //start over
giuliomoro@135: 		}
giuliomoro@139: //		printf("Received clockSync type: %d, clockSyncTimestamp: %lld\n", clockSyncType, clockSyncTimestamp);
giuliomoro@135: 		if(isSlave()==true){
giuliomoro@135: 			int ret=slaveHandleMessage();
giuliomoro@135: 			if(ret==1 && clockSyncType==kDelayResp){ //we are done, end of a cycle!
giuliomoro@135: 				return 1;
giuliomoro@135: 			} else if (ret!=1) {
giuliomoro@135: 				return -1; //
giuliomoro@135: 			} else {
giuliomoro@135: 				continue;
giuliomoro@135: 			}
giuliomoro@135: 		}
giuliomoro@135: 		if(isMaster()==true){ //this is master
giuliomoro@135: 			int ret=masterHandleMessage();
giuliomoro@135: 			if(ret==1 && clockSyncType==kDelayReq){ //we are done, end of a cycle!
giuliomoro@135: 				return 1;
giuliomoro@135: 			} else {
giuliomoro@135: 				return -2; //we are done but something was wrong
giuliomoro@135: 			}
giuliomoro@135: 		}
giuliomoro@135: 	}
giuliomoro@135: 	//printf("Receive loop timeout\n");
giuliomoro@135: 	return -1;
giuliomoro@135: }
giuliomoro@135: 
giuliomoro@135: int ClockSync::slaveHandleMessage(){
giuliomoro@135: 	switch(clockSyncType){
giuliomoro@135: 		case kSync: //the clockSync timestamp is meaningless, the localTimestamp is when kSync was received
giuliomoro@141: 			resetTs();
giuliomoro@135: 			T1p=localTimestamp;
giuliomoro@135: 			expectedClockSyncType=kFollowUp;
giuliomoro@135: 			break;
giuliomoro@135: 		case kFollowUp: //the clockSyncTimestamp is the time when kSync was sent, the localTimestamp is meaningless
giuliomoro@135: 			T1=clockSyncTimestamp;
giuliomoro@135: 			//send delayReq
giuliomoro@135: 			setType(kDelayReq);
giuliomoro@135: 			setTimestamp(-1);
giuliomoro@135: 			T2=send();
giuliomoro@135: 			if(T2<0){
giuliomoro@135: 				//printf("Error while sending delayReq\n");
giuliomoro@135: 				return -1;
giuliomoro@135: 			}
giuliomoro@135: 			expectedClockSyncType=kDelayResp;
giuliomoro@135: 			break;
giuliomoro@135: 		case kDelayResp: {//the clockSyncTimestamp is the instant when the master received the kDelayResp clockSync, the localTimestamp is meaningless
giuliomoro@135: 			T2p=clockSyncTimestamp;
giuliomoro@135: 			//TODO: evaluate things
giuliomoro@141: 			if(areTsValid()){
giuliomoro@152: 				offset = (T1p-T1-T2p+T2)/2.0d;
giuliomoro@152: 				processOffset();
giuliomoro@141: 			} else {
giuliomoro@141: 				printf("InvalidTs:\n  %lld, %lld, %lld, %lld \n", T1, T1p, T2, T2p);
giuliomoro@141: 			}
giuliomoro@135: 			expectedClockSyncType=kSync; //end of the cycle, wait for next sync.
giuliomoro@135: 			break;
giuliomoro@135: 		}
giuliomoro@135: 		default:
giuliomoro@135: 			//printf("Unexpected message type\n"); // we should never get here
giuliomoro@135: 			return -1;
giuliomoro@135: 	}
giuliomoro@135: 	return 1;
giuliomoro@135: }
giuliomoro@152: void ClockSync::processOffset(){
giuliomoro@141: 	static int calls=0;
giuliomoro@141: 	// TODO: change the flow control below so that it can happen multiple times
giuliomoro@141: 	//(base it upon the length of movingAverage rather than the number of calls)
giuliomoro@151: 	if(calls<30){
giuliomoro@151: 		 // wait for the VirtualClock to stabilize and produce meaningful outputs
giuliomoro@151: 	} else if(calls<50) { //get an initial guess
giuliomoro@141: 		movingAverage.add(offset);
giuliomoro@141: //		printf("-----------OFFSET IS : %04.4f samples, average: %04.4f samples\n",
giuliomoro@141: //				offset, movingAverage.getAverage());
giuliomoro@151: 	} else if (calls==50){ //then compensate for initial offset
giuliomoro@151: //		printf("compensating for offset: %f\n", movingAverage.getAverage());
giuliomoro@141: 		virtualClock->addOffset(movingAverage.getAverage());
giuliomoro@141: 		movingAverage.reset();
giuliomoro@151: 	} else if (calls>50){ //use IIR filter from now on
giuliomoro@141: 		//filter coefficients obtained from Matlab : [B,A]=butter(2,0.005);
giuliomoro@151: 		static IirFilter iir(1);
giuliomoro@151: 		static bool init = false;
giuliomoro@151: 		if(init == false){
giuliomoro@152: 			//cutoff 0.05
giuliomoro@151: //			double coeffs[5]={0.00554271721028068, 0.0110854344205614, 0.00554271721028068,
giuliomoro@151: //							     -1.77863177782459, 0.800802646665708};
giuliomoro@152: 			//cutoff 0.015
giuliomoro@152: 			double coeffs[5]={0.000537169774812052, 0.0010743395496241, 0.000537169774812052,
giuliomoro@152: 					-1.93338022587993, 0.935528904979178};
giuliomoro@152: //			double coeffs[5]={0.0200833655642113, 0.0401667311284225, 0.0200833655642113,
giuliomoro@152: //						-1.56101807580072, 0.641351538057563};
giuliomoro@151: 			iir.setCoefficients(coeffs);
giuliomoro@151: 			init = true;
giuliomoro@151: 		}
giuliomoro@151: 		float expectedOffsetDifferenceMean = 0.2432;
giuliomoro@152: 		float expectedOffsetDifference = 3.5;
giuliomoro@152: 		static float pastNlOffset = 0;
giuliomoro@152: 		static float pastIncrement = 0;
giuliomoro@152: 		float offsetIncrement = offset-pastNlOffset;
giuliomoro@152: 		static int filteredOut=0;
giuliomoro@152: 		if (calls>51 && (fabsf(offsetIncrement) > expectedOffsetDifference && filteredOut < 8)){
giuliomoro@151: 			// non-linear filtering: remove outliers
giuliomoro@152: 			nlOffset = pastNlOffset;
giuliomoro@152: //			printf("this actually happened\n");
giuliomoro@152: //nlOffset=offset; // BYPASS non linear filter
giuliomoro@151: //			printf("%f %f                                             0,0,0,0,0,0\n", offset,pastOffset);
giuliomoro@152: 			filteredOut++;
giuliomoro@151: 		} else {
giuliomoro@151: 			nlOffset=offset;
giuliomoro@152: 			filteredOut = 0;
giuliomoro@151: 		}
giuliomoro@152: 		pastIncrement = nlOffset - pastNlOffset;
giuliomoro@152: 		pastNlOffset = nlOffset;
giuliomoro@152: 		iirOffset = iir.process(nlOffset);
giuliomoro@141: 		static float maxOffset=0;
giuliomoro@151: 		static float pastIirOffset=0;
giuliomoro@151: 		if( calls > 0 ) {
giuliomoro@151: 			maxOffset=fabsf(iirOffset) > fabsf(maxOffset) ? iirOffset : maxOffset;
giuliomoro@151: 			pid.setError(iirOffset);
giuliomoro@152: 			float correction=pid.getOutput();
giuliomoro@149: 			static float oldSamplingRate=44100;
giuliomoro@152: 			if( (calls&0) == 0){
giuliomoro@152: 				printf("%10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f\n",
giuliomoro@152: 						offset, nlOffset, iirOffset, iirOffset - pastIirOffset,
giuliomoro@152: 						pid.getProportionalGain(), pid.getIntegralGain(), pid.getDerivativeGain(), pid.getGlobalGain(),
giuliomoro@152: 						correction, pid.getIntegralError(), oldSamplingRate); //unfiltered, filtered
giuliomoro@152: 			}
giuliomoro@151: 			pastIirOffset = iirOffset;
giuliomoro@151: 			float targetSamplingRate;
giuliomoro@152: 			// Applit PID
giuliomoro@152: 			targetSamplingRate = oldSamplingRate - correction;
giuliomoro@152: 			if(targetSamplingRate> 44110) //clip the Pid !
giuliomoro@152: 				targetSamplingRate = 44110;
giuliomoro@152: 			if (targetSamplingRate < 44090)
giuliomoro@152: 				targetSamplingRate = 44090;
giuliomoro@152: 
giuliomoro@152: 			// or use a naive hysteresis comparator
giuliomoro@151: 			static int direction=1;
giuliomoro@151: 			float thresholdL = -1;
giuliomoro@151: 			float thresholdH = 3;
giuliomoro@151: 			targetSamplingRate = 44100;
giuliomoro@151: 			if(
giuliomoro@151: 				(iirOffset > thresholdH && direction == 1) ||
giuliomoro@151: 				(iirOffset > thresholdL && direction == -1)
giuliomoro@151: 			){
giuliomoro@151: 				targetSamplingRate = 44097;
giuliomoro@151: 				direction = -1;
giuliomoro@151: 			}
giuliomoro@151: 			else if (
giuliomoro@151: 				(iirOffset < -thresholdH && direction == -1) ||
giuliomoro@151: 				(iirOffset < -thresholdL && direction == 1)
giuliomoro@151: 			)
giuliomoro@151: 			{
giuliomoro@152: 				iirOffset = 44103;
giuliomoro@151: 				direction = 1;
giuliomoro@151: 			}
giuliomoro@151: #ifndef USE_JUCE
giuliomoro@151: 			if(oldSamplingRate != targetSamplingRate){
giuliomoro@151: 				gAudioCodec->setAudioSamplingRate(targetSamplingRate);
giuliomoro@151: 			}
giuliomoro@151: #endif
giuliomoro@149: 			oldSamplingRate=targetSamplingRate;
giuliomoro@141: 		}
giuliomoro@141: 	}
giuliomoro@141: 	calls++;
giuliomoro@141: }
giuliomoro@135: int ClockSync::masterHandleMessage(){
giuliomoro@135: 	switch(clockSyncType){
giuliomoro@135: 		case kDelayReq:
giuliomoro@135: 			//send kDelayResp
giuliomoro@135: 			setType(kDelayResp);
giuliomoro@135: 			setTimestamp(localTimestamp);
giuliomoro@135: 			send();
giuliomoro@135: 			expectedClockSyncType=kNone;
giuliomoro@141: 			return 1;
giuliomoro@135: 		break;
giuliomoro@141: 		default:
giuliomoro@141: 			return -1;
giuliomoro@135: 	}
giuliomoro@135: }
giuliomoro@135: 
giuliomoro@135: int ClockSync::sendReceiveLoop(){
giuliomoro@135: 	if(isSlave()==true){
giuliomoro@135: 		 //printf("Waiting for a sync clockSync\n");
giuliomoro@135: 	} else { //if this is master
giuliomoro@152: 		usleep(10000); //this times (roughly) how often sync clockSyncs are being sent.
giuliomoro@135: 		int ret=masterSendSync();
giuliomoro@135: 		if(ret<=0)
giuliomoro@135: 			return -1;
giuliomoro@135: 	}
giuliomoro@135: 	int ret=receiveLoop();
giuliomoro@135: 	if(ret<=0)
giuliomoro@135: 		return -2;
giuliomoro@135: 	return 1;
giuliomoro@135: }
giuliomoro@135: 
giuliomoro@152: float ClockSync::getOffset(){
giuliomoro@152: 	return offset;
giuliomoro@152: }
giuliomoro@152: float ClockSync::getIirOffset(){
giuliomoro@152: 	return iirOffset;
giuliomoro@152: }