#include <Arduino.h>
#include <U8g2lib.h>
#include <SPI.h>
#include <LoRa.h>
#include <FreeRTOS.h>
#include <esp_log.h>

const char *TAG = "uas-ugv";

class U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, 16, 15, 4);

class SPIClass lora_spi(VSPI);

#define LORA_SCK 5
#define LORA_MISO 19
#define LORA_MOSI 27
#define LORA_CS 18
#define LORA_RST 14
#define LORA_IRQ 26

#define LORA_FREQ (433E6)
#define LORA_BUF_LEN 64

#define OLED_H 64
#define OLED_W 128

struct Packet
{
  int rssi;
  double snr;
  size_t buffer_len;
  char buffer[LORA_BUF_LEN];
};

uint16_t packet_num;

void loraTask(void *params);
void loraOnReceive(int packetSize);

xTaskHandle lora_task_hndl;
xQueueHandle lora_packet_recv_queue; // packets recieved (type Packet)
xQueueHandle lora_packet_isr_queue;  // packet lengths from the recieve isr (type int)

Packet packet;

void setup(void)
{
  Serial.begin(115200);
  while (!Serial)
  {
    delay(10);
  }

  ESP_LOGI(TAG, "setup");
  Serial.println("setup");

  lora_packet_recv_queue = xQueueCreate(4, sizeof(Packet));
  lora_packet_isr_queue = xQueueCreate(4, sizeof(int));
  configASSERT(lora_packet_recv_queue != 0);
  configASSERT(lora_packet_isr_queue != 0);

  lora_spi.begin(LORA_SCK, LORA_MISO, LORA_MOSI, LORA_CS);
  LoRa.setSPI(lora_spi);
  LoRa.setPins(LORA_CS, LORA_RST, LORA_IRQ);
  int res = LoRa.begin(LORA_FREQ); // 433MHz
  if (!res)
  {
    Serial.println("LoRa init failed");
  }
  LoRa.setTxPower(17);
  LoRa.setSpreadingFactor(11);
  LoRa.setSignalBandwidth(125E3);
  LoRa.setSyncWord(0x34);
  LoRa.enableCrc();
  LoRa.onReceive(loraOnReceive);
  LoRa.receive(0);

  // LoRa.dumpRegisters(Serial);

  u8g2.beginSimple(); /* Do not clear the display, wake is not required */
  u8g2.clearDisplay();
  u8g2.setPowerSave(false);

  packet_num = 0;

  xTaskCreate(loraTask, "loraTask", 1024 * 10, NULL, 2, &lora_task_hndl);
  memset(&packet, 0, sizeof(Packet));
}

#define XO 10

void drawLogo(void)
{

  u8g2.setFontMode(1); // Transparent
  u8g2.setFontDirection(0);
  u8g2.setFont(u8g2_font_inb24_mf);
  u8g2.drawStr(0 + XO, 30, "U");

  u8g2.setFontDirection(1);
  u8g2.setFont(u8g2_font_inb30_mn);
  u8g2.drawStr(21 + XO, 8, "8");

  u8g2.setFontDirection(0);
  u8g2.setFont(u8g2_font_inb24_mf);
  u8g2.drawStr(51 + XO, 30, "g");
  u8g2.drawStr(67 + XO, 30, "\xb2");

  u8g2.drawHLine(2 + XO, 35, 47);
  u8g2.drawHLine(3 + XO, 36, 47);
  u8g2.drawVLine(45 + XO, 32, 12);
  u8g2.drawVLine(46 + XO, 33, 12);
}

void drawURL(void)
{
  u8g2.setFont(u8g2_font_6x12_tr);
  u8g2.drawStr(1 + XO, 54, "github.com/olikraus/u8g2");
}

void loraOnReceive(int packetSize)
{
  if (packetSize == 0)
    return;
  ESP_LOGV(TAG, "loraOnReceive");
  xQueueSendFromISR(lora_packet_isr_queue, &packetSize, NULL);
}

void loraTask(void *params)
{
  char outBuf[20];
  const size_t outBufLen = (sizeof(outBuf) / sizeof(uint8_t));
  int packet_len;
  TickType_t send_period = pdMS_TO_TICKS(2000);
  TickType_t current_time = xTaskGetTickCount();
  TickType_t next_send = current_time + send_period;
  Packet recvd_packet;
  while (true)
  {
    TickType_t delay_ticks = next_send - current_time;
    BaseType_t didReceive = xQueueReceive(lora_packet_isr_queue, &packet_len, delay_ticks);
    if (didReceive)
    {
      int packetSize = (packet_len > LORA_BUF_LEN - 1) ? (LORA_BUF_LEN - 1) : (packet_len);
      LoRa.setTimeout(50);
      LoRa.readBytes(recvd_packet.buffer, packetSize);
      recvd_packet.buffer_len = packetSize;
      recvd_packet.buffer[packetSize - 1] = '\0';
      recvd_packet.rssi = LoRa.packetRssi();
      recvd_packet.snr = LoRa.packetSnr();

      xQueueSend(lora_packet_recv_queue, &recvd_packet, 10);
    }
    current_time = xTaskGetTickCount();
    if (current_time >= next_send) {
      sprintf(outBuf, "hello world %d", packet_num);
      packet_num++;
      LoRa.beginPacket();
      size_t written = LoRa.write((uint8_t *)outBuf, outBufLen - 1);
      LoRa.endPacket();
      Serial.printf("lora wrote %d bytes\n", written);
      LoRa.receive(0); // go back to receive mode
      
      current_time = xTaskGetTickCount();
      next_send = current_time + send_period;
    }
  }
}

void loop(void)
{
  Serial.println("loop");
  u8g2.firstPage();
  bool recieved_packet = xQueueReceive(lora_packet_recv_queue, &packet, 10);
  do
  {
    // drawLogo();
    // drawURL();

    u8g2.drawRFrame(0, 0, OLED_W, OLED_H, 4);

    uint32_t free_heap = xPortGetFreeHeapSize();
    u8g2.setFont(u8g2_font_4x6_mf);
    u8g2.drawStr(4, 8, "Hello World!");
    u8g2.setCursor(4, 8 + 8);
    u8g2.printf("free heap: %d", free_heap);

    if (packet.buffer_len)
    {
      Serial.printf("lora received packet (len %d, rssi: %d, snr: %f): %s\n",
                    packet.buffer_len, packet.rssi, packet.snr, packet.buffer);
      u8g2.setFont(u8g2_font_4x6_mf);
      u8g2.setCursor(4, 8 + 8 + 8);
      u8g2.printf("lora pkt(rssi: %d, snr: %f)", packet.rssi, packet.snr);
      u8g2.setCursor(4, 8 + 8 + 8 + 8);
      u8g2.printf("%s", packet.buffer);
    }

    /* draw a frame around the picture */
    // u8g2.drawFrame(0,0,OLED_W,OLED_H);
    // u8g2.drawFrame(1,1,384-2,240-2);
  } while (u8g2.nextPage());
  delay(1000);
}