#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, OLED_RESET,
                                                 OLED_CLOCK, OLED_DATA);
  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;
  }
}

void DisplayClass::Run() {
#ifdef COMMS_SX127X
  int32_t lora_rssi;
  uint8_t lora_lna_gain;
  int32_t last_packet_rssi;
  int8_t last_packet_snr;
#endif
  TickType_t last_packet_tick;
  messages::UGV_Status status;

  ESP_LOGD(TAG, "Started display thread");
  while (true) {
    oled->firstPage();
#ifdef COMMS_SX127X
    lora_rssi = comms_->ReadRssi();
    lora_lna_gain = comms_->ReadLnaGain();
#endif

    comms_->Lock();
    last_packet_tick = comms_->last_packet_tick;
    status.CopyFrom(comms_->status);
#ifdef COMMS_SX127X
    last_packet_rssi = comms_->last_packet_rssi;
    last_packet_snr = comms_->last_packet_snr;
#endif
    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("state: %d", status.state());

      if (last_packet_tick > 0) {
        double time_since_last_packet =
            static_cast<float>((xTaskGetTickCount() - last_packet_tick) *
                               portTICK_RATE_MS) /
            1000.f;
        oled->setCursor(4, 4 * 8);
        oled->printf("last pkt rx %.2f s ago", time_since_last_packet);
#ifdef COMMS_SX127X
        oled->setCursor(4, 5 * 8);
        oled->printf("pkt rssi: %d, snr: %f", last_packet_rssi,
                     last_packet_snr * 0.25f);
#endif
      } else {
        oled->setCursor(4, 4 * 8);
        oled->print("no pkt rx");
      }
      oled->setCursor(4, 5 * 8);
      oled->printf("yaw angle: %f", status.yaw_angle());
    } while (oled->nextPage());
    vTaskDelay(pdMS_TO_TICKS(1000));
  }
}

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

}  // namespace ugv