/**
 * \internal Odpowiednik \c gethostbyname zapewniający współbieżność.
 *
 * Jeśli dany system dostarcza \c gethostbyname_r, używa się tej wersji, jeśli
 * nie, to zwykłej \c gethostbyname.
 *
 * \param hostname Nazwa serwera
 * \param addr Wskaźnik na rezultat rozwiązywania nazwy
 * \param pthread Flaga blokowania unicestwiania wątku podczas alokacji pamięci
 *
 * \return 0 jeśli się powiodło, -1 w przypadku błędu
 */
int gg_gethostbyname_real(const char *hostname, struct in_addr *addr, int pthread)
{
#ifdef GG_CONFIG_HAVE_GETHOSTBYNAME_R
	char *buf = NULL;
	char *new_buf = NULL;
	struct hostent he;
	struct hostent *he_ptr = NULL;
	size_t buf_len = 1024;
	int result = -1;
	int h_errnop;
	int ret = 0;
#ifdef GG_CONFIG_HAVE_PTHREAD
	int old_state;
#endif

#ifdef GG_CONFIG_HAVE_PTHREAD
	pthread_cleanup_push(gg_gethostbyname_cleaner, &buf);

	if (pthread)
		pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &old_state);
#endif

	buf = malloc(buf_len);

#ifdef GG_CONFIG_HAVE_PTHREAD
	if (pthread)
		pthread_setcancelstate(old_state, NULL);
#endif

	if (buf != NULL) {
#ifndef sun
		while ((ret = gethostbyname_r(hostname, &he, buf, buf_len, &he_ptr, &h_errnop)) == ERANGE) {
#else
		while (((he_ptr = gethostbyname_r(hostname, &he, buf, buf_len, &h_errnop)) == NULL) && (errno == ERANGE)) {
#endif
			buf_len *= 2;

#ifdef GG_CONFIG_HAVE_PTHREAD
			if (pthread)
				pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &old_state);
#endif

			new_buf = realloc(buf, buf_len);

			if (new_buf != NULL)
				buf = new_buf;

#ifdef GG_CONFIG_HAVE_PTHREAD
			if (pthread)
				pthread_setcancelstate(old_state, NULL);
#endif

			if (new_buf == NULL) {
				ret = ENOMEM;
				break;
			}
		}

		if (ret == 0 && he_ptr != NULL) {
			memcpy(addr, he_ptr->h_addr, sizeof(struct in_addr));
			result = 0;
		}

#ifdef GG_CONFIG_HAVE_PTHREAD
		if (pthread)
			pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &old_state);
#endif

		free(buf);
		buf = NULL;

#ifdef GG_CONFIG_HAVE_PTHREAD
		if (pthread)
			pthread_setcancelstate(old_state, NULL);
#endif
	}

#ifdef GG_CONFIG_HAVE_PTHREAD
	pthread_cleanup_pop(1);
#endif

	return result;
#else
	struct hostent *he;

	he = gethostbyname(hostname);

	if (he == NULL)
//.........这里部分代码省略.........

//.........这里部分代码省略.........
            }

#if FITS_TYPE == PSR_FITS
            /* Determine scaling factors for quantization if appropriate */
            if (requantize) {
                mean = (double *)realloc(mean, 
                        pf.hdr.rcvr_polns * pf.hdr.nchan * sizeof(double));
                std  = (double *)realloc(std,  
                        pf.hdr.rcvr_polns * pf.hdr.nchan * sizeof(double));
                compute_stat(&pf, mean, std);
                fprintf(stderr, "Computed 2-bit stats\n");
            }
#endif
        }
        
        /* See if we need to open next file */
        if (block_count >= blocks_per_file) {
            fclose(fraw);
            filenum++;
            char fname[256];
            sprintf(fname, "%s.%4.4d.raw", pf.basefilename, filenum);
            fprintf(stderr, "Opening raw file '%s'\n", fname);
            fraw = fopen(fname, "w");
            if (fraw==NULL) {
                guppi_error("guppi_rawdisk_thread", "Error opening file.");
                pthread_exit(NULL);
            }
            block_count=0;
        }

        /* See how full databuf is */
        //total_status = guppi_databuf_total_status(db);

        /* Requantize from 8 bits to 2 bits if necessary.
         * See raw_quant.c for more usage examples.
         */
#if FITS_TYPE == PSR_FITS
        if (requantize && got_packet_0) {
            pf.sub.bytes_per_subint = orig_blocksize;
            /* Does the quantization in-place */
            quantize_2bit(&pf, mean, std);
            /* Update some parameters for output */
            hputi4(ptr, "BLOCSIZE", pf.sub.bytes_per_subint);
            hputi4(ptr, "NBITS", pf.hdr.nbits);
        }
#endif

        /* Get full data block size */
        hgeti4(ptr, "BLOCSIZE", &blocksize);

        /* If we got packet 0, write data to disk */
        if (got_packet_0) { 

            /* Note waiting status */
            guppi_status_lock_safe(&st);
            hputs(st.buf, STATUS_KEY, "writing");
            guppi_status_unlock_safe(&st);

            /* Write header to file */
            hend = ksearch(ptr, "END");
            for (ptr=ptr; ptr<=hend; ptr+=80) {
                fwrite(ptr, 80, 1, fraw);
            }

            /* Write data */
            printf("block size: %d\n", blocksize);
            ptr = guppi_databuf_data(db, curblock);
            rv = fwrite(ptr, 1, (size_t)blocksize, fraw);
            if (rv != blocksize) { 
                guppi_error("guppi_rawdisk_thread", 
                        "Error writing data.");
            }

            /* Increment counter */
            block_count++;

            /* flush output */
            fflush(fraw);
        }

        /* Mark as free */
        guppi_databuf_set_free(db, curblock);

        /* Go to next block */
        curblock = (curblock + 1) % db->n_block;

        /* Check for cancel */
        pthread_testcancel();

    }

    pthread_exit(NULL);

    pthread_cleanup_pop(0); /* Closes fclose */
    pthread_cleanup_pop(0); /* Closes guppi_databuf_detach */
    pthread_cleanup_pop(0); /* Closes guppi_free_psrfits */
    pthread_cleanup_pop(0); /* Closes set_exit_status */
    pthread_cleanup_pop(0); /* Closes guppi_status_detach */

}

/******************************************************************************
Description.: this is the main worker thread
              it loops forever, grabs a fresh frame and stores it to file
Input Value.:
Return Value:
******************************************************************************/
void *worker_thread(void *arg)
{
    int ok = 1, frame_size = 0, decompressed_size = 0;
    unsigned char *tmp_decompressed = NULL;

    int width = 320;
    int height = 240;
    clock_t start, mid, end;

    Vision cv_runner;
    /* set cleanup handler to cleanup allocated ressources */
    pthread_cleanup_push(worker_cleanup, NULL);

    while(ok >= 0 && !pglobal->stop) {
        DBG("waiting for fresh frame\n");

        pthread_mutex_lock(&pglobal->in[input_number].db);
        pthread_cond_wait(&pglobal->in[input_number].db_update, &pglobal->in[input_number].db);
        start = clock();
        DBG("getting sizes\n");

        /* Get sizes */
        int fmt_idx = pglobal->in[input_number].currentFormat;
        if(fmt_idx > (pglobal->in[input_number].formatCount - 1)) {
            DBG("Format index out of bounds");
            return NULL;
        }
        int res_idx = pglobal->in[input_number].in_formats[fmt_idx].currentResolution;
        if(res_idx > (pglobal->in[input_number].in_formats[fmt_idx].resolutionCount - 1)) {
            DBG("Res index out of bounds");
            return NULL;
        }

        // width = pglobal->in[input_number].in_formats[fmt_idx].supportedResolutions[res_idx].width;
        // height = pglobal->in[input_number].in_formats[fmt_idx].supportedResolutions[res_idx].height;
        DBG("Width: %d,Height:%d\n", width, height);
        decompressed_size = width * height * sizeof(char) * 3;

        /* read buffer */
        frame_size = pglobal->in[input_number].size;

        // TODO: resize decompressed buffer
        if (decompressed_size > max_decompressed_size) {
            DBG("increasing decompresser size to %d\n", decompressed_size);

            max_decompressed_size = decompressed_size + (1 << 16);
            if((tmp_decompressed = (unsigned char *) realloc(decompressed, max_decompressed_size)) == NULL) {
                pthread_mutex_unlock(&pglobal->in[input_number].db);
                LOG("not enough memory\n");
                return NULL;
            }

            decompressed = tmp_decompressed;
        }


        /* allow others to access the global buffer again */
        DBG("About to make the mat\n");
        vision::process_jpeg(decompressed,pglobal->in[input_number].buf,frame_size);
        pthread_mutex_unlock(&pglobal->in[input_number].db);
        DBG("Width:%d, Height:%d\n", width, height);
        cv::Mat img(height, width, CV_8UC3, decompressed, width * 3 * sizeof(char));
        mid = clock();
        DBG("Made the mat!\n");
        ok = cv_runner.loop(img);
        end = clock();
        DBG("Wrote the image!\n");
        DBG("Decompression time: %f\n",((float)mid-start)/CLOCKS_PER_SEC);
        DBG("OpenCV time: %f\n",((float)end-mid)/CLOCKS_PER_SEC);

        // Wait 100ms so there's less overwhelming of data
        //usleep(100000);
    }

    /* cleanup now */
    pthread_cleanup_pop(1);

    return NULL;
}

/*
 * Run a seperate thread that connects to the database server and
 * controls the backup or restore. When we close the VDI device we
 * also tear down this database control thread.
 */
static void *adoThread(void *data)
{
   HRESULT hr;
   adoThreadContext ado_ctx;
   bpContext *ctx = (bpContext *)data;
   plugin_ctx *p_ctx = (struct plugin_ctx *)ctx->pContext;

   memset(&ado_ctx, 0, sizeof(ado_ctx));

   /*
    * When we get canceled make sure we run the cleanup function.
    */
   pthread_cleanup_push(adoCleanupThread, &ado_ctx);

   /*
    * Initialize COM for this thread.
    */
   hr = CoInitializeEx(NULL, COINIT_MULTITHREADED);
   if (!SUCCEEDED (hr)) {
      return NULL;
   }

   /*
    * Create a COM instance for an ActiveX® Data Objects connection.
    */
   hr = CoCreateInstance(CLSID_CADOConnection,
                         NULL,
                         CLSCTX_INPROC_SERVER,
                         IID_IADOConnection,
                         (void **)&ado_ctx.adoConnection);
   if (!SUCCEEDED (hr)) {
      goto bail_out;
   }

   /*
    * Make sure the connection doesn't timeout.
    * Default timeout is not long enough most of the time
    * for the backup or restore to finish and when it times
    * out it will abort the action it was performing.
    */
   hr = ado_ctx.adoConnection->put_CommandTimeout(0);
   if (!SUCCEEDED (hr)) {
      adoThreadSetError(ctx, ado_ctx.adoConnection);
      goto bail_out;
   }

   /*
    * Open a connection to the database server with the defined connection string.
    */
   ado_ctx.ado_connect_string = str_2_BSTR(p_ctx->ado_connect_string);
   hr = ado_ctx.adoConnection->Open(ado_ctx.ado_connect_string);
   if (!SUCCEEDED (hr)) {
      adoThreadSetError(ctx, ado_ctx.adoConnection);
      goto bail_out;
   }

   /*
    * Execute the backup or restore command.
    */
   ado_ctx.ado_query = str_2_BSTR(p_ctx->ado_query);
   hr = ado_ctx.adoConnection->Execute(ado_ctx.ado_query, NULL, adExecuteNoRecords, NULL);
   if (!SUCCEEDED (hr)) {
      adoThreadSetError(ctx, ado_ctx.adoConnection);
      goto bail_out;
   }

bail_out:
   /*
    * Run the thread cleanup.
    */
   pthread_cleanup_pop(1);

   return NULL;
}

//.........这里部分代码省略.........
			buffer = malloc(size);
			if (buffer != NULL) {
				int i;
				const unsigned char *values = (const unsigned char *)value;
				for(i= (sizeof(long double) -1);((i >= 0) && (length != -1));i--) {
					char *byteBuffer = buffer + length;
					length += binary_byte_string(values[i],byteBuffer);
				}
			} else {
				ERROR_MSG("failed to allocate %u bytes to printf a binary string",size);
			}
		} else if (info->is_long) {
			/* l flag:
			 * (ell)  A  following integer conversion corresponds to a long int or unsigned long int argument, or a
			 * following n conversion corresponds to a pointer to a long int argument, or a following c  conversion
			 * corresponds  to  a  wint_t argument, or a following s conversion corresponds to a pointer to wchar_t
			 * argument.
			 */
			const size_t size = sizeof(long) * (sizeof(char)  * 8) + 1; /* 1 byte => 8 characters */
			buffer = malloc(size);
			if (buffer != NULL) {
				const unsigned char *values = (const unsigned char *)value;
				int i;
				for(i= (sizeof(long) -1);((i >= 0) && (length != -1));i--) {
					char *byteBuffer = buffer + length;
					length += binary_byte_string(values[i],byteBuffer);
				}
			} else {
				ERROR_MSG("failed to allocate %u bytes to printf a binary string",size);
			}
		} else {
			/*
			 * no length modifier has been set
			 * Warning: long type may be 32 or 64 bits
			 */
			const size_t size = sizeof(long) * (sizeof(char)  * 8) + 1; /* 1 byte => 8 characters */
			buffer = malloc(size);
			if (buffer != NULL) {
				int i;
				const unsigned char *values = (const unsigned char *)value;
				for(i= (sizeof(long) -1);((i >= 0) && (length != -1));i--) {
					char *byteBuffer = buffer + length;
					length += binary_byte_string(values[i],byteBuffer);
				}
			} else {
				ERROR_MSG("failed to allocate %u bytes to printf a binary string",size);
			}
		}

		/* add the "binary" prefix, pad to the minimum field width and print to the stream. */
		if (length != -1) {
			register const char *bufferStart = buffer;
			char precPadding[128];
			register char *pos = precPadding;
			if ((0 == info->width) && (-1 == info->prec)) {
				/* remove unnecessary 0 on the left */
				*pos = '\0';
				while( '0' == *bufferStart ) {
					bufferStart++;
				}
				/* just in case: keep at least the last bit value whatever it's value ! */
				if ('\0' == *bufferStart) {
					bufferStart--;
				}
			} else {
				if (info->prec > length) {
					/* add 0 to the left to get the right precision (weird cast ?) */
					while(info->prec >= length) {
						*pos = '0';
						length++;
						pos++;
					}
				}
				*pos = '\0';
				if (info->width < length) {
					/* try to remove unnecessary 0 to get the right width (without losing data !) */
					while( ('0' == *bufferStart) && (info->width < length)) {
						bufferStart++;
						length--;
					}
				} else if (info->width != 0) {
					/* add space to get the desired width */
					while(length < info->width) {
						*pos = ' ';
						length++;
						pos++;
					}
					*pos = '\0';
				}
				//length = fprintf (stream, "b%*s",(info->left ? -info->width : info->width),bufferStart);
			} /* !((0 == info->width) && (0 == info->prec))  */
			length = fprintf (stream, "b%s%s",precPadding,bufferStart);
		}

		/* Clean up and return. */
		free (buffer);
		pthread_cleanup_pop(0);
	} /* (BINARY_FORMAT_KEY == info[n].spec) */
	return length;
}

//.........这里部分代码省略.........

    cpuTimeThen = th->callbackCpuTime;
    {
        int policy;
        struct sched_param spm = { 0 };
        pthread_getschedparam( pthread_self(), &policy, &spm );
        PA_DEBUG(( "%s: Watchdog priority is %d\n", __FUNCTION__, spm.sched_priority ));
    }

    while( 1 )
    {
        double lowpassCoeff = 0.9, lowpassCoeff1 = 0.99999 - lowpassCoeff;

        /* Test before and after in case whatever underlying sleep call isn't interrupted by pthread_cancel */
        pthread_testcancel();
        Pa_Sleep( intervalMsec );
        pthread_testcancel();

        if( PaUtil_GetTime() - th->callbackTime > maxSeconds )
        {
            PA_DEBUG(( "Watchdog: Terminating callback thread\n" ));
            /* Tell thread to terminate */
            err = pthread_kill( th->callbackThread, SIGKILL );
            pthread_exit( NULL );
        }

        PA_DEBUG(( "%s: PortAudio reports CPU load: %g\n", __FUNCTION__, PaUtil_GetCpuLoad( th->cpuLoadMeasurer ) ));

        /* Check if we should throttle, or unthrottle :P */
        cpuTimeNow = th->callbackCpuTime;
        cpuTimeElapsed = cpuTimeNow - cpuTimeThen;
        cpuTimeThen = cpuTimeNow;

        timeNow = PaUtil_GetTime();
        timeElapsed = timeNow - timeThen;
        timeThen = timeNow;
        cpuLoad = cpuTimeElapsed / timeElapsed;
        avgCpuLoad = avgCpuLoad * lowpassCoeff + cpuLoad * lowpassCoeff1;
        /*
        if( throttled )
            PA_DEBUG(( "Watchdog: CPU load: %g, %g\n", avgCpuLoad, cpuTimeElapsed ));
            */
        if( PaUtil_GetCpuLoad( th->cpuLoadMeasurer ) > .925 )
        {
            static int policy;
            static struct sched_param spm = { 0 };
            static const struct sched_param defaultSpm = { 0 };
            PA_DEBUG(( "%s: Throttling audio thread, priority %d\n", __FUNCTION__, spm.sched_priority ));

            pthread_getschedparam( th->callbackThread, &policy, &spm );
            if( !pthread_setschedparam( th->callbackThread, SCHED_OTHER, &defaultSpm ) )
            {
                throttled = 1;
            }
            else
                PA_DEBUG(( "Watchdog: Couldn't lower priority of audio thread: %s\n", strerror( errno ) ));

            /* Give other processes a go, before raising priority again */
            PA_DEBUG(( "%s: Watchdog sleeping for %lu msecs before unthrottling\n", __FUNCTION__, th->throttledSleepTime ));
            Pa_Sleep( th->throttledSleepTime );

            /* Reset callback priority */
            if( pthread_setschedparam( th->callbackThread, SCHED_FIFO, &spm ) != 0 )
            {
                PA_DEBUG(( "%s: Couldn't raise priority of audio thread: %s\n", __FUNCTION__, strerror( errno ) ));
            }

            if( PaUtil_GetCpuLoad( th->cpuLoadMeasurer ) >= .99 )
                intervalMsec = 50;
            else
                intervalMsec = 100;

            /*
            lowpassCoeff = .97;
            lowpassCoeff1 = .99999 - lowpassCoeff;
            */
        }
        else if( throttled && avgCpuLoad < .8 )
        {
            intervalMsec = 500;
            throttled = 0;

            /*
            lowpassCoeff = .9;
            lowpassCoeff1 = .99999 - lowpassCoeff;
            */
        }
    }

    pthread_cleanup_pop( 1 );   /* Execute cleanup on exit */

error:
    /* Shouldn't get here in the normal case */

    /* Pass on error code */
    pres = malloc( sizeof (PaError) );
    *pres = result;

    pthread_exit( pres );
}

static void *open_output( void *ptr )
{
    obe_output_params_t *output_params = ptr;
    obe_t *h = output_params->h;
    struct ip_status status;
    hnd_t ip_handle = NULL;
    int num_muxed_data = 0;
    AVBufferRef **muxed_data;
    obe_udp_opts_t udp_opts;

    struct sched_param param = {0};
    param.sched_priority = 99;
    pthread_setschedparam( pthread_self(), SCHED_FIFO, &param );

    status.output_params = output_params;
    status.ip_handle = &ip_handle;
    pthread_cleanup_push( close_output, (void*)&status );

    udp_populate_opts( &udp_opts, output_params->output_opts.target );

    if( output_params->output_opts.output == OUTPUT_RTP )
    {
        if( rtp_open( &ip_handle, &udp_opts ) < 0 )
            return NULL;
    }
    else
    {
        if( udp_open( &ip_handle, &udp_opts ) < 0 )
        {
            fprintf( stderr, "[udp] Could not create udp output" );
            return NULL;
        }
    }

    while( 1 )
    {
        pthread_mutex_lock( &h->output_queue.mutex );
        while( !h->output_queue.size && !h->cancel_output_thread )
        {
            /* Often this cond_wait is not because of an underflow */
            pthread_cond_wait( &h->output_queue.in_cv, &h->output_queue.mutex );
        }

        if( h->cancel_output_thread )
        {
            pthread_mutex_unlock( &h->output_queue.mutex );
            break;
        }

        num_muxed_data = h->output_queue.size;

        muxed_data = malloc( num_muxed_data * sizeof(*muxed_data) );
        if( !muxed_data )
        {
            pthread_mutex_unlock( &h->output_queue.mutex );
            syslog( LOG_ERR, "Malloc failed\n" );
            return NULL;
        }
        memcpy( muxed_data, h->output_queue.queue, num_muxed_data * sizeof(*muxed_data) );
        pthread_mutex_unlock( &h->output_queue.mutex );

//        printf("\n START %i \n", num_muxed_data );

        for( int i = 0; i < num_muxed_data; i++ )
        {
            if( output_params->output_opts.output == OUTPUT_RTP )
            {
                if( write_rtp_pkt( ip_handle, &muxed_data[i]->data[7*sizeof(int64_t)], TS_PACKETS_SIZE, AV_RN64( muxed_data[i]->data ) ) < 0 )
                    syslog( LOG_ERR, "[rtp] Failed to write RTP packet\n" );
            }
            else
            {
                if( udp_write( ip_handle, &muxed_data[i]->data[7*sizeof(int64_t)], TS_PACKETS_SIZE ) < 0 )
                    syslog( LOG_ERR, "[udp] Failed to write UDP packet\n" );
            }

            remove_from_queue( &h->output_queue );
            av_buffer_unref( &muxed_data[i] );
        }

        free( muxed_data );
        muxed_data = NULL;
    }

    pthread_cleanup_pop( 1 );

    return NULL;
}

static void *
tpool_worker(void *arg)
{
	tpool_t *tpool = (tpool_t *)arg;
	int elapsed;
	tpool_job_t *job;
	void (*func)(void *);
	tpool_active_t active;

	sig_mutex_lock(&tpool->tp_mutex);
	pthread_cleanup_push(worker_cleanup, tpool);

	/*
	 * This is the worker's main loop.
	 * It will only be left if a timeout or an error has occured.
	 */
	active.tpa_tid = pthread_self();
	for (;;) {
		elapsed = 0;
		tpool->tp_idle++;
		if (tpool->tp_flags & TP_WAIT)
			notify_waiters(tpool);
		while ((tpool->tp_head == NULL ||
		    (tpool->tp_flags & TP_SUSPEND)) &&
		    !(tpool->tp_flags & (TP_DESTROY | TP_ABANDON))) {
			if (tpool->tp_current <= tpool->tp_minimum ||
			    tpool->tp_linger == 0) {
				(void) sig_cond_wait(&tpool->tp_workcv,
				    &tpool->tp_mutex);
			} else {
				timestruc_t timeout;

				timeout.tv_sec = tpool->tp_linger;
				timeout.tv_nsec = 0;
				if (sig_cond_reltimedwait(&tpool->tp_workcv,
				    &tpool->tp_mutex, &timeout) != 0) {
					elapsed = 1;
					break;
				}
			}
		}
		tpool->tp_idle--;
		if (tpool->tp_flags & TP_DESTROY)
			break;
		if (tpool->tp_flags & TP_ABANDON) {
			/* can't abandon a suspended pool */
			if (tpool->tp_flags & TP_SUSPEND) {
				tpool->tp_flags &= ~TP_SUSPEND;
				(void) cond_broadcast(&tpool->tp_workcv);
			}
			if (tpool->tp_head == NULL)
				break;
		}
		if ((job = tpool->tp_head) != NULL &&
		    !(tpool->tp_flags & TP_SUSPEND)) {
			elapsed = 0;
			func = job->tpj_func;
			arg = job->tpj_arg;
			tpool->tp_head = job->tpj_next;
			if (job == tpool->tp_tail)
				tpool->tp_tail = NULL;
			tpool->tp_njobs--;
			active.tpa_next = tpool->tp_active;
			tpool->tp_active = &active;
			sig_mutex_unlock(&tpool->tp_mutex);
			pthread_cleanup_push(job_cleanup, tpool);
			lfree(job, sizeof (*job));
			/*
			 * Call the specified function.
			 */
			func(arg);
			/*
			 * We don't know what this thread has been doing,
			 * so we reset its signal mask and cancellation
			 * state back to the initial values.
			 */
			(void) pthread_sigmask(SIG_SETMASK, &maskset, NULL);
			(void) pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,
			    NULL);
			(void) pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,
			    NULL);
			pthread_cleanup_pop(1);
		}
		if (elapsed && tpool->tp_current > tpool->tp_minimum) {
			/*
			 * We timed out and there is no work to be done
			 * and the number of workers exceeds the minimum.
			 * Exit now to reduce the size of the pool.
			 */
			break;
		}
	}
	pthread_cleanup_pop(1);
	return (arg);
}

//.........这里部分代码省略.........
              gettimeofday(&t2,NULL);
              pthread_mutex_lock(&coord_lock);
              rc = pthread_create(&thread, NULL, (void *)&coordination_handler,(void *) control_program);
              pthread_join(thread,NULL);
              pthread_mutex_unlock(&coord_lock);
              gettimeofday(&t3,NULL);
              if (verbose > 1) {
                   elapsed=(t3.tv_sec-t2.tv_sec)*1E6;
                   elapsed+=(t3.tv_usec-t2.tv_usec);
                   if (verbose > 1 ) printf("Client:Set Ready: Coord Elapsed Microseconds: %ld\n",elapsed);
              }
              pthread_mutex_unlock(&controlprogram_list_lock);
            }
            send_data(socket, &msg, sizeof(struct ROSMsg));
            gettimeofday(&t1,NULL);
            if (verbose > 1) {
              elapsed=(t1.tv_sec-t0.tv_sec)*1E6;
              elapsed+=(t1.tv_usec-t0.tv_usec);
              if (verbose > 1 ) printf("Client:  Set Ready Elapsed Microseconds: %ld\n",elapsed);
            }
            if (verbose > 1 ) fprintf(stderr,"Client: End READY: %ld %ld :: %d %d\n",(long)t1.tv_sec,(long)t1.tv_usec,r,c);
            break;

          case REQUEST_CLEAR_FREQ_SEARCH:
            gettimeofday(&t0,NULL);
            pthread_mutex_lock(&controlprogram_list_lock);
            recv_data(socket,&control_program->clrfreqsearch, sizeof(struct CLRFreqPRM)); // requested search parameters
            if (verbose > 1 )printf("Client: Requst CLRSearch: %d %d\n",control_program->clrfreqsearch.start,control_program->clrfreqsearch.end);
            if ( (r < 0) || (c < 0)) {
              msg.status=-1;
            } else {
              rc = pthread_create(&threads[0], NULL, (void *) &DIO_clrfreq,control_program);
              pthread_join(threads[0],NULL);
              rc = pthread_create(&threads[0], NULL, (void *) &receiver_clrfreq,control_program);
              pthread_join(threads[0],NULL);
              rc = pthread_create(&threads[0], NULL, (void *) &DIO_rxfe_reset,NULL);
              pthread_join(threads[0],NULL);
              msg.status=control_program->state->freq_change_needed;
            }
            send_data(socket, &msg, sizeof(struct ROSMsg));
            pthread_mutex_unlock(&controlprogram_list_lock);
            gettimeofday(&t1,NULL);
            if (verbose > 1) {
              elapsed=(t1.tv_sec-t0.tv_sec)*1E6;
              elapsed+=(t1.tv_usec-t0.tv_usec);
              if (verbose > 1 ) printf("Client:  CLR Elapsed Microseconds: %ld\n",elapsed);
            }
            break;
          case REQUEST_ASSIGNED_FREQ:
            gettimeofday(&t0,NULL);
            pthread_mutex_lock(&controlprogram_list_lock);
            if ( (r < 0) || (c < 0)) {
              msg.status=-1;
              control_program->state->current_assigned_freq=0;
              control_program->state->current_assigned_noise=0;
            } else {
              rc = pthread_create(&threads[0], NULL, (void *) &receiver_assign_frequency,(void *)  control_program);
              pthread_join(threads[0],NULL);
              msg.status=control_program->state->best_assigned_freq!=control_program->state->current_assigned_freq;
            }
            //control_program->state->current_assigned_noise=1;
            current_freq=control_program->state->current_assigned_freq; 
            send_data(socket, &current_freq, sizeof(int32_t));
            send_data(socket, &control_program->state->current_assigned_noise, sizeof(float));
            send_data(socket, &msg, sizeof(struct ROSMsg));
            pthread_mutex_unlock(&controlprogram_list_lock);
            gettimeofday(&t1,NULL);
            if (verbose > 1) {
              elapsed=(t1.tv_sec-t0.tv_sec)*1E6;
              elapsed+=(t1.tv_usec-t0.tv_usec);
              if (verbose > 1 ) printf("Client:  Request Freq Elapsed Microseconds: %ld\n",elapsed);
            }
            break;

          case QUIT:
            gettimeofday(&t0,NULL);
            fprintf(stdout,"Client QUIT:: %d.%06d \n",(int)t0.tv_sec,(int)t0.tv_usec);
            fflush(stdout);
            msg.status=0;
            send_data(socket, &msg, sizeof(struct ROSMsg));
            //controlprogram_exit(control_program);
            pthread_exit(NULL);
            break;
          default:
            msg.status=1;
            send_data(socket, &msg, sizeof(struct ROSMsg));
        }
          /* FD_ISSET(0, &rfds) will be true. */
      } //else printf("No data within five seconds.\n");
//        if (verbose>1) printf("Client: test cancel\n");
        
        pthread_testcancel();
   }
   fprintf(stdout,"Outside of socket while loop : %p\n",control_program);
   fflush(stdout);
   pthread_testcancel();
   pthread_cleanup_pop(0);
   controlprogram_exit(control_program);
   pthread_exit(NULL);
};

size_t buffer_get_data (buf_t *buf, char *data, long nbytes)
{
  int write_amount;
  int orig_size;

  orig_size = nbytes;

  DEBUG("Enter buffer_get_data");

  pthread_cleanup_push(buffer_mutex_unlock, buf);

  LOCK_MUTEX(buf->mutex);

  /* Put the data into the buffer as space is made available */
  while (nbytes > 0) {

    if (buf->abort_write)
      break;

    DEBUG("Obtaining lock on buffer");
    /* Block until we can read something */
    if (buf->curfill == 0 && buf->eos)
      break; /* No more data to read */
      
    if (buf->curfill == 0 || (buf->prebuffering && !buf->eos)) {
      DEBUG("Waiting for more data to copy.");
      COND_WAIT(buf->playback_cond, buf->mutex);
    }

    if (buf->abort_write)
      break;

    /* Note: Even if curfill is still 0, nothing bad will happen here */
    
    /* For simplicity, the number of bytes played must satisfy
       the following three requirements:
       
       1. Do not copy more bytes than are stored in the buffer.
       2. Do not copy more bytes than the reqested data size.
       3. Do not run off the end of the buffer. */
    write_amount = compute_dequeue_size(buf, nbytes);

    UNLOCK_MUTEX(buf->mutex);
    execute_actions(buf, &buf->actions, buf->position);

    /* No need to lock mutex here because the other thread will
       NEVER reduce the number of bytes stored in the buffer */
    DEBUG1("Copying %d bytes from the buffer", write_amount);
    memcpy(data, buf->buffer + buf->start, write_amount);
    LOCK_MUTEX(buf->mutex);

    buf->curfill -= write_amount;
    data += write_amount;
    nbytes -= write_amount;
    buf->start = (buf->start + write_amount) % buf->size;
    DEBUG1("Updated buffer fill, curfill = %ld", buf->curfill);
    
    /* Signal a waiting decoder thread that they can put more
       audio into the buffer */
    DEBUG("Signal decoder thread that buffer space is available");
    COND_SIGNAL(buf->write_cond);
  }

  UNLOCK_MUTEX(buf->mutex);

  pthread_cleanup_pop(0);
  
  pthread_testcancel();

  DEBUG("Exit buffer_get_data");
   
  return orig_size - nbytes;
}

/******************************************************************************
Description.: this is the main worker thread
              it loops forever, grabs a fresh frame, decompressed the JPEG
              and displays the decoded data using SDL
Input Value.:
Return Value:
******************************************************************************/
void *worker_thread(void *arg)
{
    int frame_size = 0, firstrun = 1;

    SDL_Surface *screen = NULL, *image = NULL;
    decompressed_image rgbimage;

    /* initialze the buffer for the decompressed image */
    rgbimage.buffersize = 0;
    rgbimage.buffer = NULL;

    /* initialze the SDL video subsystem */
    if(SDL_Init(SDL_INIT_VIDEO) < 0) {
        fprintf(stderr, "Couldn't initialize SDL: %s\n", SDL_GetError());
        exit(EXIT_FAILURE);
    }

    /* just allocate a large buffer for the JPEGs */
    if((frame = malloc(4096 * 1024)) == NULL) {
        OPRINT("not enough memory for worker thread\n");
        exit(EXIT_FAILURE);
    }

    /* set cleanup handler to cleanup allocated ressources */
    pthread_cleanup_push(worker_cleanup, NULL);

    while(!pglobal->stop) {
        DBG("waiting for fresh frame\n");
        pthread_cond_wait(&pglobal->db_update, &pglobal->db);

        /* read buffer */
        frame_size = pglobal->size;
        memcpy(frame, pglobal->buf, frame_size);

        pthread_mutex_unlock(&pglobal->db);

        /* decompress the JPEG and store results in memory */
        if(decompress_jpeg(frame, frame_size, &rgbimage)) {
            DBG("could not properly decompress JPEG data\n");
            continue;
        }

        if(firstrun) {
            /* create the primary surface (the visible window) */
            screen = SDL_SetVideoMode(rgbimage.width, rgbimage.height, 0, SDL_ANYFORMAT | SDL_HWSURFACE);
            SDL_WM_SetCaption("MJPG-Streamer Viewer", NULL);

            /* create a SDL surface to display the data */
            image = SDL_AllocSurface(SDL_SWSURFACE, rgbimage.width, rgbimage.height, 24,
#if SDL_BYTEORDER == SDL_LIL_ENDIAN
                                     0x0000FF, 0x00FF00, 0xFF0000,
#else
                                     0xFF0000, 0x00FF00, 0x0000FF,
#endif
                                     0);

            /* copy the decoded data across */
            memcpy(image->pixels, rgbimage.buffer, rgbimage.width * rgbimage.height * 3);
            free(rgbimage.buffer);

            /* now, that we know the dimensions, we can directly copy to the right surface */
            rgbimage.buffer = image->pixels;
            rgbimage.buffersize = rgbimage.width * rgbimage.height * 3;

            firstrun = 0;
        }

        /* copy the image to the primary surface */
        SDL_BlitSurface(image, NULL, screen, NULL);

        /* redraw the whole surface */
        SDL_Flip(screen);
    }

    pthread_cleanup_pop(1);

    /* get rid of the image */
    SDL_FreeSurface(image);

    return NULL;
}

void submit_data_chunk (buf_t *buf, char *data, size_t size)
{
  long   buf_write_pos; /* offset of first available write location */
  size_t write_size;

  DEBUG1("Enter submit_data_chunk, size %d", size);

  pthread_cleanup_push(buffer_mutex_unlock, buf);

  /* Put the data into the buffer as space is made available */
  while (size > 0 && !buf->abort_write) {
    
    /* Section 1: Write a chunk of data */
    DEBUG("Obtaining lock on buffer");
    LOCK_MUTEX(buf->mutex);
    if (buf->size - buf->curfill > 0) {
      
      /* Figure how much we can write into the buffer.  Requirements:
	 1. Don't write more data than we have.
	 2. Don't write more data than we have room for.
	 3. Don't write past the end of the buffer. */
      buf_write_pos = (buf->start + buf->curfill) % buf->size;
      write_size = MIN3(size, buf->size - buf->curfill,
			buf->size - buf_write_pos);
      
      memcpy(buf->buffer + buf_write_pos, data, write_size);
      buf->curfill += write_size;
      data += write_size;
      size -= write_size;
      buf->position_end += write_size;
      DEBUG1("writing chunk into buffer, curfill = %ld", buf->curfill);
    }
    else {

      /* No room for more data, wait until there is */
      DEBUG("No room for data in buffer.  Waiting.");
      COND_WAIT(buf->write_cond, buf->mutex);
    }
      
    /* Section 2: signal if we are not prebuffering, done
       prebuffering, or paused */
    if (buf->prebuffering && (buf->prebuffer_size <= buf->curfill)) {

      DEBUG("prebuffering done")
	buf->prebuffering = 0; /* done prebuffering */
    }

    if (!buf->prebuffering && !buf->paused) {

      DEBUG("Signalling playback thread that more data is available.");
      COND_SIGNAL(buf->playback_cond);      
    } else
      DEBUG("Not signalling playback thread since prebuffering or paused.");
      
    UNLOCK_MUTEX(buf->mutex);
  }

  pthread_cleanup_pop(0);

  DEBUG("Exit submit_data_chunk");
}

void *buffer_thread_func (void *arg)
{
  buf_t *buf = (buf_t*) arg;
  size_t write_amount;
  
  DEBUG("Enter buffer_thread_func");
  
  buffer_thread_init(buf);

  pthread_cleanup_push(buffer_thread_cleanup, buf);
  
  DEBUG("Start main play loop");
  
  /* This test is safe since curfill will never decrease and eos will
     never be unset. */
  while ( !(buf->eos && buf->curfill == 0)) {

    if (buf->cancel_flag || sig_request.cancel)
      break;
    
    DEBUG("Check for something to play");
    /* Block until we can play something */
    LOCK_MUTEX (buf->mutex);
    if (buf->prebuffering || 
	buf->paused || 
	(buf->curfill < buf->audio_chunk_size && !buf->eos)) {

      DEBUG("Waiting for more data to play.");
      COND_WAIT(buf->playback_cond, buf->mutex);
    }
    
    DEBUG("Ready to play");
    
    UNLOCK_MUTEX(buf->mutex);

    if (buf->cancel_flag || sig_request.cancel)
      break;

    /* Don't need to lock buffer while running actions since position
       won't change.  We clear out any actions before we compute the
       dequeue size so we don't consider actions that need to
       run right now.  */
    execute_actions(buf, &buf->actions, buf->position);

    LOCK_MUTEX(buf->mutex);

    /* Need to be locked while we check things. */
    write_amount = compute_dequeue_size(buf, buf->audio_chunk_size);

    UNLOCK_MUTEX(buf->mutex);
 
    /* No need to lock mutex here because the other thread will
       NEVER reduce the number of bytes stored in the buffer */
    DEBUG1("Sending %d bytes to the audio device", write_amount);
    write_amount = buf->write_func(buf->buffer + buf->start, write_amount,
		    /* Only set EOS if this is the last chunk */
		    write_amount == buf->curfill ? buf->eos : 0,
		    buf->write_arg);
      
    LOCK_MUTEX(buf->mutex);

    buf->curfill -= write_amount;
    buf->position += write_amount;
    buf->start = (buf->start + write_amount) % buf->size;
    DEBUG1("Updated buffer fill, curfill = %ld", buf->curfill);

    /* If we've essentially emptied the buffer and prebuffering is enabled,
       we need to do another prebuffering session */
    if (!buf->eos && (buf->curfill < buf->audio_chunk_size))
      buf->prebuffering = buf->prebuffer_size > 0;
    
    /* Signal a waiting decoder thread that they can put more audio into the
       buffer */
    DEBUG("Signal decoder thread that buffer space is available");
    COND_SIGNAL(buf->write_cond);
    
    UNLOCK_MUTEX(buf->mutex);
  }
  
  pthread_cleanup_pop(1);
  DEBUG("exiting buffer_thread_func");

  return 0;
}

static void* ngx_http_monitor_send_mainloop(void* args) {
    ngx_http_monitor_sender_t* sender = (ngx_http_monitor_sender_t*)args;

    pthread_cleanup_push(ngx_http_monitor_destory_sender, sender);

    ngx_http_monitor_server_t* uscfp = sender->servers->elts;

    int totalfd = sender->servers->nelts;
    int maxfd = 0;
    int i;

    u_char* sendp;
    ngx_int_t sendlen;

    struct epoll_event event;
    struct epoll_event events[totalfd];

    //since linux 2.6.8, the size arg is unused, but must be > 0
//  int epollfd = epoll_create1(0); // won't compile
    int epollfd = epoll_create(1024);
    if (epollfd == -1) {
        // error
        return NULL;
    }

    for (i = 0; i < totalfd; i++) {
        int sockfd = uscfp[i].sockfd;

        event.events = EPOLLOUT | EPOLLET;
        event.data.fd = sockfd;
        epoll_ctl(epollfd, EPOLL_CTL_ADD, sockfd, &event);

        if (sockfd > maxfd) {
            maxfd = sockfd;
        }
    }

    while (!ngx_quit && !ngx_terminate) {
        ngx_http_monitor_str_t* str = ngx_http_monitor_pull_queue( sender->queue );

        ngx_uint_t leftfd = totalfd;

        // FIXME:
        while (leftfd > 0) {
            int nfds = epoll_wait(epollfd, events, maxfd + 1, 1000);

            if (nfds <= 0) {
                // error
                continue;
            }

            for (i = 0; i < nfds; i++) {
                struct sockaddr_in* addr = uscfp[i].sockaddr;
                socklen_t socklen = uscfp[i].socklen;
                int sockfd = uscfp[i].sockfd;

                sendp = str->buf;
                sendlen = str->buflen;

                while (sendlen > 0) {
                    int ret = sendto ( sockfd, sendp, sendlen, 0, addr, socklen );
                    if (ret == -1) {
                        if (EAGAIN == errno) {
                            continue;
                        }

                        ngx_log_error ( NGX_LOG_ERR, sender->log, 0, "Failed to send messege to peer:%s %d, %s",
                                        addr->sin_addr.s_addr,addr->sin_port, strerror(errno));
                    }

                    sendp += ret;
                    sendlen -= ret;
                }

                event.events = EPOLLOUT | EPOLLET;
                event.data.fd = sockfd;
                epoll_ctl(epollfd, EPOLL_CTL_DEL, sockfd, &event);
            }

            leftfd -= nfds;
        }

        free(str->buf);
        free(str);

        for (i = 0; i < totalfd; i++) {
            int sockfd = uscfp[i].sockfd;

            event.events = EPOLLOUT | EPOLLET;
            event.data.fd = sockfd;
            epoll_ctl(epollfd, EPOLL_CTL_ADD, sockfd, &event);
        }
    }

    pthread_cleanup_pop(0);

    return NULL;
}

//.........这里部分代码省略.........
    addr.sin_addr.s_addr = INADDR_ANY;
    addr.sin_port = htons(port);
    if(bind(sd, (struct sockaddr*)&addr, sizeof(addr)) != 0)
        perror("bind");
    // -----------------------------------------------------------

    while(ok >= 0 && !pglobal->stop) {
        DBG("waiting for a UDP message\n");

        // UDP receive ---------------------------------------------
        memset(udpbuffer, 0, sizeof(udpbuffer));
        bytes = recvfrom(sd, udpbuffer, sizeof(udpbuffer), 0, (struct sockaddr*)&addr, &addr_len);
        // ---------------------------------------------------------



        DBG(&