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

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

namespace xsdk = xavia::sdk;

/*  ERROR HANDLING EXAMPLE
 * 
 * This example demonstrates how to handle errors in the SDK. The basic principle
 * is similar to point clouds and events: create a callback and attach it to the
 * factory. The callback will be called on errors.
 * 
 * There are 2 kind of errors in the SDK:
 * - When you call a function (e.g. Start()), it can fail to execute and generate
 *   an error. Those errors are thrown as exceptions and should be handled with
 *   try catch structures.
 * - Errors coming from the sensor can happen at any time, and are not necessarily
 *   triggered by a user action. These are asynchronously presented to the call-
 *   back (if any).
 * 
 * Note: You do not need to react to all async errors. In some cases, the sensor
 * can communicate errors that certain actions failed, which does not need to be
 * a problem. For example: The error 'cannot start because the sensor is already
 * running' can arrive due to another user or system trying to start the sensor.
 */

void HandleErrors(const xsdk::Exception& error)
{
  /* xsdk::Exception is derived from std::exception and thus implements 'what()'.
   * For Localization purposes use the 'GetType()' function which will return an
   * enum of xavia::sdk::SensorError.
   */
  switch(error.GetType())
  {
    case xsdk::SensorError::INVALID_FRAME:
      std::wcerr << L"Failed to retrieve point cloud measurement from hardware." << std::endl;
      break;
    case xsdk::SensorError::DISK_FULL:
      std::wcerr << L"Warning Xavia local disk is full." << std::endl;
      break;
    case xsdk::SensorError::UNKNOWN_ERROR:
      std::wcerr << L"Xavia encountered an unspecified error." << std::endl;
      break;
    default:
      std::wcerr << L"Received error that we are not handling: " << error.what() << std::endl;
  }
}

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

  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;

    // attach error callback for asynchronous errors
    factory.SetErrorCallback(HandleErrors);

    auto sensor = factory.Build();

    try // try catch for synchronous errors
    {
      std::wcout << L"Checking sensor state and starting if necessary." << std::endl;
      if(sensor->GetCurrentState() != xsdk::SensorState::RUNNING)
      {
        sensor->Start();
        std::this_thread::sleep_for(std::chrono::seconds(3));
      }
      std::wcout << L"Trying to start the sensor again to trigger an error." << std::endl;
      sensor->Start(); // ERRROR: cannot start twice ==> generates UNABLE_TO_START error
      std::wcout << L"This line will not be reached due to the error." << std::endl;
    }
    catch(const xsdk::Exception& e) // std::exception would also work, but you won't be able to call 'GetType()'.
    {
      switch(e.GetType())
      {
        case xsdk::SensorError::UNABLE_TO_START:
          std::wcerr << L"Failed to send start command to sensor or received failure response: " << e.what() << std::endl;
          break;
        default:
          throw; // unexpected error. Let main error catch handle it.
      }
    }
    sensor->Stop();
  }
  catch(const std::exception& e)
  {
    std::wcerr << L"Error received: " << e.what() << std::endl;
    return EXIT_FAILURE;
  }

  std::wcout << L"=== Error Example Completed ===" << std::endl;
  return EXIT_SUCCESS;
}
