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

#include <math.h>

namespace ugv {

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

static const char *TAG = "ugv_main";

constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
static const float PI             = atanf(1.0) * 4.0;

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);
  }

  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");
    int64_t time_us = esp_timer_get_time();
    float   time_s  = ((float)time_us) / 1e6;
    io->ReadInputs(inputs);
    outputs.left_motor  = sinf(time_s * PI);
    outputs.right_motor = cosf(time_s * PI);
    io->WriteOutputs(outputs);
  }
};

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(); }