giuliomoro@135: #include "ClockSync.h"
giuliomoro@135: 
giuliomoro@135: void ClockSync::setVirtualClock(VirtualClock &aVirtualClock){
giuliomoro@135: 	virtualClock=&aVirtualClock;
giuliomoro@135: }
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@135: 	T1=-1;
giuliomoro@135: 	T1p=-1;
giuliomoro@135: 	T2=-1;
giuliomoro@135: 	T2p=-1;
giuliomoro@135: 	receiveLoopSleepUs=10;
giuliomoro@135: 	receiveLoopTimeout=1e5;
giuliomoro@135: 	movingAverage.setLength(201);
giuliomoro@135: 	expectedClockSyncType=isSlave() ? kSync : kNone;
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@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@135: 	ret=client.waitUntilReady(false, 0);
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@135: 				//  //printf("Socket not ready to be read: %lld\n", localTimestamp);
giuliomoro@135: 			}
giuliomoro@135: 			else if(localTimestamp==-1){
giuliomoro@135: 				//printf("Error while receiving: %lld\n", localTimestamp);
giuliomoro@135: 			}
giuliomoro@135: 			else if(localTimestamp==-2){
giuliomoro@135: 				//  //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@135: 		//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@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@135: 			double offset=(T1p-T1-T2p+T2)/2.0d;
giuliomoro@135: 			printf("-----------OFFSET IS : %04.1f, average: %04.2f\n", offset, movingAverage.add(offset));
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@135: 
giuliomoro@135: int ClockSync::masterHandleMessage(){
giuliomoro@135: 	switch(clockSyncType){
giuliomoro@135: 		case kDelayReq:
giuliomoro@135: 			//TODO: do something with it
giuliomoro@135: 			//send kDelayResp
giuliomoro@135: 			setType(kDelayResp);
giuliomoro@135: 			setTimestamp(localTimestamp);
giuliomoro@135: 			send();
giuliomoro@135: 			expectedClockSyncType=kNone;
giuliomoro@135: 		break;
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@135: 		usleep(100000); //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@135: