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

#include <iostream>
#include <fstream>
#include <chrono>
#include <thread>
#include <atomic>

namespace xsdk = xavia::sdk;

std::atomic<int> pointcloudLineCount{0};
std::atomic<int> pointcloudFrameCount{0};
std::atomic<int> imageCount{0};

/* LIDAR AND CAMERA RECORDING EXAMPLE
 * 
 * This example demonstrates how to handle combine lidar and camera data.
 * The following steps are performed:
 * 1. Set up callbacks for point cloud and camera data.
 * 2. Build the sensor and get the camera interface.
 * 3. Start the sensor and camera.
 * 4. Process incoming point cloud lines and camera images in the callbacks.
 * 5. Stop the sensor and camera after some time.
 */

void pointCloudCallback(std::unique_ptr<xsdk::IPointCloud> pointCloud)
{
  // step 4a. Process incoming point cloud line
  ++pointcloudLineCount;
  auto md = pointCloud->GetMetaData();
  if(md.lineNumber == 0)
  {
    ++pointcloudFrameCount;
    std::wcout << L"PointCloud # " << pointcloudFrameCount << L": " << md.frameNumber <<std::endl;
  }
}

void imageCallback(std::unique_ptr<xsdk::IImage> image)
{
  // step 4b. Process incoming camera image
  ++imageCount;
  const auto& metadata = image->GetMetaData();
  const auto bytesPerPixel = image->GetPixelData().size() / (metadata.width * metadata.height);
  
  std::wcout << L"Image # " << imageCount << L": " << metadata.frameNumber << std::endl;
}

void errorCallback(const xsdk::Exception& ex)
{
  std::wcerr << L"Error: " << ex.what() << std::endl;
}

int main(int argc, char** argv)
{
  try
  {
    std::wcout << L"=== Lidar and Camera Example ===" << std::endl;

    // logging is not mandatory, but we need to convert to std::wcout.
    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);
    
    xsdk::SensorFactory factory;
    factory.SetErrorCallback(errorCallback); // optional
    
    // step 1. Set up callbacks
    factory.SetPointCloudCallback(pointCloudCallback);
    factory.SetImageCallback(imageCallback);
    
    // step 2. Build the sensor and get the camera
    auto sensor = factory.Build();
    auto camera = sensor->GetCamera();
    
    // always check if the camera is available before trying to use it.
    // The camera may not be available due to various reasons.
    if (!camera)
    {
      std::wcerr << L"Camera not available" << std::endl;
      return EXIT_FAILURE;
    }
    std::wcout << L"Connected to Camera ID: " << camera->GetCameraIdW() << std::endl;

    // step 3. Start the sensors
    if(sensor->GetCurrentState()!= xsdk::SensorState::RUNNING)
    {
      sensor->Start();
      camera->Start();
    }

    std::this_thread::sleep_for(std::chrono::seconds(5));

    // step 5. Stop the sensors
    sensor->Stop();
    camera->Stop();

    std::wcout << L"Total point cloud lines received: " << pointcloudLineCount << std::endl;
    std::wcout << L"Total point cloud frames received: " << pointcloudFrameCount << std::endl;
    std::wcout << L"Total images received: " << imageCount << std::endl;
  }
  catch (const std::exception& ex)
  {
    std::wcerr << L"Exception: " << ex.what() << std::endl;
    return EXIT_FAILURE;
  }

  std::wcout << L"=== Lidar and Camera Example Completed ===" << std::endl;
  return EXIT_SUCCESS;
}
