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

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

namespace xsdk = xavia::sdk;

/*  SERIALIZE/DESERIALIZE EXAMPLE
 * 
 * The SDK supports dumping the point cloud data to disk in the proprietary XPC
 * format of XenomatiX. This can be easily achieved by asking the serialized version
 * of a point cloud. Similarly, the point cloud object has a deserialize function
 * to reinterpret that same data.
 * 
 * Both serialization and deserialization functions do not directly access the disk
 * but instead work on byte buffers. This allows clients to choose where to store
 * the buffer. Storing it to disk, a database or streaming it further on another
 * serial port, ... are all possible.
 * 
 * To demonstrate this, this example is split into 2 parts:
 *  - The first part runs the sensor for 5 seconds, storing all the point clouds into a buffer.
 *  - The second part reads the buffer and parses all the point clouds again, printing some information.
 */

void PointCloudWriter(xsdk::IPointCloud& pc, std::vector<std::vector<std::uint8_t>>& buffer)
{
  // Serialize() returns a const reference std::vector<std::uint8_t> with all the bytes.
  // do not change any of these bytes if you intend to use Deserialize() to read them again.
  buffer.push_back(pc.Serialize());
  // you can still access the information on pc to retrieve e.g. the timestamp, frame counter,
  // or line counter in order to e.g. create a file name.
}

void PART1_SERIALIZE(std::vector<std::vector<std::uint8_t>>& buffer)
{
  xsdk::SensorFactory factory;

  // register the point cloud handling function
  factory.SetPointCloudCallback([&buffer](std::unique_ptr<xsdk::IPointCloud> pc) { PointCloudWriter(*pc, buffer); });

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

void PART2_DESERIALIZE(std::vector<std::vector<std::uint8_t>>& buffer)
{
  // here the same pc memory is reused every time, but it is also possible to
  // create a new point cloud object every time.
  auto pc = xsdk::CreatePointCloud();
  std::size_t currentCounter = 0;
  for(auto& pcData : buffer)
  {
    pc->Deserialize(pcData);
    if(currentCounter != pc->GetMetaData().frameNumber)
    {
      std::cout << std::endl
                << "Deserialized point cloud for frame counter: " << pc->GetMetaData().frameNumber
                << " and line number: " << std::to_string(pc->GetMetaData().lineNumber);
      currentCounter = pc->GetMetaData().frameNumber;
    }
    else
    {
      std::cout << ", " << std::to_string(pc->GetMetaData().lineNumber);
    }
  }
  std::cout << std::endl;
}

int main()
{
  std::cout << "=== Starting Ser/Deser Example ===" << std::endl;

  try
  {
    // buffer in which the data will be stored
    std::vector<std::vector<std::uint8_t>> buffer;

    std::cout << "PART 1: capturing point clouds from Xavia and store the serialized version to the buffer."
              << std::endl;
    PART1_SERIALIZE(buffer);

    std::cout << "Captured " << buffer.size() << " point cloud lines in the buffer." << std::endl;
    // It would be possible to now store the information

    std::cout << "PART 2: Deserializing the point clouds in the buffer.";
    PART2_DESERIALIZE(buffer);

    std::cout << "Deserialized all " << buffer.size() << " point cloud lines in the buffer." << std::endl;
  }
  catch(const std::exception& e)
  {
    std::cerr << "Error received: " << e.what() << std::endl;
    return EXIT_FAILURE;
  }

  std::cout << "=== Ser/Deser Example Completed ===" << std::endl;
  return EXIT_SUCCESS;
}
