#include <dw/core/Logger.h>
#include <dw/sensors/SensorSerializer.h>
#include <dw/sensors/Sensors.h>
#include <dw/sensors/imu/IMU.h>

#include <chrono>
#include <csignal>
#include <cstdlib>
#include <iostream>

bool gRun = true;

void sig_int_handler(int)  // NOLINT(readability/casting)
{
  gRun = false;
}

void Check(dwStatus status)
{
  if (status != DW_SUCCESS)
  {
    std::cout << dwGetStatusName(status) << "\n";
    std::exit(1);
  }
}

int main()
{
  struct sigaction action = {};
  action.sa_handler = sig_int_handler;
  sigaction(SIGHUP, &action, NULL);   // controlling terminal closed, Ctrl-D
  sigaction(SIGINT, &action, NULL);   // Ctrl-C
  sigaction(SIGQUIT, &action, NULL);  // Ctrl-\, clean quit with core dump
  sigaction(SIGABRT, &action, NULL);  // abort() called.
  sigaction(SIGTERM, &action, NULL);  // kill command

  // platform
  Check(dwLogger_initialize([](dwContextHandle_t, dwLoggerVerbosity, const char* msg) { std::cout << msg; }));
  Check(dwLogger_setLogLevel(DW_LOG_VERBOSE));
  dwContextParameters sdkParams{};
  dwVersion sdkVersion{};
  Check(dwGetVersion(&sdkVersion));
  dwContextHandle_t context;
  Check(dwInitialize(&context, sdkVersion, &sdkParams));
  dwSALHandle_t sal;
  Check(dwSAL_initialize(&sal, context));

  // imu
  dwSensorParams imuParams{};
  imuParams.protocol = "imu.xsens";
  imuParams.parameters = "device=/dev/ttyUSB0,frequency=100";
  dwSensorHandle_t imu;
  Check(dwSAL_createSensor(&imu, imuParams, sal));
  Check(dwSensor_start(imu));

  // serializer
  dwSerializerParams serParams{};
  dwSensorSerializerHandle_t ser;
  serParams.parameters = "type=disk,file=/home/nvidia/xsens.bin";
  Check(dwSensorSerializer_initialize(&ser, &serParams, imu));
  Check(dwSensorSerializer_start(ser));

  // main loop
  while (gRun)
  {
    const uint8_t* buffer = nullptr;
    size_t bytesRead;
    dwStatus pollStatus = DW_NOT_READY;
    const auto startTime = std::chrono::high_resolution_clock::now();
    while (pollStatus == DW_NOT_READY)
    {
      const auto now = std::chrono::high_resolution_clock::now();
      if (std::chrono::duration_cast<std::chrono::microseconds>(now - startTime).count() > 100000)
      {
        printf("egregious timeout! exiting!\n");
        goto cleanup;
      }
      pollStatus = dwSensor_readRawData(&buffer, &bytesRead, 11000, imu);

      if (pollStatus == DW_SUCCESS)
      {
        pollStatus = dwSensorIMU_processRawData(buffer, bytesRead, imu);
        Check(dwSensorSerializer_serializeDataAsync(buffer, bytesRead, ser));
      }
      dwSensor_returnRawData(buffer, imu);
    }

    const auto now = std::chrono::high_resolution_clock::now();
    int pollElapsedUs = std::chrono::duration_cast<std::chrono::microseconds>(now - startTime).count();
    if (pollElapsedUs > 10000)
    {
      printf("poll took longer than expected: %d us (expected < 10000)\n", pollElapsedUs);
    }

    dwIMUFrame imuFrame;
    if (pollStatus == DW_SUCCESS)
    {
      // get usable data
      dwSensorIMU_popFrame(&imuFrame, imu);
      printf("accel [%.3f, %.3f, %.3f]\n", imuFrame.acceleration[0], imuFrame.acceleration[1],
             imuFrame.acceleration[2]);
    }
  }

  // clean up
cleanup:
  dwSensorSerializer_stop(ser);
  dwSensorSerializer_release(ser);
  dwSensor_stop(imu);
  dwSAL_releaseSensor(imu);
  dwSAL_release(sal);
  dwRelease(context);

  return 0;
}