#include "ugv_display.hh"

#include <esp_log.h>
#include "U8g2lib.hh"
#include "ugv_comms.hh"
#include "ugv_config.h"

namespace ugv {

static const char* TAG = "ugv_display";

using comms::CommsClass;

DisplayClass::DisplayClass(CommsClass* comms) : comms_(comms) {}

void DisplayClass::Init() {
  ESP_LOGD(TAG, "Initializing u8g2 display");
  // 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);

  BaseType_t xRet = xTaskCreate(DisplayClass::RunThread, "ugv_display",
                                8 * 1024, this, 1, &this->task_handle_);
  if (xRet != pdTRUE) {
    ESP_LOGE(TAG, "error starting display thread");
    return;
  }
  ESP_LOGD(TAG, "Started display thread");
}

void DisplayClass::Run() {
  int32_t    lora_rssi;
  uint8_t    lora_lna_gain;
  TickType_t last_packet_tick;
  int32_t    last_packet_rssi;
  int8_t     last_packet_snr;

  while (true) {
    // 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->setCursor(4, 8);
      oled->print("=====UAS UGV=====");
      oled->setCursor(4, 2 * 8);
      oled->printf("heap allc/free %d/%d", heap_info.total_allocated_bytes,
                   heap_info.total_free_bytes);

      oled->setCursor(4, 3 * 8);
      oled->printf("rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);

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

void DisplayClass::RunThread(void* arg) {
  auto display = (DisplayClass*)arg;
  display->Run();
  vTaskDelete(NULL);
}

}  // namespace ugv