#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;
  int64_t         last_print;

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

  void Init() {
    esp_timer_init();

    comms->Init();
    io->Init();
    // display->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);
    last_print = 0;
  }

  void OnTick() {
    ESP_LOGV(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);
    if (time_us >= last_print + 1 * 1000 * 1000) { // 1s
      ESP_LOGI(TAG,
               "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
               inputs.mpu.accel.x, inputs.mpu.accel.y, inputs.mpu.accel.z,
               inputs.mpu.gyro_rate.x, inputs.mpu.gyro_rate.y,
               inputs.mpu.gyro_rate.z, inputs.mpu.mag.x, inputs.mpu.mag.y,
               inputs.mpu.mag.z);
      last_print = time_us;
    }
  }
};

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