#include <SPI.h>
#include <LoRa.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SYNC_WORD 0xCE
#define SENDER_ID 0xAA
#define SELF_ID 0xAB

#define LORA_SCK 5
#define LORA_MISO 19
#define LORA_MOSI 27
#define LORA_CS 18
#define LORA_RST 23
#define LORA_IRQ 26
#define LORA_FREQ 868E6

// OLED Settings
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void sendMessage(const String& msg) {
  LoRa.beginPacket();
  LoRa.write(SYNC_WORD);
  LoRa.write(SENDER_ID);
  LoRa.write(msg.length());
  LoRa.print(msg);
  LoRa.endPacket(true);
}

bool receiveMessage(String& out) {
  LoRa.parsePacket();

  byte sync = LoRa.read();
  byte sender = LoRa.read();
  if (sync != SYNC_WORD || sender != SELF_ID) {
    LoRa.flush();
    // Serial.println("Unidentifiziertes Paket: " + String(sync) + String(sender));
    return false;
  }

  byte len = LoRa.read();
  String data;
  while (LoRa.available()) data += (char)LoRa.read();

  if (data.length() != len) {
    out = "ERR: LEN";
  } else {
    out = data;
    sendMessage("ACK");
  }
  return true;
}

void showOnOLED(const String& line1, const String& line2 = "", const String& line3 = "") {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(line1);
  if (line2.length()) {
    display.setCursor(0, 16);
    display.println(line2);
  }
  if (line3.length()) {
    display.setCursor(0, 32);
    display.println(line3);
  }
  display.display();
}

void printHelp() {
  Serial.println("g – Standort anfragen\nh – Hilfe");
  showOnOLED("Befehle:", "g Standort / h Hilfe");
}

void handleCommandG() {
  sendMessage("g");
  unsigned long t0 = millis();
  String answer;
  while (millis() - t0 < 1000) {
    if (receiveMessage(answer)) {
      Serial.println(answer);
      showOnOLED("Antwort von Sender:", answer);
      return;
    }
    delay(10);
  }
  Serial.println("Timeout");
  showOnOLED("Timeout beim", "Empfangen");
}

void setup() {
  Serial.begin(115200);

  // OLED init
  Wire.begin();  // SDA/SCL Default
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED init fehlgeschlagen");
    while (1) delay(1000);
  }
  showOnOLED("LoRa OLED Init", "Bitte warten...");

  // LoRa init
  SPI.begin(LORA_SCK, LORA_MISO, LORA_MOSI, LORA_CS);
  LoRa.setPins(LORA_CS, LORA_RST, LORA_IRQ);
  if (!LoRa.begin(LORA_FREQ)) {
    Serial.println("Konnte LoRa nicht initialisieren!");
    showOnOLED("LoRa Init fehl", "Neustart nötig");
    while (1) delay(1000);
  }
  LoRa.enableCrc();
  showOnOLED("LoRa bereit", "Creator:", "Timo Niemann");
}

void loop() {
  if (Serial.available()) {
    char cmd = Serial.read();
    switch (cmd) {
      case 'h': printHelp(); break;
      case 'g': handleCommandG(); break;
      default: printHelp(); break;
    }
  }

  String msg;
  if (receiveMessage(msg)) {
    Serial.printf("RX: %s\n", msg.c_str());
    showOnOLED("Empfangen:", msg);
  }
}