#include <esp_log.h>
#include <esp_timer.h>
#include "ugv_comms.hh"
#include "ugv_display.hh"
#include "ugv_io.hh"

namespace ugv {

using ugv::comms::CommsClass;
using ugv::io::IOClass;

static const char *TAG = "ugv_main";
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 1;

extern "C" void OnTimeout(void *arg);

struct State {
 public:
  CommsClass *       comms;
  IOClass *          io;
  DisplayClass *     display;
  esp_timer_handle_t timer_handle;
  io::Inputs         inputs;
  io::Outputs        outputs;

  State() {
    comms   = new CommsClass();
    io      = new IOClass();
    display = new DisplayClass(comms, io);
  }

  void Init() {
    comms->Init();
    io->Init();
    display->Init();

    esp_timer_init();

    esp_timer_create_args_t timer_args;
    timer_args.callback = OnTimeout;
    timer_args.arg = this;
    timer_args.dispatch_method = ESP_TIMER_TASK;
    timer_args.name = "ugv_main_loop";
    esp_timer_create(&timer_args, &this->timer_handle);
    esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US);
  }

  void OnTick() {
    ESP_LOGD(TAG, "OnTick");
  }
};

extern "C" void OnTimeout(void *arg) {
  State *state = (State*)arg;
  state->OnTick();
}

State *state;

void Setup(void) {
  ESP_LOGI(TAG, "Starting UAS UGV");
  state = new State();
  state->Init();
  ESP_LOGI(TAG, "Setup finished");
}

}  // namespace ugv

extern "C" void app_main() { ugv::Setup(); }