ASDP_Analysis_Module_Median_Threshold.cu

/*
 * Copyright (C) 2024: Arizona Board of Regents on Behalf of the University of Arizona
 */

// This is a client that connects to the first server it encounters and runs an example
// image analysis in CUDA on the data stream coming from one of the cameras.  It gets a
// subset of the frames from the camera over time so that it can keep up easily.

/**
 * @file ASDP_Analysis_Module_CUDA.cu
 * @brief Apache Strap-Down Pilotage example analysis module using CUDA.
 *
* @author ReliaSolve.
* @date April 25, 2024.
*/

#include <iostream>
#include <chrono>
#include <map>
#include <set>
#include <mutex>
#include <thread>
#include <ASDP_Core_API.h>
#include <ASDP_SpinFreeQueue.hpp>
#include <ASDP_BufferPool.h>
#include <ASDP_StreamPacketSortedQueue.h>
#include <CUDABufferPool.h>
#include <cuda.h>
#include <cuda_runtime.h>

using namespace asdp;

/// @brief Global JSON string sender to report analysis results.  Needed because the results are
/// handled in a function called by a handler function.
std::shared_ptr<JSONStringSender> g_jsonSender;

int g_verbosity = 1;            ///< The verbosity level, higher is more verbose.

struct Detection {
  int id;           ///< Unique ID for this detection, which should globally unique.
  float Loc[2];     ///< Location in fractional coordinates, with 0,0 being the top left of the image and 1,1 being the bottom right.
  float Rect[2];    ///< 0,0 means that the detection does not have a bounding box.
  float Chance;     ///< Chance of the type being correct, from 0 to 1.
  char Type[32];    ///< This code offers only a single type, but multiples can be specified
  char IFF[8];      ///< Should be "friend", "foe", "neutral", or "unknown".
};

/// @brief The result of the analysis that is returned from the GPU to the CPU.
#define MAX_DETECTIONS_PER_IMAGE (100)
struct GPUResultType {
  int numDetections = 0; ///< The number of detections filled into the array below.
  Detection detections[MAX_DETECTIONS_PER_IMAGE]; ///< The detections found in the image.  Only the first numDetections are valid.
};

/// @brief The result of the analysis that is passed to the completion thread.
/// @details These will all have been constructed by the thread that is pushing them onto the queue,
/// with custom destructors as needed to destroy them when the shared_ptr is destroyed.
struct ResultType {
  std::shared_ptr<uint8_t> cpuResult;               ///< The result passed back from the GPU, stored in pinned CPU memory.
  uint32_t cameraID = 0;                            ///< The camera ID that the result is for.
  Time timestamp = { 0, 0 };                        ///< The timestamp of the frame that was analyzed.
  std::shared_ptr<cudaEvent_t> completionEventPtr;  ///< The event to signal when the result is ready.
};

/// @brief Structure to hold the data needed to send data to the GPU and run the kernel.
/// @details These will all have been constructed by the thread that is pushing them onto the queue,
/// with custom destructors as needed to free the memory when the shared_ptr is destroyed.
struct DataToSendToGPU {
  std::shared_ptr<StreamPacket> streamPacketPtr;    ///< The stream packet that was received, which includes camera ID
  std::shared_ptr<unsigned char> cpuImageBufferPtr; ///< The pinned-memory buffer on the CPU that holds the image data
  std::shared_ptr<unsigned char> gpuImageBufferPtr; ///< The buffer on the GPU that holds the image data
  std::shared_ptr<unsigned char> gpuMedianBufferPtr;///< The buffer on the GPU that holds the median-filtered image data
  std::shared_ptr<GPUResultType> gpuResultPtr;      ///< The buffer on the GPU that holds the result data
  std::shared_ptr<GPUResultType> gpuPreviousResultPtr;  ///< The buffer on the GPU that holds the previous result data
  std::shared_ptr<cudaStream_t> streamPtr;          ///< Stream to use to for the copy and kernel run
  std::shared_ptr<int> nextDroneIDPtr;              ///< The next drone ID to use for detections; used as atomic in kernels
};

/// @brief Compute the median-filtered image.
/// @param data The image data.
/// @param median The median-filtered image data buffer to write to.
/// @param width The width of the data.
/// @param height The height of the data.
__global__ void medianKernel(uint16_t* data, uint16_t* median, int width, int height)
{
  int idx = threadIdx.x + blockIdx.x * blockDim.x;
  int idy = threadIdx.y + blockIdx.y * blockDim.y;

  // This avoids running on the edge pixels to avoid the need for boundary checks.  A real
  // kernel might check those pixels.
  uint16_t neighbors[9];
  if ( (idx > 0) && (idx < width - 1) && (idy > 0) && (idy < height - 1)) {
    // Get the 3x3 neighborhood of pixels around the current pixel.
    int count = 0;
    for (int y = -1; y <= 1; ++y) {
      for (int x = -1; x <= 1; ++x) {
        neighbors[count++] = data[(idy + y) * width + (idx + x)];
      }
    }
    // Sort the neighbors to find the median value.  This is a simple bubble sort, which is not efficient but is simple to implement.
    for (int i = 0; i < 9; ++i) {
      for (int j = i + 1; j < 9; ++j) {
        if (neighbors[j] < neighbors[i]) {
          uint16_t temp = neighbors[i];
          neighbors[i] = neighbors[j];
          neighbors[j] = temp;
        }
      }
    }
    median[idy * width + idx] = neighbors[4]; // The median value is the middle value after sorting.
  }
}

/// @brief Compute the median-filtered image.
/// @param data The image data.
/// @param nextDroneID The next drone ID to use for detections; used as atomic in kernels.
/// @param median The median-filtered image data buffer to write to.
/// @param width The width of the data.
/// @param height The height of the data.
/// @param border The border to ignore around the edges of the image, which will not be processed to avoid the need for boundary checks.
/// @param threshold The threshold in pixel counts for drone above background.
/// @param neighborStride The stride to use to determine neighbors.
/// @param result The result buffer to write the results to.
/// @param count A device pointer to an integer that is to be used as an atomic counter for the number of detections found,
///        which will be used to determine the next index in the result buffer to write to.
__global__ void detectKernel(uint16_t* data, int* nextDroneID, int width, int height, int border, int threshold, int neighborStride,
  GPUResultType* result)
{
  int idx = threadIdx.x + blockIdx.x * blockDim.x;
  int idy = threadIdx.y + blockIdx.y * blockDim.y;

  // Resrict ourselves to running on on pixels that are at least neighborStride + border pixels from the
  // edge of the image to avoid the need for boundary checks.
  if ((idx < border + neighborStride) || (idx >= width - border - neighborStride) ||
      (idy < border + neighborStride) || (idy >= height - border - neighborStride)) {
    return;
  }

  // Find the maximum value at +/- neighborStride pixels in the x and y directions along with finding our
  // value.
  uint16_t value = data[idy * width + idx];
  uint16_t maxNeighborValue = 0;
  for (int y = -neighborStride; y <= neighborStride; y += neighborStride) {
    for (int x = -neighborStride; x <= neighborStride; x += neighborStride) {
      if ((x == 0) && (y == 0)) {
        continue;
      }
      uint16_t neighborValue = data[(idy + y) * width + (idx + x)];
      if (neighborValue > maxNeighborValue) {
        maxNeighborValue = neighborValue;
      }
    }
  }

  // If our value is above the threshold compared to the maximum neighbor value, then we have a detection.
  if (value >= maxNeighborValue + threshold) {
    char typeStr[32] = "ASDP_drone";        ///< Exact size of the expected output.
    char iffStr[8] = "foe";                 ///< Exact size of the expected output.
    int which = atomicAdd(&result->numDetections, 1);   ///< Returns the number before it was incremented.
    int id = atomicAdd(nextDroneID, 1);     ///< Returns the number before it was incremented.
    if (which >= MAX_DETECTIONS_PER_IMAGE) {
      // Too many detections, ignore this one.
      return;
    }
    //result->numDetections = 1;
    result->detections[which].id = id;
    result->detections[which].Loc[0] = idx / (width - 1.0f);
    result->detections[which].Loc[1] = idy / (height - 1.0f);
    result->detections[which].Rect[0] = 2 * neighborStride / (width - 1.0f);
    result->detections[which].Rect[1] = 2 * neighborStride / (height - 1.0f);
    result->detections[which].Chance = 1.0f;
    for (int i = 0; i < 32; ++i) {
      result->detections[which].Type[i] = typeStr[i];
    }
    for (int i = 0; i < 8; ++i) {
      result->detections[which].IFF[i] = iffStr[i];
    }
  }
}

/// @brief Compact in space by merging too-close detections and in time by keeping IDs from previous results.
/// @param result The result buffer to read the current results from and write the compacted results to.
/// @param previousResult The result buffer to read the previous results from and write the current results
///        to for use in the next iteration.
/// @param neighborStride The stride to use to determine neighbors for both merging current results and matching to previous results.
__global__ void compactKernel(GPUResultType* result, GPUResultType* previousResult, int neighborStride)
{
  // We only have one thread, so we don't need to check if we should run -- we always do.

  // Ensure that the number of detections is not greater than the maximum.
  // The kernel that locates detections will not fill the table beyond this, but may increment the count beyond this.
  if (result->numDetections > MAX_DETECTIONS_PER_IMAGE) {
    result->numDetections = MAX_DETECTIONS_PER_IMAGE;
  }

  // Go through each detection and check whether any later detections are within the neighbor stride in X and Y.
  // If so, average the location, weighting the differences by 1/n as every new point is added to the same
  // detection.
  for (int i = 0; i < result->numDetections - 1; ++i) {
    Detection& detection = result->detections[i];
    int count = 1;
    for (int j = i + 1; j < result->numDetections; ++j) {
      Detection& otherDetection = result->detections[j];
      if (abs(detection.Loc[0] - otherDetection.Loc[0]) <= neighborStride &&
          abs(detection.Loc[1] - otherDetection.Loc[1]) <= neighborStride) {
        // This is within the neighbor stride, so we consider it the same drone and average the location.
        count++;
        detection.Loc[0] = ((count - 1) * detection.Loc[0] + otherDetection.Loc[0]) / count;
        detection.Loc[1] = ((count - 1) * detection.Loc[1] + otherDetection.Loc[1]) / count;
        // Take the highest chance value.
        detection.Chance = fmaxf(detection.Chance, otherDetection.Chance);

        // Move the last detection to the one we're replacing and decrement the count to delete the current
        // one.  Also decrement our inner loop counter so we re-try this location after incrementing it at
        // the next iteration.
        otherDetection = result->detections[--result->numDetections];
        j--;
      }
    }
  }

  // For each entry in the current results, see if there is an entry in the previous results
  // that is within the neighbor stride. If so, then copy the ID from the previous results to the
  // current results.
  for (int i = 0; i < result->numDetections; ++i) {
    Detection& detection = result->detections[i];
    for (int j = 0; j < previousResult->numDetections; ++j) {
      Detection& previousDetection = previousResult->detections[j];
      if (abs(detection.Loc[0] - previousDetection.Loc[0]) <= neighborStride &&
          abs(detection.Loc[1] - previousDetection.Loc[1]) <= neighborStride) {
        // This is within the neighbor stride, so we consider it the same drone and copy the ID.
        detection.id = previousDetection.id;
        break;
      }
    }
  }

  // Copy the current results entries to the previous results entries, resetting its count to match.
  previousResult->numDetections = result->numDetections;
  for (int i = 0; i < result->numDetections; ++i) {
    previousResult->detections[i] = result->detections[i];
  }
}

/// @brief Function to copy data to the GPU, run the kernel, and enqueue a result.
/// It must create and record the event after all operations are complete.  All operations must be
/// done on the stream that is passed in and they must all be asynchronous.
/// @param width The width of the image data.
/// @param height The height of the image data.
/// @param threshold The threshold in pixel counts for drone above background.  Can change during run.
/// @param neighborStride The stride to use to determine neighbors.  Can change during run.
/// @param done A flag that is set to true when the program is done.
/// @param inQueue The queue that we receive requests on.
/// @param outQueue The queue that we send results to.
/// @param batchSize The number of lines to send to the GPU at once.  This is tuned to trade off latency
/// for throughput, and it should be set to a value that is large enough to amortize the cost of sending
/// data to the GPU, but small enough to keep latency low.  The value of 16 is a good starting point.
static void CopyDataToGPUAndRunKernel(uint16_t width, uint16_t height,
  std::atomic_int &threshold, std::atomic_int &neighborStride,
  std::atomic<bool>& done,
  std::shared_ptr< SpinFreeQueue< std::shared_ptr<DataToSendToGPU> > > inQueue,
  std::shared_ptr< SpinFreeQueue< std::shared_ptr<ResultType> > > outQueue,
  size_t batchSize)
{
  Status status;

  // Buffer pool for pinned CPU memory buffers to copy GPU results back.
  render::CUDABufferPool bufferPool(sizeof(GPUResultType), 100, true);

  while (!done) {
    std::shared_ptr<DataToSendToGPU> data;
    // Time out after 10 milliseconds so that we can check the done flag frequently.
    if (inQueue->dequeue(data, std::chrono::milliseconds(10))) {

      // Parse all of the messages in the stream packet, handling each of them in turn.
      std::shared_ptr<Message> message;
      status = data->streamPacketPtr->GetNextMessage(message);
      if (OKAY != status) {
        std::cerr << "CopyDataToGPUAndRunKernel: GetNextMessage() failed: " << ErrorMessage(status) << std::endl;
        done = true;
        return;
      }
      while (message != nullptr) {
        MessageID messageType;
        if (OKAY != message->GetType(messageType)) { return; }
        if (messageType == CONSOLIDATED_FRAME_DATA) {

            MessageConsolidatedFrameData frameData(*message);
            if (frameData.GetConstructorStatus() != asdp::OKAY) {
              std::cerr << "Error constructing FrameData message: " << ErrorMessage(frameData.GetConstructorStatus()) << std::endl;
              return;
            }

            bool isBeginFrame;
            status = frameData.GetBeginFrameFlag(isBeginFrame);
            if (OKAY != status) {
              std::cerr << "CopyDataToGPUAndRunKernel: GetBeginFrameFlag() failed: " << ErrorMessage(status) << std::endl;
              done = true;
              return;
            }
            if (isBeginFrame) {
              // Initialize the result to zero.
              cudaMemsetAsync(&data->gpuResultPtr, 0, sizeof(GPUResultType), *data->streamPtr);
            }

            // Copy the data to the pinned CPU memory buffer, and then asynchronously to the GPU buffer as
            // we get enough data for a minimum block size.  We send the data to the GPU in chunks so that
            // we amortize the per-send cost, but we send in chunks to reduce the latency and enable overlap
            // between data copying and processing (which increases throughput).

            // Get the region to copy and the data pointer from the message.
            if (frameData.GetConstructorStatus() != OKAY) {
              std::cerr << "CopyDataToGPUAndRunKernel: Failed to construct MessageFrameData: " << ErrorMessage(frameData.GetConstructorStatus()) << std::endl;
              done = true;
              return;
            }
            uint16_t left, right, top, bottom;
            status = frameData.GetLeft(left);
            if (OKAY != status) {
              std::cerr << "CopyDataToGPUAndRunKernel: GetLeft() failed: " << ErrorMessage(status) << std::endl;
              done = true;
              return;
            }
            status = frameData.GetRight(right);
            if (OKAY != status) {
              std::cerr << "CopyDataToGPUAndRunKernel: GetRight() failed: " << ErrorMessage(status) << std::endl;
              done = true;
              return;
            }
            status = frameData.GetTop(top);
            if (OKAY != status) {
              std::cerr << "CopyDataToGPUAndRunKernel: GetTop() failed: " << ErrorMessage(status) << std::endl;
              done = true;
              return;
            }
            status = frameData.GetBottom(bottom);
            if (OKAY != status) {
              std::cerr << "CopyDataToGPUAndRunKernel: GetBottom() failed: " << ErrorMessage(status) << std::endl;
              done = true;
              return;
            }
            uint8_t* dataPtr;
            status = frameData.GetDataPointer(dataPtr);
            if (OKAY != status) {
              std::cerr << "CopyDataToGPUAndRunKernel: GetDataPointer() failed: " << ErrorMessage(status) << std::endl;
              done = true;
              return;
            }

            // Copy the image data to the pinned CPU memory buffer.
            uint16_t* cpuBuffer = reinterpret_cast<uint16_t*>(data->cpuImageBufferPtr.get());
            uint16_t* dataPtr16 = reinterpret_cast<uint16_t*>(dataPtr);
            memcpy(cpuBuffer + top * width + left, dataPtr16, (right - left + 1) * (bottom - top + 1) * sizeof(uint16_t));

            // Copy the image data to the GPU if we've completed a chunk of lines.
            // We assume that the number of lines coming in from the camera is smaller than the batch size, so that we will
            // send at most one batch.  We check every line from bottom to the top of the region and send the batch including
            // that line if it is ever the last line in the region.
            for (uint16_t line = top; line <= bottom; ++line) {
              if ((line + 1) % batchSize == 0) {
                // The offset is to the start of the region, which is batchSize-1 lines before the current line.
                size_t offset = (line + 1 - batchSize) * width * sizeof(uint16_t);
                // Copy the batch to the GPU.
                cudaError_t ret =  cudaMemcpyAsync(data->gpuImageBufferPtr.get() + offset, data->cpuImageBufferPtr.get() + offset,
                  batchSize * width * sizeof(uint16_t),
                  cudaMemcpyHostToDevice, *data->streamPtr);
                if (ret != cudaSuccess) {
                  std::cerr << "CopyDataToGPUAndRunKernel: cudaMemcpyAsync() failed: " << cudaGetErrorString(ret) << std::endl;
                  done = true;
                  return;
                }
              }
            }

            bool isEndFrame;
            status = frameData.GetEndFrameFlag(isEndFrame);
            if (OKAY != status) {
              std::cerr << "CopyDataToGPUAndRunKernel: GetEndFrameFlag() failed: " << ErrorMessage(status) << std::endl;
              done = true;
              return;
            }
            if (isEndFrame) {
              // Run the kernel and enqueue the result.
              {
                // The result to be sent forward once the asynchronous operations have all been queued.
                std::shared_ptr <ResultType> result = std::make_shared<ResultType>();

                // The target of an asychronous copy from GPU must be in pinned CPU memory.
                result->cpuResult = bufferPool.GetBuffer(true, 0);
                if (result->cpuResult == nullptr) {
                  std::cerr << "CopyDataToGPUAndRunKernel: Failed to get buffer"<< std::endl;
                  done = true;
                  return;
                }

                // Store the camera ID in the result.
                status = frameData.GetCameraID(result->cameraID);
                if (OKAY != status) {
                  std::cerr << "CopyDataToGPUAndRunKernel: GetCameraID() failed: " << ErrorMessage(status) << std::endl;
                  done = true;
                  return;
                }

                // Store the timestamp in the result.
                status = frameData.GetTime(result->timestamp);
                if (OKAY != status) {
                  std::cerr << "CopyDataToGPUAndRunKernel: GetTime() failed: " << ErrorMessage(status) << std::endl;
                  done = true;
                  return;
                }

                // Run the median kernel.  All of the data for this image will have been copied to the GPU by now.
                dim3 blockSize(16, 16);
                dim3 gridSize((width + blockSize.x - 1) / blockSize.x, (height + blockSize.y - 1) / blockSize.y);
                medianKernel << <gridSize, blockSize, 0, *data->streamPtr >> >
                  (reinterpret_cast<uint16_t*>(data->gpuImageBufferPtr.get()), reinterpret_cast<uint16_t*>(data->gpuMedianBufferPtr.get()),
                    width, height);

                // Zero the GPU counter for the number of detections found so that the kernel can use it as an atomic counter.
                // NOTE: This relies on the structure being laid out with numDetections as the first member.
                cudaMemsetAsync(data->gpuResultPtr.get(), 0, sizeof(GPUResultType::numDetections), *data->streamPtr);

                // Run the detection kernel.
                detectKernel << <gridSize, blockSize, 0, *data->streamPtr >> >
                  (reinterpret_cast<uint16_t*>(data->gpuMedianBufferPtr.get()), data->nextDroneIDPtr.get(),
                    width, height, 1,
                    threshold, neighborStride,
                    data->gpuResultPtr.get());

                // Run a single block with a single thread to compact the results because we expect a number of responses
                // on a given drone. When results are within the neighborStride, they will be considered the same dron and
                // will have their locations averaged and the lowest ID and highest chance among them.
                dim3 singleBlockSize(1, 1);
                dim3 singleGridSize(1, 1);
                compactKernel << <singleGridSize, singleBlockSize, 0, *data->streamPtr >> >
                  (data->gpuResultPtr.get(), data->gpuPreviousResultPtr.get(), neighborStride);

                // Copy the result back to the CPU.
                cudaMemcpyAsync(result->cpuResult.get(), data->gpuResultPtr.get(), sizeof(GPUResultType), cudaMemcpyDeviceToHost, *data->streamPtr);

                // Note: This must be done after all other operations on the stream so that it will wait for them to complete.
                // Create the completion event, storing a pointer to it in a shared pointer whose destructor will delete
                // the event when there are no more references to it.  Record the event so that the caller can wait for it.
                cudaEvent_t* eventPtr = new cudaEvent_t;
                cudaEventCreate(eventPtr);
                cudaEventRecord(*eventPtr, *(data->streamPtr));
                result->completionEventPtr = std::shared_ptr<cudaEvent_t>(eventPtr,
                  [](cudaEvent_t* ptr) { cudaEventDestroy(*ptr); delete ptr; }
                );

                // Enqueue the result for the completion thread.
                outQueue->enqueue(result);
              }
            }
        } // End of switch on frame data.

        status = data->streamPacketPtr->GetNextMessage(message);
        if (OKAY != status) {
          done = true;
          std::cerr << "CopyDataToGPUAndRunKernel: GetNextMessage() failed: " << ErrorMessage(status) << std::endl;
          return;
        }
      } // End of while we have messages in the stream packet.
    } // End of if we got a message from the queue.
  } // End of while we are not done.

  // Reset our output queue so it will free all of our buffers
  outQueue.reset();
}

/// This example copies the data to the GPU, runs the kernel, and enqueues the result.
/// @brief Function to handle result data.
/// @param result The result data to handle.
void HandleGPUResults(ResultType const& result)
{
  // Reinterpret the uint8_t pointer as a GPUResultType pointer so that we can access the result data.
  GPUResultType* gpuResult = reinterpret_cast<GPUResultType*>(result.cpuResult.get());
  if (gpuResult->numDetections) {
    if (g_verbosity >= 2) {
      std::cout << "  Camera " << result.cameraID << " has " << gpuResult->numDetections << " detections:" << std::endl;
    }
  }
  for (uint32_t i = 0; i < gpuResult->numDetections; ++i) {
    const Detection& detection = gpuResult->detections[i];
    std::string name = "Drone " + std::to_string(detection.id);
    if (g_verbosity >= 2) {
      std::cout << "    " << name << ", Type=" << detection.Type << ", IFF=" << detection.IFF
        << ", Chance=" << detection.Chance << ", Loc=(" << detection.Loc[0] << "," << detection.Loc[1] << ")"
        << ", Rect=(" << detection.Rect[0] << "," << detection.Rect[1] << ")" << std::endl;
    }
    std::string rectString;
    if (detection.Rect[0] != 0 || detection.Rect[1] != 0) {
      rectString = ",\"Rect\":[" + std::to_string(detection.Rect[0]) + "," + std::to_string(detection.Rect[1]) + "]";
    }
    std::string jsonString = std::string("{\"CamID\":") + std::to_string(result.cameraID)
      + ",\"Time\":"
      + "[" + std::to_string(result.timestamp.seconds) + "," + std::to_string(result.timestamp.microseconds) + "]"
      + ",\"Name\":\"" + name + "\""
      + ",\"Loc\":[" + std::to_string(detection.Loc[0]) + "," + std::to_string(detection.Loc[1]) + "]"
      + rectString
      + ",\"Class\":[{\"Type\":\"" + detection.Type + "\",\"Chance\":" + std::to_string(detection.Chance)
      + ",\"IFF\":\"" + detection.IFF + "\"}]}";
    if (g_jsonSender == nullptr) {
      std::cerr << "HandleGPUResults Error: JSON string sender is not initialized." << std::endl;
      return;
    }
    Status status = g_jsonSender->Send(jsonString);
    if (status != asdp::OKAY) {
      std::cerr << "HandleGPUResults Error sending JSON string: " << ErrorMessage(status) << std::endl;
      return;
    }
  }
}

/// @brief Function to wait for and handle GPU results.
/// It must create and record the event after all operations are complete.  All operations must be
/// done on the stream that is passed in and they must all be asynchronous.
/// @param done A flag that is set to true when the program is done.
/// @param inQueue The queue that we receive results on.
static void HandleResults(std::atomic<bool>& done,
  std::shared_ptr< SpinFreeQueue< std::shared_ptr<ResultType> > > inQueue)
{
  while (!done) {
    std::shared_ptr<ResultType> result;
    // Time out after 10 milliseconds so that we can check the done flag frequently.
    if (inQueue->dequeue(result, std::chrono::milliseconds(10))) {
      // Wait for the completion event to be recorded.
      cudaEventSynchronize(*(result->completionEventPtr));

      // Handle the result.
      HandleGPUResults(*result);
    }
  }
}

static void ReceiveDataThread(ReceiverUDP& receiveSocket, size_t maxBytesPerPacket, std::atomic<bool>& done,
  std::shared_ptr<unsigned char> cpuImageBufferPtr, std::shared_ptr<unsigned char> gpuImageBufferPtr,
  std::shared_ptr<unsigned char> gpuMedianBufferPtr,
  std::shared_ptr<int> nextDroneIDPtr,
  std::shared_ptr<GPUResultType> gpuResultPtr, std::shared_ptr<GPUResultType> gpuPreviousResultPtr,
  std::shared_ptr<cudaStream_t> streamPtr,
  std::shared_ptr< SpinFreeQueue< std::shared_ptr<DataToSendToGPU> > > outQueue)
{
  // Generate a buffer pool to use to get pre-allocated buffers for reading the data from
  // the network.  Initially fill it with 100 buffers to give us enough to handle buffering a fraction
  // of a frame before the first packets are handled.  It will automatically expand if needed.
  BufferPool bufferPool(maxBytesPerPacket, 100);

  // Use a sorting queue to ensure that we process the messages in order even if the UDP packets
  // arrive out of order.
  StreamPacketSortedQueue sortedQueue(50);

  // Loop through and receive packets until we've been told to quit.
  size_t packetsReceived = 0;
  DataToSendToGPU data;
  while (!done) {

    // Get the next packet into a preallocated buffer, timing out quickly to ensure that we check
    // the done flag.
    std::shared_ptr< std::vector<uint8_t> > buffer = bufferPool.GetBuffer();
    size_t offset = 0;
    std::shared_ptr<StreamPacket> packet;
    Status status = receiveSocket.ReceiveStreamPacket(0.1, packet, offset, buffer);
    if (status == TIMEOUT) {
      continue;
    }
    if (status != OKAY) {
      std::cerr << "Error receiving data: " << ErrorMessage(status) << std::endl;
      done = true;
      break;
    }

    // Add to the sorted queue and then handle any messages that are ready to be processed.
    std::list< std::shared_ptr<StreamPacket> > readyPackets = sortedQueue.AddPacket(packet);
    if (readyPackets.size() > 1) {
      std::cerr << "Warning: More than one packet ready to process (re-ordered or missing packet)." << std::endl;
    }
    while (!readyPackets.empty()) {

      std::shared_ptr<asdp::StreamPacket> streamPacket = readyPackets.front();
      readyPackets.pop_front();

      // Increment the number of packets received
      packetsReceived++;

      // Enqueue the packet for processing.
      data.streamPacketPtr = streamPacket;
      data.cpuImageBufferPtr = cpuImageBufferPtr;
      data.gpuImageBufferPtr = gpuImageBufferPtr;
      data.gpuMedianBufferPtr = gpuMedianBufferPtr;
      data.nextDroneIDPtr = nextDroneIDPtr;
      data.gpuResultPtr = gpuResultPtr;
      data.gpuPreviousResultPtr = gpuPreviousResultPtr;
      data.streamPtr = streamPtr;
      outQueue->enqueue(std::make_shared<DataToSendToGPU>(data));
    }
  }

  // Release our out-queue pointer so it will be destroyed and release all its resources back to our
  // buffer pool.
  outQueue.reset();
}

std::shared_ptr<Message> WaitForMessageType(std::shared_ptr<Receiver> receiver, MessageID type, float seconds)
{
  std::shared_ptr<Message> empty;   ///< We return this on failure.
  std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
  do {
    std::shared_ptr<StreamPacket> response;
    size_t offset = 0;
    Status status = receiver->ReceiveStreamPacket(0, response, offset);
    if ((status != OKAY) && (status != TIMEOUT)) {
      return empty;
    }
    if (response != nullptr) {
      std::shared_ptr<Message> message;
      status = response->GetNextMessage(message);
      if (status != OKAY) {
        return empty;
      }
      while (message != nullptr) {
        MessageID messageType;
        status = message->GetType(messageType);
        if (status != OKAY) {
          return empty;
        }
        if (messageType == type) {
          // Worked!
          return message;
        }
        status = response->GetNextMessage(message);
        if (status != OKAY) {
          return empty;
        }
      }
    }
  } while (std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count() <= seconds);

  return empty;
}

// Reader thread: blocks on std::getline but does not block main thread.
void stdinReader(SpinFreeQueue<std::string>& queue)
{
  std::string line;
  while (std::getline(std::cin, line)) {
    queue.enqueue(std::move(line));
  }
}

void usage(std::string name)
{
  std::cerr << "Usage: " << name << " <options> <ip_address>" << std::endl;
  std::cerr << " <ip_address>  The IP address to listen on." << std::endl;
  std::cerr << std::endl;
  std::cerr << " Options:" << std::endl;
  std::cerr << "  --frameStride <frameStride>  Read one out of every this many frames. Default of 1 for every frame." << std::endl;
  std::cerr << "  --duration <durationSeconds>  Run for this many seconds (default 1e10)" << std::endl;
  std::cerr << "  --camera <cameraID>       The camera to analyze.  Defaults to all cameras, can specify multiple cameras." << std::endl;
  std::cerr << "  --analysisServer URL      URL of the analysis module server. Default is tcp://localhost." << std::endl;
  std::cerr << "  --replay <stream id>      ID of the stream to replay (1+)." << std::endl;
  std::cerr << "  --replayTime <sec> <usec> Replay start time in seconds and microseconds (default 10 0)." << std::endl;
  std::cerr << "  --serial <serial number>  The serial number of the server to connect to (default -1 means any)." << std::endl;
  std::cerr << "  --threshold <threshold>   The threshold in pixel counts for drone above background (default 350)." << std::endl;
  std::cerr << "  --neighborStride <neighborStride>  The stride to use to determine neighbors (default 20)." << std::endl;
  std::cerr << "  --verbosity <level>       Set the verbosity level (default 1, higher is more verbose)." << std::endl;
}

int main(int argc, char** argv)
{
  uint32_t frameStride = 1;     ///< Read one out of every this many frames. Set to 1 for every frame.
  std::string ip_address;       ///< The IP address to listen on.
  std::atomic_int threshold{ 350 };          ///< The threshold in pixel counts for drone above background.
  std::atomic_int neighborStride{ 20 };      ///< The stride to use to determine neighbors.
  float durationSeconds = 1e10; ///< Run for this many seconds
  std::string analysisURL = "tcp://localhost";  ///< URL of the analysis module server.
  std::set<uint32_t> cameraIDs; ///< The camera IDs to analyze.
  uint32_t replayStreamID = 0;  ///< The stream ID to replay, 0 for live.
  Time replayStartTime = { 10, 0 }; ///< The time to start the replay from, default 10 seconds.
  int serialNumber = -1;        ///< The serial number of the server to connect to, -1 means any.
  size_t realParams = 0;

  // Parse the command line arguments, with the first non-flag argument being the
  // name of the IP address to listen on.
  for (int i = 1; i < argc; ++i) {
    if (std::string(argv[i]) == "--analysisServer") {
      if ((i + 1) >= argc) {
        usage(argv[0]);
        return 1;
      }
      analysisURL = argv[++i];
    }
    else if (std::string("--frameStride") == argv[i]) {
      if (++i >= argc) {
        usage(argv[0]);
        return 2;
      }
      frameStride = std::stoi(argv[i]);
    }
    else if (std::string("--duration") == argv[i]) {
      if (++i >= argc) {
        usage(argv[0]);
        return 2;
      }
      durationSeconds = std::stof(argv[i]);
    }
    else if (std::string("--camera") == argv[i]) {
      if (++i >= argc) {
        usage(argv[0]);
        return 2;
      }
      uint32_t cameraID = std::stoi(argv[i]);
      if (cameraID == 0) {
        usage(argv[0]);
        return 2;
      }
      cameraIDs.insert(cameraID);
    }
    else if (std::string("--replay") == argv[i]) {
      if (++i >= argc) {
        usage(argv[0]);
        return 2;
      }
      replayStreamID = std::stoi(argv[i]);
    }
    else if (std::string("--serial") == argv[i]) {
      if (++i >= argc) {
        usage(argv[0]);
        return 2;
      }
      serialNumber = std::stoi(argv[i]);
    }
    else if (std::string("--threshold") == argv[i]) {
      if (++i >= argc) {
        usage(argv[0]);
        return 2;
      }
      threshold = std::stoi(argv[i]);
    }
    else if (std::string("--neighborStride") == argv[i]) {
      if (++i >= argc) {
        usage(argv[0]);
        return 2;
      }
      neighborStride = std::stoi(argv[i]);
    }
    else if (std::string("--replayTime") == argv[i]) {
      if ((i + 2) >= argc) {
        usage(argv[0]);
        return 2;
      }
      replayStartTime.seconds = std::stoi(argv[++i]);
      replayStartTime.microseconds = std::stoi(argv[++i]);
    }
    else if (std::string("--verbosity") == argv[i]) {
      if (++i >= argc) {
        usage(argv[0]);
        return 2;
      }
      g_verbosity = std::stoi(argv[i]);
    }
    else if (argv[i][0] == '-') {
      std::cerr << "Unknown flag: " << argv[i] << std::endl;
      return 1;
    }
    else switch (realParams++) {
    case 0:
      ip_address = argv[i];
      break;
    default:
      usage(argv[0]);
      return 2;
    }
  }
  if (realParams != 1) {
    usage(argv[0]);
    return 2;
  }

  // Run inside a block so that the destructors will be called for all objects before we exit.
  {
    // Open a JSON string server to report analysis results to.
    Status s = JSONStringSender::Create(analysisURL, g_jsonSender);
    if (s != OKAY) {
      std::cerr << "Failed to create JSONStringSender: " << ErrorMessage(s) << std::endl;
      return 100;
    }

    // Open a client, specifying the IP address to listen on.
    CoreClient client(ip_address);
    if (client.GetConstructorStatus() != OKAY) {
      std::cerr << "Failed to open client: " << ErrorMessage(client.GetConstructorStatus()) << std::endl;
      return 3;
    }
    if (g_verbosity >= 1) {
      std::cout << "Listening for servers on " << ip_address << std::endl;
    }

    // Wait for up to two seconds to allow servers to send Discovery messages.
    std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
    std::map<uint32_t, std::string> servers;
    Status threadStatus;
    Status status;
    do {
      status = client.GetDiscoveryThreadStatus(threadStatus);
      if (status != OKAY) {
        std::cerr << "Failed to get discovery thread status: " << ErrorMessage(status) << std::endl;
        return 4;
      }
      if (threadStatus != OKAY) {
        std::cerr << "Discovery thread status: " << ErrorMessage(threadStatus) << std::endl;
        return 5;
      }
      status = client.IdentifiedServers(servers);
      if (status != OKAY) {
        std::cerr << "Failed to get identified servers: " << ErrorMessage(status) << std::endl;
        return 6;
      }

      // If we have been asked for a specific serial number, remove all others.
      if (serialNumber >= 0) {
        std::map<uint32_t, std::string> filteredServers;
        for (const auto& server : servers) {
          uint32_t serverSerialNumber = server.first;
          if (serverSerialNumber == static_cast<uint32_t>(serialNumber)) {
            filteredServers[server.first] = server.second;
          }
        }
        servers = filteredServers;
      }

      if (!servers.empty()) { break; }
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    } while (std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count() <= 2.0);
    if (servers.empty()) {
      std::cerr << "No servers found; be sure to run the server first." << std::endl;
      return 7;
    }
    if (g_verbosity >= 1) {
      std::cout << "Servers found: " << servers.size() << std::endl;
      for (const auto& server : servers) {
        std::cout << "  " << server.second << std::endl;
      }
    }

    // Connect to the first server found.
    if (g_verbosity >= 1) {
      std::cout << "Connecting to " << servers.begin()->second << std::endl;
    }
    uint16_t major, minor, patch;
    status = client.ConnectToServer(servers.begin()->second, major, minor, patch);
    if (status != OKAY) {
      std::cerr << "Failed to connect to server: " << ErrorMessage(status) << std::endl;
      return 8;
    }
    if (g_verbosity >= 1) {
    std::cout << "  Connected to server version " << major << "." << minor << "." << patch
      << " with serial number " << servers.begin()->first << std::endl;
    }

    // Get the main stream receiver
    std::shared_ptr<Receiver> receiver;
    status = client.GetMainStreamReceiver(receiver);
    if (status != OKAY) {
      std::cerr << "Failed to get main stream receiver: " << ErrorMessage(status) << std::endl;
      return 10;
    }

    // Ensure that we get a state message from the server within a reasonable time.
    // Report information about the cameras that were found.
    std::shared_ptr<Message> msg = WaitForMessageType(receiver, STATE, 5.0);
    if (msg == nullptr) {
      std::cerr << "Did not get state message." << std::endl;
      return 12;
    }
    MessageState state(*msg);
    if (state.GetConstructorStatus() != OKAY) {
      std::cerr << "Failed to construct state message: " << ErrorMessage(state.GetConstructorStatus()) << std::endl;
      return 13;
    }
    std::vector<CameraInfo> cameras;
    status = state.GetCameras(cameras);
    if (g_verbosity >= 1) {
      std::cout << "Found " << cameras.size() << " cameras" << std::endl;
    }

    // If we have an empty set of camera IDs, then we want to analyze all cameras.
    if (cameraIDs.empty()) {
      for (uint32_t ID = 1; ID <= cameras.size(); ID++) {
        cameraIDs.insert(ID);
      }
    }

    // We need more than one GPU-feeding thread to handle all of the cameras at once.
    size_t NUM_GPU_SEND_THREADS = 3;
    std::vector<std::shared_ptr< SpinFreeQueue< std::shared_ptr<DataToSendToGPU> > >> dataQueues;
    for (size_t i = 0; i < NUM_GPU_SEND_THREADS; i++) {
      dataQueues.push_back(std::make_shared< SpinFreeQueue< std::shared_ptr<DataToSendToGPU> > >());
    }

    // Construct shared pointers to the data structures that we'll need to run the analysis, with the
    // custom destructors that will clean up when the shared_ptr is destroyed.
    std::atomic<bool> done(false);
    std::shared_ptr< SpinFreeQueue< std::shared_ptr<ResultType> > > resultQueue =
      std::make_shared< SpinFreeQueue< std::shared_ptr<ResultType> > >();
    std::vector< std::shared_ptr<unsigned char> > cpuPinnedImageBuffers;
    std::vector< std::shared_ptr<unsigned char> > gpuImageBuffers;
    std::vector< std::shared_ptr<unsigned char> > gpuMedianBuffers;
    std::vector< std::shared_ptr<GPUResultType> > gpuResultBuffers;
    std::vector< std::shared_ptr<GPUResultType> > gpuPreviousResultBuffers;
    std::shared_ptr<int> nextDroneIDPtr;
    std::vector< std::shared_ptr<cudaStream_t> >  streams;
    std::vector< std::shared_ptr<ReceiverUDP> >   UDPReceivers;
    for (size_t i = 0; i < cameras.size(); i++) {
      // Allocate pinned memory for the CPU image buffer.
      unsigned char* cpuPinnedImageBuffer;
      cudaMallocHost(&cpuPinnedImageBuffer, cameras[i].width * cameras[i].height * sizeof(uint16_t));
      cpuPinnedImageBuffers.push_back(std::shared_ptr<unsigned char>(cpuPinnedImageBuffer,
        [](unsigned char* ptr) { cudaFreeHost(ptr); }
      ));

      // Allocate memory for the GPU image buffer.
      unsigned char* gpuImageBuffer;
      cudaMalloc(&gpuImageBuffer, cameras[i].width * cameras[i].height * sizeof(uint16_t));
      gpuImageBuffers.push_back(std::shared_ptr<unsigned char>(gpuImageBuffer,
        [](unsigned char* ptr) { cudaFree(ptr); }
      ));

      // Allocate memory for the GPU median buffer.
      unsigned char* gpuMedianBuffer;
      cudaMalloc(&gpuMedianBuffer, cameras[i].width* cameras[i].height * sizeof(uint16_t));
      gpuMedianBuffers.push_back(std::shared_ptr<unsigned char>(gpuMedianBuffer,
        [](unsigned char* ptr) { cudaFree(ptr); }
      ));

      // Allocate memory for the next drone ID.
      int* nextDroneID;
      cudaMalloc(&nextDroneID, sizeof(int));
      cudaMemset(nextDroneID, 0, sizeof(int));
      nextDroneIDPtr = std::shared_ptr<int>(nextDroneID,
        [](int* ptr) { cudaFree(ptr); }
      );

      // Allocate memory for the GPU result buffer and fill it with zeroes.
      GPUResultType* gpuResultBuffer;
      cudaMalloc(&gpuResultBuffer, sizeof(GPUResultType));
      cudaMemset(gpuResultBuffer, 0, sizeof(GPUResultType));
      gpuResultBuffers.push_back(std::shared_ptr<GPUResultType>(gpuResultBuffer,
        [](GPUResultType* ptr) { cudaFree(ptr); }
      ));

      // Allocate memory for the GPU previous result buffer and fill it with zeroes.
      GPUResultType* gpuPreviousResultBuffer;
      cudaMalloc(&gpuPreviousResultBuffer, sizeof(GPUResultType));
      cudaMemset(gpuPreviousResultBuffer, 0, sizeof(GPUResultType));
      gpuPreviousResultBuffers.push_back(std::shared_ptr<GPUResultType>(gpuPreviousResultBuffer,
        [](GPUResultType* ptr) { cudaFree(ptr); }
      ));

      // Create a stream for the GPU to use.
      cudaStream_t* streamPtr = new cudaStream_t;
      cudaStreamCreate(streamPtr);
      streams.push_back(std::shared_ptr<cudaStream_t>(streamPtr,
        [](cudaStream_t* ptr) { cudaStreamDestroy(*ptr); delete ptr; }
      ));

      // Create a UDP receiver for the camera.
      std::shared_ptr<ReceiverUDP> receiverUDP = std::make_shared<ReceiverUDP>(ip_address);
      if (receiverUDP->GetConstructorStatus() != OKAY) {
        std::cerr << "Error constructing ReceiverUDP: " << ErrorMessage(receiverUDP->GetConstructorStatus()) << std::endl;
        return 14;
      }
      UDPReceivers.push_back(receiverUDP);
    }

    // Launch the threads, hooking them together using the queues.
    std::vector<std::thread> copyDataToGPUAndRunKernelThreads;
    for (size_t i = 0; i < dataQueues.size(); i++) {
      copyDataToGPUAndRunKernelThreads.push_back(std::thread(CopyDataToGPUAndRunKernel, cameras[0].width, cameras[0].height,
        std::ref(threshold), std::ref(neighborStride),
        std::ref(done), dataQueues[i], resultQueue, 16));
    }

    std::thread handleResultsThread(HandleResults, std::ref(done), resultQueue);

    std::vector<std::thread> receiveDataThreads;
    for (size_t i = 0; i < cameras.size(); i++) {
      receiveDataThreads.push_back(std::thread(ReceiveDataThread, std::ref(*UDPReceivers[i]), 9000,
        std::ref(done),
        cpuPinnedImageBuffers[i], gpuImageBuffers[i], gpuMedianBuffers[i], nextDroneIDPtr,
        gpuResultBuffers[i], gpuPreviousResultBuffers[i], streams[i],
        dataQueues[i % dataQueues.size()]));
    }

    // If we've been asked to replay a stream, then send a request to do this.
    if (replayStreamID) {
      if (g_verbosity >= 1) {
        std::cout << "Requesting replay of stream " << replayStreamID << std::endl;
      }
      // Set the initial time to be above zero so that we never predict backwards to negative time.
      status = client.SendCommandPacket(CommandPacketStartReplay(replayStreamID, replayStartTime));
      if (status != OKAY) {
        std::cerr << "Failed to start replay: " << ErrorMessage(status) << std::endl;
        return 34;
      }
    }

    // Request streaming on the cameras at their maximum rates.
    if (g_verbosity >= 1) {
      std::cout << "Analyzing every " << frameStride << " images from " << cameraIDs.size() << " cameras for " << durationSeconds << " seconds" << std::endl;
    }
    for (auto& camID : cameraIDs) {
      // We don't send triggers when we replay.
      if (replayStreamID == 0) {
        // Find the minimum period for the camera and which internal trigger ID it uses, then
        // configure the trigger to run at that rate.
        TriggerInfo ti;
        ti.ID = cameras[camID - 1].trigger;
        ti.mode = 1;
        ti.period = cameras[camID - 1].minTriggerPeriod;
        ti.offset = 0;
        ti.trackingFactor = 0.5;
        status = client.SendCommandPacket(CommandPacketConfigureTrigger(ti));
        if (status != OKAY) {
          std::cerr << "Failed to configure trigger: " << ErrorMessage(status) << std::endl;
          return 29;
        }
      }

      // Request the camera to stream full-frame images once every frameStride frames.
      uint16_t port;
      status = UDPReceivers[camID - 1]->GetPort(port);
      if (status != OKAY) {
        std::cerr << "Failed to get port: " << ErrorMessage(status) << std::endl;
        return 30;
      }
      StreamEndpoint endpoint(ip_address, port);
      SubregionDescription region;
      region.cameraID = camID;
      region.skipFrames = frameStride - 1;
      region.startTimeSeconds = 0;
      region.startTimeMicroseconds = 0;
      region.left = 0;
      region.top = 0;
      region.right = cameras[camID - 1].width - 1;
      region.bottom = cameras[camID - 1].height - 1;
      status = client.SendCommandPacket(CommandPacketStreamSubregion(endpoint, region));
      if (status != OKAY) {
        std::cerr << "Failed to stream images: " << ErrorMessage(status) << std::endl;
        return 32;
      }
    }

    // Make a spin-free queue to receive lines from stdin and launch (and detach) a thread to read them.
    // We detach the thread because it will block on std::getline and we don't want to have to wait for it to finish when we exit.
    SpinFreeQueue<std::string> stdinQueue;
    std::thread stdinThread(stdinReader, std::ref(stdinQueue));
    stdinThread.detach();

    // Do analysis on frames until we've run for the requested duration or someone has marked us to be done.
    if (g_verbosity >= 1) {
      std::cout << "Running analysis..." << std::endl;
#ifdef _WIN32
      std::cout << "  Enter 'q' to quit" << std::endl;
#else
      std::cout << "  Press ctrl-C to quit" << std::endl;
#endif
      std::cout << "  Enter 't#' to set a new threshold" << std::endl;
      std::cout << "  Enter 's#' to set a new neighbor stride" << std::endl;
      std::cout << "  Enter 'v#' to set a new verbosity level" << std::endl;
    }
    start = std::chrono::steady_clock::now();
    while (!done && (std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count() <= durationSeconds)) {
      // See if we get a line from stdin, and if so, we'll parse it to decide what to do.
      std::string line;
      if (stdinQueue.dequeue(line, std::chrono::milliseconds(100))) {
        if (line[0] == 'q') {
          if (g_verbosity >= 1) {
            std::cout << "Quitting..." << std::endl;
          }
          done = true;
        }
        else if (line[0] == 't') {
          // Read an integer from the rest of the line to set a new threshold.
          try {
            int newThreshold = std::stoi(line.substr(1));
            if (newThreshold > 0) {
              threshold = newThreshold;
              if (g_verbosity >= 1) {
                std::cout << "Setting new threshold: " << newThreshold << std::endl;
              }
            }
          } catch (const std::exception& e) {
            std::cerr << "Error parsing threshold: " << e.what() << std::endl;
          }
        }
        else if (line[0] == 's') {
          // Read an integer from the rest of the line to set a new neighbor stride.
          try {
            int newNeighborStride = std::stoi(line.substr(1));
            if (newNeighborStride > 0) {
              neighborStride = newNeighborStride;
              if (g_verbosity >= 1) {
                std::cout << "Setting new neighbor stride: " << newNeighborStride << std::endl;
              }
            }
          } catch (const std::exception& e) {
            std::cerr << "Error parsing neighbor stride: " << e.what() << std::endl;
          }
        }
        else if (line[0] == 'v') {
          // Read an integer from the rest of the line to set a new verbosity level.
          try {
            g_verbosity = std::stoi(line.substr(1));
            if (g_verbosity >= 0) {
              std::cout << "Setting new verbosity level: " << g_verbosity << std::endl;
            }
          } catch (const std::exception& e) {
            std::cerr << "Error parsing verbosity level: " << e.what() << std::endl;
          }
        }
      }
    }

    // Set done and wait for all of our singleton threads to join.
    done = true;
    handleResultsThread.join();

    // Clear all remaining data from the queues.
    // Do this in a block so that the shared_ptrs are destroyed before we join the receiveDataThreads.
    {
      std::shared_ptr<ResultType> result;
      while (resultQueue->dequeue(result, std::chrono::milliseconds(1))) {}
    }
    resultQueue.reset();
    for (auto& thread : copyDataToGPUAndRunKernelThreads) {
      thread.join();
    }

    // Clear all remaining data from the queues.
    // Do this in a block so that the shared_ptrs are destroyed before we join the receiveDataThreads.
    {
      std::shared_ptr<DataToSendToGPU> data;
      for (auto& dataQueue : dataQueues) {
        while (dataQueue->dequeue(data, std::chrono::milliseconds(1))) {}
      }
    }
    for (auto& dataQueue : dataQueues) {
      dataQueue.reset();
    }

    // Now that all of the buffers have been returned to the buffer queue, join our receive-data threads.
    for (auto& thread : receiveDataThreads) {
      thread.join();
    }
  }

  return 0;
}