// Arduino Core
#include <Arduino.h>

// nBus Core
#include "nbus_app.h"
#include "memory_dummy.h"

// nBus ESP32 Platform Driver
#include "NbusSlaveDriver/nbus_app_interface_esp32.h"
#include "NbusLoopDriver/NbusLoopEsp32.h"

// nBus Application Driver
#include "nbus_app_driver/nbus_app_driver.h"

// Application Config
#include "master_config.h"

// nbus loop driver
NbusLoopEsp32 loop_driver(Dev.EnSerial, APP_LOOP_TIMEOUT_MS);

void setup()
{
  
  // initialize HW
  if (Dev.hardware_init() != ESP_OK)
  {
    while (true)
    {
      Serial.println("Error: Device is not initialized!");
      delay(APP_ERR_DELAY_MS);
    }
  }

  // initialize nBus stack
  nbus_init(getAppdrvDriver(), nbus_platform_init(&loop_driver));
  nbus_init_memory_driver(getDummyMemDriver());
  nbus_init_app(NULL, NULL);

  // initialize measuring tasks
  xTaskCreate(read_tof_task, TASK_TOF_NAME, TASK_TOF_STACK_DEPTH, NULL, TASK_TOF_PRIORITY, NULL);
  xTaskCreate(read_imu_task, TASK_IMU_NAME, TASK_IMU_STACK_DEPTH, NULL, TASK_IMU_PRIORITY, NULL);
  xTaskCreate(read_env_task, TASK_ENV_NAME, TASK_ENV_STACK_DEPTH, NULL, TASK_ENV_PRIORITY, NULL);
}

void loop()
{
  // run nBus
  nbus_stack();
  nbus_platform_loop();
}