#include <esp_log.h>
#include <string.h>
#include <u8g2.h>

#include "U8g2lib.hh"
#include "ugv_comms.hh"
#include "ugv_config.h"
#include "ugv_io.hh"

namespace ugv {

using ugv::comms::Comms;
using ugv::io::IO;

static const char *TAG = "ugv_main";

U8G2 *oled;

void setup_oled(void) {
  // For Heltec ESP32 LoRa
  // oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, 16, 15, 4);
  // For wemos Lolin ESP32
  oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, U8X8_PIN_NONE, 4, 5);
  oled->initDisplay();
  oled->clearDisplay();
  oled->setPowerSave(false);
}

void setup(void) {
  ESP_LOGI(TAG, "setup");

  setup_oled();
  Comms.Init();
  IO.Init();
}

#define BUF_SZ 32

void loop(void) {
  static int32_t    lora_rssi;
  static uint8_t    lora_lna_gain;
  static TickType_t last_packet_tick;
  static int32_t    last_packet_rssi;
  static int8_t     last_packet_snr;

  static char        buf[BUF_SZ];
  static io::Inputs  inputs;
  static io::Outputs outputs;

  IO.ReadInputs(inputs);
  outputs.left_motor  = outputs.left_motor > 0 ? -0.5 : 1.0;
  outputs.right_motor = outputs.right_motor > 0 ? -0.5 : 1.0;
  IO.WriteOutputs(outputs);
  // ESP_LOGI(TAG, "inputs %s", inputs.ToString());

  oled->firstPage();
  lora_rssi     = Comms.ReadRssi();
  lora_lna_gain = Comms.ReadLnaGain();

  Comms.Lock();
  last_packet_tick = Comms.last_packet_tick;
  last_packet_rssi = Comms.last_packet_rssi;
  last_packet_snr  = Comms.last_packet_snr;
  Comms.Unlock();
  do {
    oled->drawRFrame(0, 0, OLED_W, OLED_H, 4);

    multi_heap_info_t heap_info;
    heap_caps_get_info(&heap_info, MALLOC_CAP_DEFAULT);
    oled->setFont(u8g2_font_4x6_mr);
    oled->drawStr(4, 8, "=====UAS UGV=====");
    snprintf(buf, BUF_SZ, "heap allc/free %d/%d",
             heap_info.total_allocated_bytes, heap_info.total_free_bytes);
    oled->drawStr(4, 2 * 8, buf);

    snprintf(buf, BUF_SZ, "rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);
    oled->drawStr(4, 3 * 8, buf);

    if (last_packet_tick > 0) {
      double time_since_last_packet =
          1000.0f /
          ((xTaskGetTickCount() - last_packet_tick) * portTICK_RATE_MS);
      snprintf(buf, BUF_SZ, "last pkt rx %f s ago", time_since_last_packet);
      oled->drawStr(4, 4 * 8, buf);
      snprintf(buf, BUF_SZ, "pkt rssi: %d, snr: %f", last_packet_rssi,
               last_packet_snr * 0.25f);
      oled->drawStr(4, 5 * 8, buf);
    } else {
      oled->drawStr(4, 4 * 8, "no pkt rx");
    }
    snprintf(buf, BUF_SZ, "motors: (%f, %f)", outputs.left_motor,
             outputs.right_motor);
    oled->drawStr(4, 6 * 8, buf);
  } while (oled->nextPage());
  vTaskDelay(pdMS_TO_TICKS(1000));
}

void loopTask(void *pvUser) {
  setup();
  while (1) {
    loop();
  }
}

}  // namespace ugv

extern "C" void app_main() {
  xTaskCreatePinnedToCore(ugv::loopTask, "loopTask", 8192, NULL, 1, NULL, 1);
}