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

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

namespace xsdk = xavia::sdk;

/* QUERY EXAMPLE
 *
 * The following example demonstrates the querying capabilities of the SDK.
 * It connects to a sensor, retrieves its serial number, current state,
 * and lists any errors that might be present.
 *
 * This example also shows how to handle the sensor state and error reporting.
 */

static std::wstring SensorStateToWString(const xsdk::SensorState& state)
{
  switch (state)
  {
    case xsdk::SensorState::UNKNOWN:
      return L"Unknown";
    case xsdk::SensorState::UNINITIALIZED:
      return L"Uninitialized";
    case xsdk::SensorState::IDLE:
      return L"Idle";
    case xsdk::SensorState::RUNNING:
      return L"Running";
    case xsdk::SensorState::SENSOR_ERROR:
      return L"Error";
    default:
      return L"Unknown";
  }
}

static std::wstring ErrorsToString(const std::vector<xsdk::SensorError>& errors)
{
  std::wstringstream ss;
  if (errors.empty())
  {
    ss << L"No Errors Reported";
  }
  else
  {
    ss << L"Sensor reported " << errors.size() << " errors:" << std::endl;
    for (const auto& error : errors)
    {
      ss << L"Error: " << static_cast<int>(error) << std::endl;
    }
  }
  return ss.str();
}

int main()
{
  std::wcout << L"=== Starting Query 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;
    auto sensor = factory.Build();

    // Retrieve the sensor's serial number
    std::wstring serialNumber = sensor->GetSerialNumber().ToWString();
    std::wcout << L"Sensor Serial Number: " << serialNumber << std::endl;

    // Get current sensor state
    xsdk::SensorState state = sensor->GetCurrentState();
    std::wcout << L"Current Sensor State: " << SensorStateToWString(state) << std::endl;

    // Check for any errors reported by the sensor
    auto errors = sensor->GetErrors();
    std::wcout << ErrorsToString(errors) << std::endl;

    if(sensor->GetCurrentState()!= xsdk::SensorState::RUNNING)
    {
      sensor->Start();
    }

    // Monitor state during startup
    state = sensor->GetCurrentState();
    std::wcout << L"Sensor State After Start: " << SensorStateToWString(state) << std::endl;

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

    // Check state after startup
    state = sensor->GetCurrentState();
    std::wcout << L"Sensor State After Start: " << SensorStateToWString(state) << std::endl;

    sensor->Stop();

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

    // Check state after stopping
    state = sensor->GetCurrentState();
    std::wcout << L"Sensor State After Stop: " << SensorStateToWString(state) << std::endl;

  }
  catch(const std::exception& e)
  {
    std::wcerr << L"Error received: " << e.what() << std::endl;
    return EXIT_FAILURE;
  }

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