#include "PointCloudPrinter.h"

#include <xavia/error.h>
#include <xavia/iPointCloud.h>
#include <xavia/iSensor.h>
#include <xavia/logs.h>
#include <xavia/sensorFactory.h>
#include <xavia/sensorState.h>

#include <cstdlib>
#include <chrono>
#include <iostream>
#include <string>
#include <thread>

namespace xsdk = xavia::sdk;

/*  POINT CLOUD HANDLING EXAMPLE
 *
 * This examples how to connect a point cloud handling function to the sensor object
 * Your handling function will be called by the SDK for each point cloud line it
 * receives while streaming is active.
 * 
 * The callback will receive a unique pointer to an object of interface 
 * `IPointCloud`. This point cloud object is now under your control and you can
 * move it, copy it or ignore it.
 * 
 * In this example a lambda function is used to wrap the scope and convert the
 * smart pointer into a reference for the internal method call, but that is optional.
 * 
 * The callback should be quick. it will be called on average 1120 times per second.
 * The SDK has a limited internal buffer to cope with timing variations, but if the 
 * callback takes more than 0.9 ms on average, lines will be dropped by the SDK.
 * 
 * Within the scope of the callback, the point cloud object is available. Several
 * accessors can be called to: get the xyz data, get the metadata, or serialize it.
 */

int main()
{
  std::wcout << L"=== Starting Point Cloud Handling Example ===" << std::endl;

  // capture log output and send to std::cout.
  xsdk::SetLogCallBack([](const xsdk::Severity& sev, const std::wstring& msg)
                       { std::wcout << xsdk::GetLogTypeString(sev) << L": " << msg << std::endl; });
  xsdk::SetLogSeverity(xsdk::Severity::XM_INFO);

  try
  {
    xsdk::SensorFactory factory;

    // configuration of callbacks
    PointCloudPrinter printer;
    auto pcCallbackWrapper  = [&](std::unique_ptr<xsdk::IPointCloud> pc) { printer.OnNewPointCloud(*pc); };
    factory.SetPointCloudCallback(pcCallbackWrapper);
    
    // build sensor
    auto sensor = factory.Build();

    // run
    if(sensor->GetCurrentState()!= xsdk::SensorState::RUNNING)
    {
      sensor->Start();
    }
    std::this_thread::sleep_for(std::chrono::seconds(5));
    sensor->Stop();
  }
  catch(const std::exception& e)
  {
    std::cerr << "Error received: " << e.what() << std::endl;
    return EXIT_FAILURE;
  }

  std::wcout << L"=== Point Cloud Handling Example Completed ===" << std::endl;
  return EXIT_SUCCESS;
}
