Percentile_CXX.cpp

/*
 * Copyright (C) 2025: 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 C++ 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.

/**
 * @file Percentile_CXX.cpp
 * @brief Apache Strap-Down Pilotage example analysis module using C++.
 *
* @author ReliaSolve.
* @date August 25, 2025.
*/

#include <iostream>
#include <chrono>
#include <algorithm>
#include <vector>
#include <ASDP_Core_API.h>
#include <ASDP_StreamPacketSortedQueue.h>

using namespace asdp;

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;
}

void usage(std::string progName)
{
  std::cerr << "Usage: " << progName << " [options] <ip_address> <percentile>" << std::endl;
  std::cerr << "  <ip_address> is the IP address to listen on for servers." << std::endl;
  std::cerr << "  <percentile> is the floating-point percentile value to compute (0-100)." << std::endl;
  std::cerr << "  Options:" << std::endl;
  std::cerr << "    --canera <camera_id>  Specify the camera ID to analyze (default is 1)." << std::endl;
  std::cerr << "    --frame-stride <n>    Read one out of every n frames (default is 1)." << std::endl;
  std::cerr << "    --duration <seconds>  Run for this many seconds (default is 10)." << std::endl;
  std::cerr << "  The program will connect to the first server it finds and analyze images from one of its cameras." << 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.
  float durationSeconds = 10;   ///< Run for this many seconds
  std::string ip_address;
  double percentile = 50.0;    ///< Percentile to compute (0-100)
  size_t realParams = 0;
  uint32_t camID = 1;

  // Parse the command line arguments, with the first non-flag argument being the
  // name of the IP address to listen on.  There is a --serial flag to specify
  // the serial number of the server, which defaults to 1.
  for (int i = 1; i < argc; ++i) {
    if (argv[i] == std::string("--help") || argv[i] == std::string("-h")) {
      usage(argv[0]);
      return 0;
    } else if (argv[i] == std::string("--frame-stride")) {
      if (++i >= argc) {
        std::cerr << "Missing value for --frame-stride" << std::endl;
        usage(argv[0]);
        return 1;
      }
      frameStride = static_cast<uint32_t>(std::stoi(argv[i]));
    } else if (argv[i] == std::string("--duration")) {
      if (++i >= argc) {
        std::cerr << "Missing value for --duration" << std::endl;
        usage(argv[0]);
        return 1;
      }
      durationSeconds = static_cast<float>(std::stof(argv[i]));
    } else if (argv[i] == std::string("--camera")) {
      if (++i >= argc) {
        std::cerr << "Missing value for --camera" << std::endl;
        usage(argv[0]);
        return 1;
      }
      camID = static_cast<uint32_t>(std::stoi(argv[i]));
      if (camID < 1) {
        std::cerr << "Camera ID must be at least 1." << std::endl;
        usage(argv[0]);
        return 1;
      }
    } else if (argv[i][0] == '-' ) {
      std::cerr << "Unknown flag: " << argv[i] << std::endl;
      usage(argv[0]);
      return 1;
    } else switch (realParams++) {
      case 0:
        ip_address = argv[i];
        break;
      case 1:
        percentile = std::stod(argv[i]);
        if ((percentile < 0.0) || (percentile > 100.0)) {
          std::cerr << "Percentile must be between 0 and 100." << std::endl;
          usage(argv[0]);
          return 1;
        }
        break;
      default:
        std::cerr << "Unexpected parameter: " << argv[i] << std::endl;
        usage(argv[0]);
        return 2;
    }
  }
  if (realParams != 2) {
    usage(argv[0]);
    return 2;
  }

  // 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;
  }
  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 (!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;
  }
  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.
  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;
  }
  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);
  std::cout << "Found " << cameras.size() << " cameras" << std::endl;

  // If we have a valid camera ID, analyze it.
  if (camID > cameras.size()) {
    std::cerr << "Camera ID " << camID << " is invalid; this server has " << cameras.size() << " cameras." << std::endl;
    return 20;
  }
  {
    // 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;
    }

    // Construct a UDP receiver for a stream from the camera.
    ReceiverUDP receiverUDP;
    if (receiverUDP.GetConstructorStatus() != OKAY) {
      std::cerr << "Error constructing ReceiverUDP: " << ErrorMessage(receiverUDP.GetConstructorStatus()) << std::endl;
      return 30;
    }
    uint16_t port;
    asdp::Status status = receiverUDP.GetPort(port);
    if (status != asdp::OKAY) {
      std::cerr << "Error getting port from ReceiverUDP: " << ErrorMessage(status) << std::endl;
      return 31;
    }

    std::cout << "Analyzing every " << frameStride << " images from camera " << camID << " on port " << port
      << " for " << durationSeconds << " seconds" << std::endl;

    // Request the camera to stream full-frame images once every frameStride frames.
    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;
    }

    // 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);

    // Do analysis on frames until we've run for the specified duration.
    // We initialize the data every FRAME_BEGIN, accumulate over all FRAME_DATA, and complete at FRAME_END
    start = std::chrono::steady_clock::now();
    size_t numFrames = 0;
    std::vector<double> allPixels;
    do {
      // Get the next packet.
      std::shared_ptr<asdp::StreamPacket> packet;
      size_t offset = 0;
      status = receiverUDP.ReceiveStreamPacket(10.0, packet, offset);
      if (status != asdp::OKAY) {
        std::cerr << "Error receiving StreamPacket: " << ErrorMessage(status) << std::endl;
        return 33;
      }
      // 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> receiveStreamPacket = readyPackets.front();
        readyPackets.pop_front();

        // Get and handle all messages from the packet.
        std::shared_ptr<asdp::Message> message;
        status = receiveStreamPacket->GetNextMessage(message);
        if (status != asdp::OKAY) {
          std::cerr << "Error getting message from packet: " << ErrorMessage(status) << std::endl;
          return 35;
        }
        while (message != nullptr) {
          asdp::MessageID rID;
          status = message->GetType(rID);
          if (status != asdp::OKAY) {
            std::cerr << "Error getting type from message: " << ErrorMessage(status) << std::endl;
            return 36;
          }
          if (rID == CONSOLIDATED_FRAME_DATA) {
            MessageConsolidatedFrameData frameData(*message);
            if (frameData.GetConstructorStatus() != asdp::OKAY) {
              std::cerr << "Error constructing FrameData message: " << ErrorMessage(frameData.GetConstructorStatus()) << std::endl;
              return 37;
            }

            bool isBeginFrame;
            status = frameData.GetBeginFrameFlag(isBeginFrame);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting begin frame flag from FrameData message: " << ErrorMessage(status) << std::endl;
              return 39;
            }
            if (isBeginFrame) {
              allPixels.clear();
            }

            // Find out how many pixels are in the frame and sum their values.
            if (frameData.GetConstructorStatus() != asdp::OKAY) {
              std::cerr << "Error constructing FrameData message: " << ErrorMessage(frameData.GetConstructorStatus()) << std::endl;
              return 37;
            }
            uint16_t left, right, top, bottom;
            status = frameData.GetLeft(left);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting left from FrameData message: " << ErrorMessage(status) << std::endl;
              return 38;
            }
            status = frameData.GetRight(right);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting right from FrameData message: " << ErrorMessage(status) << std::endl;
              return 39;
            }
            status = frameData.GetTop(top);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting top from FrameData message: " << ErrorMessage(status) << std::endl;
              return 40;
            }
            status = frameData.GetBottom(bottom);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting bottom from FrameData message: " << ErrorMessage(status) << std::endl;
              return 41;
            }
            uint8_t* rawData;
            status = frameData.GetDataPointer(rawData);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting data pointer from FrameData message: " << ErrorMessage(status) << std::endl;
              return 42;
            }
            uint16_t* data = reinterpret_cast<uint16_t*>(rawData);
            uint16_t stride = (right - left + 1);
            for (uint16_t y = 0; y <= bottom - top; ++y) {
              for (uint16_t x = 0; x <= right - left; ++x) {
                allPixels.push_back(data[y * stride + x]);
              }
            }

            bool isEndFrame;
            status = frameData.GetEndFrameFlag(isEndFrame);
            if (status != asdp::OKAY) {
              std::cerr << "Error getting end frame flag from FrameData message: " << ErrorMessage(status) << std::endl;
              return 42;
            }
            if (isEndFrame) {
              // Sort the pixels and find the specified percentile value.
              std::sort(allPixels.begin(), allPixels.end());
              size_t index = static_cast<size_t>((percentile / 100.0) * (allPixels.size() - 1) + 0.5);
              std::cout << "  Frame " << ++numFrames << " has " << percentile << " percentile value " << allPixels[index] << std::endl;
            }
          }

          status = receiveStreamPacket->GetNextMessage(message);
          if (status != asdp::OKAY) {
            std::cerr << "Error getting first message from packet: " << ErrorMessage(status) << std::endl;
            return 43;
          }
        }
      }

    } while (std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count() <= durationSeconds);
    std::cout << "Analyzed " << numFrames << " images." << std::endl;

    // Turn off streaming.
    status = client.SendCommandPacket(CommandPacketCancelSubregion(camID, endpoint));
    if (status != OKAY) {
      std::cerr << "Failed to cancel stream images: " << ErrorMessage(status) << std::endl;
      return 44;
    }
  }

  std::cout << std::endl << "Success!" << std::endl;
  return 0;
}