giuliomoro@217: #include "ReceiveAudioThread.h"
giuliomoro@217: 
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217: #else
giuliomoro@217: //initialise static members
giuliomoro@217: bool ReceiveAudioThread::staticConstructed=false;
giuliomoro@217: AuxiliaryTask ReceiveAudioThread::receiveDataTask=NULL;
giuliomoro@217: std::vector<ReceiveAudioThread *> ReceiveAudioThread::objAddrs(0);
giuliomoro@217: bool ReceiveAudioThread::threadRunning;
giuliomoro@217: bool ReceiveAudioThread::threadIsExiting;
giuliomoro@217: int ReceiveAudioThread::sleepTime;
giuliomoro@217: 
giuliomoro@217: void receiveData(){
giuliomoro@217: 	ReceiveAudioThread::run();
giuliomoro@217: }
giuliomoro@217: void ReceiveAudioThread::staticConstructor(){
giuliomoro@217: 	if(staticConstructed==true)
giuliomoro@217: 		return;
giuliomoro@217: 	staticConstructed=true;
giuliomoro@217:     threadIsExiting=false;
giuliomoro@217: 	receiveDataTask=BeagleRT_createAuxiliaryTask(receiveData, 90, "receiveDataTask"); //TODO: allow different priorities
giuliomoro@217: }
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@217: 
giuliomoro@217: void ReceiveAudioThread::dealloc(){
giuliomoro@217:     free(buffer);
giuliomoro@217:     buffer=NULL;
giuliomoro@217:     free(stackBuffer);
giuliomoro@217:     stackBuffer=NULL;
giuliomoro@217: }
giuliomoro@217: void ReceiveAudioThread::wrapWritePointer(){
giuliomoro@217:     //this is not quite a simple wrapping as you would do in a circular buffer,
giuliomoro@217:     //as there is no guarantee the buffer will be full at all times, given that there must alwas be enough space at the end of it
giuliomoro@217:     //to hold a full payload
giuliomoro@217:     // lastValidPointer indicates the last pointer in the buffer containing valid data
giuliomoro@217:     //
giuliomoro@217:     if(writePointer+payloadLength+headerLength>bufferLength){ //if we are going to exceed the length of the buffer with the next reading
giuliomoro@217:         //  lastValidPointer=writePointer+headerLength; //remember where the last valid data are
giuliomoro@217:         //  for(int n=headerLength;n<lastValidPointer; n++){
giuliomoro@217:             //  fprintf(fd2, "%f\n",buffer[n]); //DEBUG
giuliomoro@217:         //  }
giuliomoro@222:     	writePointer=0; //and reset to beginning of the buffer
giuliomoro@217:     }
giuliomoro@217: }
giuliomoro@217: void ReceiveAudioThread::pushPayload(int startIndex){ //backup the payload samples that will be overwritten by the new header
giuliomoro@217:     for(int n=0; n<headerLength; n++){
giuliomoro@217:         stackBuffer[n]=buffer[startIndex+n];
giuliomoro@217:     }
giuliomoro@217: }
giuliomoro@217: void ReceiveAudioThread::popPayload(int startIndex){
giuliomoro@217:     for(int n=0; n<headerLength; n++){
giuliomoro@217:         buffer[startIndex+n]=stackBuffer[n];
giuliomoro@217:     }        
giuliomoro@217: }
giuliomoro@217: 
giuliomoro@217: int ReceiveAudioThread::readUdpToBuffer(){
giuliomoro@222: 
giuliomoro@222: 	if(listening==false || bufferReady==false)
giuliomoro@217:         return 0;
giuliomoro@217:     if(writePointer<0)
giuliomoro@217:         return 0;
giuliomoro@217:     if(socket.waitUntilReady(true, waitForSocketTime)){// TODO: if waitForSocketTime here is >>5, the
giuliomoro@222:     	// destructor (always or sometimes) never actually gets called, despite run() returns ...see issue #1381
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217: #else
giuliomoro@217: 		lastTime=rt_timer_read();
giuliomoro@217: //        rt_printf("lastTimeread= %llu\n", lastTime);
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@217:         pushPayload(writePointer); //backup headerLength samples. This could be skipped if writePointer==0
giuliomoro@217:         //read header+payload
giuliomoro@217:         int numBytes=socket.read(buffer+writePointer, bytesToRead, true); //read without waiting.
giuliomoro@217:             //TODO: (if using variable-length payload) validate the actual numBytes read against the size declared in the header
giuliomoro@217:         if(numBytes<0){
giuliomoro@217:             printf("error numBytes1\n");
giuliomoro@217:             return -3; //TODO: something went wrong, you have to discard the rest of the packet!
giuliomoro@217:         }
giuliomoro@217:         if(numBytes==0){//TODO: this should not happen unless you actually receive a packet of size zero (is it at all possible?)
giuliomoro@217: //        	printf("received 0 bytes\n");
giuliomoro@217:         	return 0;
giuliomoro@217:         }
giuliomoro@217:         if(numBytes != bytesToRead){ //this is equivalent to (numBytes<bytesToRead)
giuliomoro@217:             printf("error numBytes2: %d\n", numBytes);
giuliomoro@217:             return -4; //TODO: something went wrong, less bytes than expected in the payload.
giuliomoro@217:         }
giuliomoro@217:         if(channel!=(int)buffer[writePointer]){
giuliomoro@217: //        	printf("I am channel %d, but I received data for channel %d\n", channel, (int)buffer[writePointer]);
giuliomoro@217:         	return -5;
giuliomoro@217:         }
giuliomoro@217:         if(buffer[writePointer+1]!=timestamp+1)
giuliomoro@217:         	printf("missing a timestamp: %d\n",timestamp+1);
giuliomoro@217:         timestamp=buffer[writePointer+1];
giuliomoro@217: //        rt_printf("Received a message of length %d, it was on channel %d and timestamp %d\n", numBytes, (int)buffer[writePointer], (int)buffer[writePointer+1]);
giuliomoro@217: 
giuliomoro@217:         popPayload(writePointer); //restore headerLength payload samples. This could be skipped if writePointer==0
giuliomoro@217:         //even though we just wrote (payloadLength+headerLength) samples in the buffer,
giuliomoro@217:         //we only increment by payloadLength. This way, next time a socket.read is performed, we will
giuliomoro@217:         //backup the last headerLength samples that we just wrote and we will overwrite them with
giuliomoro@217:         //the header from the new read. After parsing the header we will then restore the backed up samples.
giuliomoro@217:         //This way we guarantee that, apart from the first headerLength samples, buffer is a circular buffer!
giuliomoro@217:         writePointer+=payloadLength;
giuliomoro@217:         wrapWritePointer();
giuliomoro@217:         return numBytes;
giuliomoro@217:     }
giuliomoro@217:     return 0; //timeout occurred
giuliomoro@217: }
giuliomoro@217: //USE_JUCE    Thread(threadName),
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217: ReceiveAudioThread::ReceiveAudioThread(const String &threadName) :
giuliomoro@217: 			Thread(threadName),
giuliomoro@217: #else
giuliomoro@217: ReceiveAudioThread::ReceiveAudioThread() :
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@217: 	socket(0),
giuliomoro@217:     listening(false),
giuliomoro@217:     bufferReady(false),
giuliomoro@217:     buffer(NULL),
giuliomoro@217:     stackBuffer(NULL),
giuliomoro@217:     bufferLength(0),
giuliomoro@217:     lastValidPointer(0),
giuliomoro@217:     waitForSocketTime(5),
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217:     threadPriority(5)
giuliomoro@217: #else
giuliomoro@217:     threadPriority(88)
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@217: {};
giuliomoro@217: ReceiveAudioThread::~ReceiveAudioThread(){
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217: 	stopThread(1000);
giuliomoro@217: #else
giuliomoro@217: 	stopThread();
giuliomoro@217: 	while(threadRunning){
giuliomoro@217: 		usleep(sleepTime*2);	//wait for thread to stop
giuliomoro@217: 		std::cout<< "Waiting for receiveAudioTask to stop" << std::endl;
giuliomoro@217: 	}
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@217: 	//TODO: check if thread stopped, otherwise kill it before dealloc
giuliomoro@217:     dealloc();
giuliomoro@217: }
giuliomoro@217: void ReceiveAudioThread::init(int aPort, int aSamplesPerBlock, int aChannel){
giuliomoro@217:   dealloc();
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217: #else
giuliomoro@217:   staticConstructor();
giuliomoro@217:   objAddrs.push_back(this);//TODO: this line should be in the constructor
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@217:   bindToPort(aPort);
giuliomoro@217:   channel=aChannel;
giuliomoro@217:   printf("Channel %d is receiving on port %d\n",aChannel, aPort);
giuliomoro@217:   //  fd=fopen("output.m","w"); //DEBUG
giuliomoro@217:   //  fprintf(fd,"var=["); //DEBUG
giuliomoro@217:   headerLength=2;
giuliomoro@217:   payloadLength=300; //TODO: make sure that payloadLength and headerLength are the same as the client is sending.
giuliomoro@222:   bufferLength=10 * std::max(headerLength+(payloadLength*4), headerLength+(aSamplesPerBlock*4)); //there are many considerations that can be done here ...
giuliomoro@217:                       //We keep a headerLength padding at the beginning of the array to allow full reads from the socket
giuliomoro@217:   buffer=(float*)malloc(sizeof(float)*bufferLength);
giuliomoro@217:   if(buffer==NULL) // something wrong
giuliomoro@217:     return;
giuliomoro@217:   lastValidPointer=headerLength+ ((bufferLength-headerLength)/payloadLength)*payloadLength;
giuliomoro@217:   memset(buffer,0,bufferLength*sizeof(float));
giuliomoro@217:   stackBuffer=(float*)malloc(sizeof(float)*headerLength);
giuliomoro@217:   if(stackBuffer==NULL) // something wrong
giuliomoro@217:       return;
giuliomoro@217:   bufferReady=true;
giuliomoro@217:   bytesToRead=sizeof(float)*(payloadLength + headerLength);
giuliomoro@217:   writePointer=-1;
giuliomoro@217:   readPointer=0;
giuliomoro@217:   sleepTime=payloadLength/(float)44100 /4.0; //set sleepTime so that you do not check too often or too infrequently
giuliomoro@217:   timestamp=0;
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217:   startThread(threadPriority);
giuliomoro@217: #else
giuliomoro@217:   //TODO: the thread cannot be started here at the moment because init() is called in setup(), where tasks cannot be scheduled
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@217: }
giuliomoro@217: 
giuliomoro@217: void ReceiveAudioThread::bindToPort(int aPort){
giuliomoro@217:     listening=socket.bindToPort(aPort);
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217: #else
giuliomoro@217:     if(listening==false) //this condition is valid also for USE_JUCE, but we do not printf in USE_JUCE
giuliomoro@217:     	printf("Could not bind to port %d\n",aPort);
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@217: }
giuliomoro@217: bool ReceiveAudioThread::isListening(){
giuliomoro@217:     return listening;
giuliomoro@217: }
giuliomoro@217: float* ReceiveAudioThread::getCurrentBuffer(int length){ // NOTE: this cannot work all the time unless samplesPerBuffer and payloadLength are multiples
giuliomoro@217:     //TODO: make it return the number of samples actually available at the specified location
giuliomoro@217:     if(isListening()==false || length>bufferLength)
giuliomoro@217:         return NULL;
giuliomoro@217:     readPointer+=length;
giuliomoro@217:     if(readPointer>lastValidPointer){
giuliomoro@217:         readPointer=headerLength;
giuliomoro@217:     }
giuliomoro@217:     return buffer+(int)readPointer;
giuliomoro@217: };
giuliomoro@217: int ReceiveAudioThread::getSamplesSrc(float *destination, int length,
giuliomoro@217: 		float samplingRateRatio, int numChannelsInDestination,
giuliomoro@217: 		int channelToWriteTo)
giuliomoro@217: {
giuliomoro@217:     if (!(samplingRateRatio>0 && samplingRateRatio<=2))
giuliomoro@217:         return -2;
giuliomoro@217:     if(isListening()==false)
giuliomoro@217:         return -1;
giuliomoro@217:     static int numCalls=0;
giuliomoro@217:     if(writePointer<0 /*|| (numCalls&16383)==0*/){ //if writePointer has not been initalized yet ...
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217: #else //debug
giuliomoro@222:     	readPointer = headerLength;
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@222:     	// this cumbersome line means: start writing at a position which is as close as possible
giuliomoro@222:     	// to the center of the buffer, but still is aligned to (payloadLength*x)+headerLength
giuliomoro@222:     	// thus allowing buffering to allow clock drift to go either way
giuliomoro@222:         writePointer = headerLength + ((bufferLength-headerLength)/payloadLength/2)*payloadLength;
giuliomoro@217:                             // This will help keeping them in sync.
giuliomoro@217:                             //TODO: handle what happens when the remote stream is interrupted and then restarted
giuliomoro@222:         printf("write pointer inited at: %d\n", writePointer);
giuliomoro@217:     }
giuliomoro@217:     numCalls++;
giuliomoro@217:     if(length>lastValidPointer) { 
giuliomoro@217:         //not enough samples available, we fill the buffer with what is available, but the destination buffer will not be filled completely
giuliomoro@217:         //at this very moment the other thread might be writing at most one payload into the buffer.
giuliomoro@217:         //To avoid a race condition, we need to let alone the buffer where we are currently writing
giuliomoro@217:         //as writing the payload also temporarily overwrites the previous headerLength samples, we need to account for them as well
giuliomoro@217:         //TODO: This assumes that the writePointer and readPointer do not drift. When doing clock synchronization we will find out that it is not true!
giuliomoro@217:         length=lastValidPointer-payloadLength-headerLength;
giuliomoro@217:         if(length<0) //no samples available at all! 
giuliomoro@217:             return 0;
giuliomoro@217:     }
giuliomoro@217:     for(int n=0; n<length; n++){
giuliomoro@217:         destination[n*numChannelsInDestination+channelToWriteTo]=buffer[(int)(0.5+readPointer)];//simple ZOH non-interpolation (nearest neighbour)
giuliomoro@217:         //  fprintf(fd,"%f, %d, %f;\n",readPointer,writePointer,destination[n]); //DEBUG
giuliomoro@217:         readPointer+=samplingRateRatio;
giuliomoro@217:         if((int)(0.5+readPointer)>=lastValidPointer){
giuliomoro@217:             readPointer=readPointer-lastValidPointer+headerLength;
giuliomoro@217:         }
giuliomoro@217:     }
giuliomoro@217:     return length;
giuliomoro@217: }
giuliomoro@217: int ReceiveAudioThread::getSamplesSrc(float *destination, int length, float samplingRateRatio){
giuliomoro@217: 	return getSamplesSrc(destination, length, samplingRateRatio, 1,0);
giuliomoro@217: 	// TODO: rewriting this so that it does not call the override method we can save a multiply and add
giuliomoro@217: 	// for each sample.
giuliomoro@217: }
giuliomoro@217: bool ReceiveAudioThread::isBufferReady(){
giuliomoro@217:     return bufferReady;
giuliomoro@217: }
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217: #else
giuliomoro@217: void ReceiveAudioThread::startThread(){
giuliomoro@217: 	BeagleRT_scheduleAuxiliaryTask(receiveDataTask);
giuliomoro@217: }
giuliomoro@217: void ReceiveAudioThread::stopThread(){
giuliomoro@217: 	threadIsExiting=true;
giuliomoro@217: }
giuliomoro@217: bool ReceiveAudioThread::threadShouldExit(){
giuliomoro@217: 	return(gShouldStop || threadIsExiting );
giuliomoro@217: }
giuliomoro@217: RTIME ReceiveAudioThread::getLastTime(){
giuliomoro@217: 	return lastTime;
giuliomoro@217: }
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@217: int ReceiveAudioThread::getTimestamp(){
giuliomoro@217: 	return timestamp;
giuliomoro@217: }
giuliomoro@217: void ReceiveAudioThread::run(){
giuliomoro@217:     //  fd2=fopen("buffer.m","w"); //DEBUG
giuliomoro@217:     //  fprintf(fd2, "buf=["); //DEBUG
giuliomoro@217: 	threadRunning=true;
giuliomoro@217: 	int maxCount=10;
giuliomoro@217: 	int count=0;
giuliomoro@217: 	// Clean the socket from anything that is currently in it.
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217: 	// this is borrowed from BeagleRT's UdpServer class.
giuliomoro@217: 	int n;
giuliomoro@217: 	do {
giuliomoro@217: 		float waste;
giuliomoro@217: 		if(socket.waitUntilReady(true, 0)==0)
giuliomoro@217: 			break;
giuliomoro@217: 		n=socket.read((void*)&waste, sizeof(float), false);
giuliomoro@217: 		count++;
giuliomoro@217: 		if(n<0){
giuliomoro@217: 			printf("error\n");
giuliomoro@217: 			break;
giuliomoro@217: 		}
giuliomoro@217: 		printf("n: %d\n",n);
giuliomoro@217: 	} while (n>0 && (maxCount<=0 || count<maxCount));
giuliomoro@217: #else
giuliomoro@217: 	for(unsigned int n=0; n<objAddrs.size(); n++){
giuliomoro@217: 		count=objAddrs[n]->socket.empty(maxCount);
giuliomoro@217: 	}
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@217: 	printf("socket emptied with %d reads\n", count);
giuliomoro@217: 
giuliomoro@217:     while(!threadShouldExit()){ //TODO: check that the socket buffer is empty before starting
giuliomoro@217: #ifdef USE_JUCE
giuliomoro@217:         readUdpToBuffer(); // read into the oldBuffer
giuliomoro@217:         sleep(sleepTime);
giuliomoro@217: #else
giuliomoro@217: 		for(unsigned int n=0; n<ReceiveAudioThread::objAddrs.size(); n++){
giuliomoro@217: 			ReceiveAudioThread::objAddrs[n]->readUdpToBuffer();
giuliomoro@217: 		}
giuliomoro@217: 		usleep(sleepTime); //TODO: use rt_task_sleep instead
giuliomoro@217: #endif /* USE_JUCE */
giuliomoro@217:     }
giuliomoro@217:     threadRunning=false;
giuliomoro@217:     printf("Thread is not running \n");
giuliomoro@217:     //  fprintf(fd,"];readPointer,writePointer,lastValidPointer,destination]=deal(var(:,1), var(:,2), var(:,3), var(:,4));"); //DEBUG
giuliomoro@217:     //  fclose(fd);//DEBUG
giuliomoro@217:     //  fprintf(fd2,"];");//DEBUG
giuliomoro@217:     //  fclose(fd2); //DEBUG
giuliomoro@217: }