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

#include "SDCardHelper.hpp"

// LORA Settings
#define SYNC_WORD 0xCE
#define RECEIVER_ID 0xAB  // receiver
#define SELF_ID 0xAA  // sender

#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

// GPS Settings
#define _RX_GPS 25
#define _TX_GPS 4
#define _BAUDRATE_GPS 9600

// OLED Settings
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

#define SD_CS 13

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
TinyGPSPlus gps;

SDHelper logger;

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

bool waitForPacket(unsigned long timeout_ms) {
  unsigned long t0 = millis();
  while (millis() - t0 < timeout_ms) {
    if (LoRa.parsePacket()) return true;
    vTaskDelay(pdMS_TO_TICKS(5));;
  }
  return false;  // Timeout
}

bool getPacket(String& payload, byte& id) {
  LoRa.parsePacket();

  byte sync = LoRa.read();
  id = LoRa.read();

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

  byte len = LoRa.read();

  while (LoRa.available()) payload += (char)LoRa.read();
  return (payload.length() == len);
}

void showOnOLED(const String& line1, const String& line2 = "", const String& line3 = "", const String& line4 = "") {
  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);
  }
  if (line4.length()) {
    display.setCursor(0, 48);
    display.println(line4);
  }

  display.display();
}

void SerialPrintGPS(){
  Serial.print("LAT: ");
  Serial.println(gps.location.lat(), 6);
  Serial.print("LONG: "); 
  Serial.println(gps.location.lng(), 6);
  Serial.print("SPEED (km/h) = "); 
  Serial.println(gps.speed.kmph()); 
  Serial.print("ALT (min)= "); 
  Serial.println(gps.altitude.meters());
  Serial.print("HDOP = "); 
  Serial.println(gps.hdop.value() / 100.0); 
  Serial.print("Satellites = "); 
  Serial.println(gps.satellites.value()); 
  Serial.print("Time in UTC: ");
  Serial.println(String(gps.date.year()) + "/" + String(gps.date.month()) + "/" + String(gps.date.day()) + "," + String(gps.time.hour()) + ":" + String(gps.time.minute()) + ":" + String(gps.time.second()));
  Serial.println("");
}

void storeToSD(const String& lat, const String& lng){
  static int i = 1;
  Serial.println("Filename: " + logger.Filename());
  logger.Log(String(i++) + ": " + lat + " " + lng);
}

void setup() {
  Serial.begin(115200);
  Serial2.begin(_BAUDRATE_GPS, SERIAL_8N1, _RX_GPS, _TX_GPS);

  // OLED init
  Wire.begin();  // SDA/SCL Default
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED init fehlgeschlagen");
    while (1) vTaskDelay(pdMS_TO_TICKS(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) vTaskDelay(pdMS_TO_TICKS(1000));;
  }
  LoRa.enableCrc();
  showOnOLED("LoRa bereit", "Creator:", "Timo Niemann");
}

void loop() {
  static size_t start = millis();
  if(Serial2.available()){
    
    size_t while_break = millis();
    while(!Serial2.available() && millis() - while_break < 1000) vTaskDelay(pdMS_TO_TICKS(10));
    
    gps.encode(Serial2.read());
    SerialPrintGPS();
    
    if(gps.location.isUpdated()){
      Serial.println("GPS Update!");
    }
    
    start = millis();
  }

  if (waitForPacket(10)) {
    String cmd;
    byte snd;
    if (!getPacket(cmd, snd)) {
      showOnOLED("Fehler:", "Packet ungueltig");
      return;
    }

    if (cmd == "g") {
      Serial.println("Empfangen: " + cmd);
      showOnOLED("Empfangen von:", "ID " + String(snd), "Cmd: " + cmd);
      
      String lat_str = "LAT: " + String(gps.location.lat(), 6);
      String lng_str = "LON: " + String(gps.location.lng(), 6);
      String satellites = "Satelliten: " + String(gps.satellites.value());
      Serial.println("Sende GPS:\n" + lat_str + "\n" + lng_str);
      showOnOLED("Sende GPS Koords:", lat_str, lng_str, satellites);

      storeToSD(lat_str, lng_str);
      sendMessage(lat_str + "\n\n" + lng_str + "\n\n" + satellites);
    }
  }
  vTaskDelay(pdMS_TO_TICKS(10));
}