#include "PointCloudPrinter.h"

#include <xavia/iPointCloud.h>

#include <iostream>

namespace xsdk = xavia::sdk;

/*
 * This function is called when a new point cloud line is received.
 * It will log which lines numbers are received.
 * Only when a new frame arrives will it print information to the screen.
 * (this is because printing at +1000Hz is too slow)
 */
void PointCloudPrinter::OnNewPointCloud(const xsdk::IPointCloud& pc)
{
  auto metadata = pc.GetMetaData();
  if(metadata.frameNumber == currentFrameNumber)
  {
    receivedLineNumbers.push_back(metadata.lineNumber);
  }
  else
  {
    PrintInfo();
    currentFrameNumber = metadata.frameNumber;
    receivedLineNumbers.clear();
    receivedLineNumbers.push_back(metadata.lineNumber);
  }
}

void PointCloudPrinter::PrintInfo()
{
  std::wcout << "Received point cloud frame number: " << currentFrameNumber;
  std::wcout << " with line numbers: ";
  for(auto lineNumber : receivedLineNumbers)
  { 
    std::wcout << lineNumber << ", "; 
  }
  std::wcout << std::endl;
}
