#include <Arduino.h>
#include <Wire.h>

// --- Pin Definitionen ---
#define SDA_PIN 21  // Standard ESP32 SDA
#define SCL_PIN 22  // Standard ESP32 SCL
#define SQW_PIN 19  // Sicherer, freier Pin für den Interrupt

#define DS1307_CTRL_ID 0x68



// --- Globale Synchronisations-Variablen ---
volatile uint32_t isr_micros = 0;           // Speichert den exakten Zeitpunkt der SQW Flanke
volatile bool new_second_trigger = false;   // Flag für Core 0, dass eine neue Sekunde begonnen hat
portMUX_TYPE isr_mux = portMUX_INITIALIZER_UNLOCKED;

// --- Variablen für den lokalen Timer ---
uint32_t current_epoch = 0;       // Die von der RTC gelesene Zeit in Sekunden (vereinfacht als Tagessekunden)
uint32_t epoch_micros_base = 0;   // Der exakte micros() Wert, an dem diese Sekunde startete
portMUX_TYPE time_mux = portMUX_INITIALIZER_UNLOCKED;

// Task Handles
TaskHandle_t TaskRTC_Handle;
TaskHandle_t TaskTest_Handle;

// Hilfsfunktion: BCD zu Dezimal
uint8_t bcd2dec(uint8_t val) {
  return ((val / 16 * 10) + (val % 16));
}

// Hilfsfunktion: Dezimal zu BCD
uint8_t dec2bcd(uint8_t val) {
  return ((val / 10 * 16) + (val % 10));
}

// -----------------------------------------------------------------
// INTERRUPT SERVICE ROUTINE (Wird exakt bei der SQW Flanke aufgerufen)
// -----------------------------------------------------------------
void IRAM_ATTR sqw_isr() {
  portENTER_CRITICAL_ISR(&isr_mux);
  uint32_t current_micros = micros();
  isr_micros = current_micros;
  new_second_trigger = true;
  portEXIT_CRITICAL_ISR(&isr_mux);

  // Wecke den Test-Task auf Core 1, übermittle den exakten Zeitstempel der Flanke
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
  vTaskNotifyGiveFromISR(TaskTest_Handle, &xHigherPriorityTaskWoken);
  if (xHigherPriorityTaskWoken) {
    portYIELD_FROM_ISR();
  }
}

// -----------------------------------------------------------------
// LOKALER TIMER FUNKTION (jederzeit aufrufbar)
// -----------------------------------------------------------------
// Gibt die Zeit in Mikrosekunden zurück. 
// Interpoliert zwischen den 1Hz Impulsen extrem präzise.
// Setze diesen Wert zum Testen (z.B. 150 für eine Verletzung der Spezifikation)
float EMULATED_DRIFT_PPM = 99.0; 

uint64_t get_local_time_at_us(uint32_t target_micros) {
  portENTER_CRITICAL(&time_mux);
  uint32_t base_us = epoch_micros_base;
  uint32_t epoch = current_epoch;
  portEXIT_CRITICAL(&time_mux);

  // Die echte vergangene Zeit in Mikrosekunden
  int32_t real_diff = target_micros - base_us; 
  
  // Wir emulieren die Drift: 
  // Ein positiver PPM-Wert lässt die interne Uhr "schneller" laufen.
  float drift_factor = 1.0 + (EMULATED_DRIFT_PPM / 1000000.0);
  uint32_t drifted_diff = (uint32_t)(real_diff * drift_factor);

  return ((uint64_t)epoch * 1000000ULL) + drifted_diff;
}


// -----------------------------------------------------------------
// CORE 0 TASK: RTC Manager (Liest I2C, max 1x pro Sekunde)
// -----------------------------------------------------------------
void TaskRTC(void *pvParameters) {
  // I2C Setup
  Wire.begin(SDA_PIN, SCL_PIN);
  Wire.setClock(400000); // 400kHz Fast Mode

  // DS1307 konfigurieren: 1Hz Rechtecksignal auf SQW aktivieren
  Wire.beginTransmission(DS1307_CTRL_ID);
  Wire.write(0x07); // Control Register
  Wire.write(0x10); // OUT=0, SQWE=1, RS1=0, RS0=0 -> 1Hz
  Wire.endTransmission();

  // Interrupt anheften
  pinMode(SQW_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(SQW_PIN), sqw_isr, FALLING);

  while (true) {
    bool trigger = false;
    uint32_t edge_micros = 0;

    // Prüfen, ob ISR gefeuert hat
    portENTER_CRITICAL(&isr_mux);
    if (new_second_trigger) {
      trigger = true;
      edge_micros = isr_micros;
      new_second_trigger = false;
    }
    portEXIT_CRITICAL(&isr_mux);

    if (trigger) {
      // Höchstens alle 1000ms: I2C abfragen!
      Wire.beginTransmission(DS1307_CTRL_ID);
      Wire.write(0x00);
      if (Wire.endTransmission() == 0) {
        Wire.requestFrom(DS1307_CTRL_ID, 3); // Lese Sek, Min, Std
        if (Wire.available() == 3) {
          uint8_t s = bcd2dec(Wire.read() & 0x7F);
          uint8_t m = bcd2dec(Wire.read());
          uint8_t h = bcd2dec(Wire.read() & 0x3F);

          // Berechne "Epoch" (hier vereinfacht als Sekunden seit 00:00:00)
          uint32_t total_seconds = (h * 3600) + (m * 60) + s;

          // Lokalen Timer updaten (Thread-safe)
          portENTER_CRITICAL(&time_mux);
          current_epoch = total_seconds;
          epoch_micros_base = edge_micros; // Das Wichtigste: Wir verknüpfen die Zeit mit der Flanke!
          portEXIT_CRITICAL(&time_mux);
        }
      }
    }
    // Kurze Pause, um den Watchdog auf Core 0 nicht auszulösen
    vTaskDelay(pdMS_TO_TICKS(10));
  }
}

// -----------------------------------------------------------------
// CORE 1 TASK: Tester (Weist die Spezifikation nach)
// -----------------------------------------------------------------
void TaskTest(void *pvParameters) {
  while (true) {
    // Blockiert, bis die ISR (Hardware-Flanke) uns aufweckt
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

    // Wir holen uns den exakten Zeitstempel der letzten Flanke sicher aus der globalen Variable
    portENTER_CRITICAL(&isr_mux);
    uint32_t test_micros = isr_micros;
    portEXIT_CRITICAL(&isr_mux);

    // WICHTIG: Da die Uhrzeit erst jetzt gerade auf Core 0 per I2C ausgelesen wird,
    // nutzt die folgende Funktion noch den "alten" base_epoch Wert von vor 1 Sekunde.
    // Dank unserer Interpolation muss das Ergebnis trotzdem exakt eine volle Sekunde sein!
    uint64_t interpolated_time = get_local_time_at_us(test_micros);

    // Da der SQW-Interrupt EXAKT auf der vollen Sekunde auslöst, müssen 
    // die Mikrosekunden unseres lokalen Timers zu diesem Zeitpunkt exakt "000000" sein.
    uint32_t fractional_us = interpolated_time % 1000000;

    // Abweichung berechnen (falls es durch minimalen ESP32-Quarz-Drift leicht unter oder über der Sekunde liegt)
    uint32_t deviation = fractional_us;
    if (deviation > 500000) {
      deviation = 1000000 - deviation; // Wrap-around behandeln (z.B. 999998 µs sind 2 µs Abweichung)
    }

    // Ausgabe
    Serial.printf("[Core %d] Lokaler Timer bei SQW-Flanke: %llu us | Abweichung: %lu us\n", 
                  xPortGetCoreID(), interpolated_time, deviation);

    if (deviation <= 100) {
      Serial.println("  -> OK: Innerhalb der 100us Spezifikation!");
    } else {
      Serial.println("  -> FEHLER: Spezifikation verletzt!");
    }
  }
}

// -----------------------------------------------------------------
// SETUP & LOOP
// -----------------------------------------------------------------
void setup() {


  Serial.begin(115200);
  Serial.println("Starte hochpräzisen RTC Timer Test...");

  // Core 0 Task für das Auslesen der RTC
  xTaskCreatePinnedToCore(
    TaskRTC,        // Task-Funktion
    "RTC_Task",     // Name
    4096,           // Stack
    NULL,           // Parameter
    2,              // Priorität (Höher ist wichtiger)
    &TaskRTC_Handle,// Handle
    0               // Core 0
  );

  // Core 1 Task für den Test
  xTaskCreatePinnedToCore(
    TaskTest,       // Task-Funktion
    "Test_Task",    // Name
    4096,           // Stack
    NULL,           // Parameter
    1,              // Priorität
    &TaskTest_Handle,// Handle
    1               // Core 1
  );
}

void loop() {
  // Loop wird nicht benötigt, FreeRTOS übernimmt. 
  // vTaskDelete(NULL) killt den Arduino-Setup-Task und gibt Speicher frei.
  vTaskDelete(NULL);
}