#include <Arduino.h>
#include "freertos/FreeRTOS.h"
#include <freertos/task.h>
#include <freertos/semphr.h>
#include <driver/gpio.h>
#include <esp_timer.h>

// --- Konfiguration ---
#define BUTTON_GPIO GPIO_NUM_2 // Verwenden Sie einen Pin, der in Wokwi gut zugänglich ist, z.B. D2
#define SYNC_QUEUE_LENGTH 5

// Struktur für die Zeit-Synchronisationsanfrage
typedef struct {
    int core_id;             // Core, der die Anfrage sendet (1)
    uint64_t client_send_us; // Zeitstempel des Clients beim Senden der Anfrage (T_start)
} SyncRequest_t;

// Struktur für die Synchronisationsantwort
typedef struct {
    uint64_t server_time_us; // Zeitstempel des Servers beim Empfang der Anfrage (T_server)
} SyncResponse_t;

// Globale FreeRTOS Objekte
QueueHandle_t syncRequestQueue;
QueueHandle_t syncResponseQueue;

// Mutex zum Schutz der geteilten Zeitvariablen
SemaphoreHandle_t core0_time_mutex; 
SemaphoreHandle_t core1_time_mutex;

// Geteilte Zeitvariablen (simulierte Uhren)
volatile uint64_t core0_simulated_time_us = 0;
volatile uint64_t core1_simulated_time_us = 0;

// Konstanten zur Simulation des Drifts (in us pro Millisekunde des echten Timers)
// Core 0: Drift von 10 us pro 1000 us (1% zu schnell)
// Core 1: Drift von 5 us pro 1000 us (0.5% zu schnell)
// Ziel: Core 0 (Server) läuft schneller, Core 1 (Client) läuft etwas langsamer, aber beide driften leicht.
const uint64_t CORE0_DRIFT_PER_MS = 10;
const uint64_t CORE1_DRIFT_PER_MS = 5;

// Letzter Zeitstempel für die Drift-Berechnung
uint64_t core0_last_update_us = 0;
uint64_t core1_last_update_us = 0;



// --- Funktionen zur Uhrenverwaltung ---

void update_simulated_time(volatile uint64_t& simulated_time, volatile uint64_t& last_update_us, uint64_t drift_per_ms, SemaphoreHandle_t mutex) {
    if (xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE) {
        uint64_t now_us = esp_timer_get_time();
        if (last_update_us == 0) {
            last_update_us = now_us;
        }

        uint64_t elapsed_us = now_us - last_update_us;
        uint64_t elapsed_ms = elapsed_us / 1000;
        
        // Berechne Drift und aktualisiere die simulierte Zeit
        simulated_time += elapsed_us;
        simulated_time += (elapsed_ms * drift_per_ms);

        last_update_us = now_us;
        xSemaphoreGive(mutex);
    }
}

uint64_t get_simulated_time(volatile uint64_t& simulated_time, SemaphoreHandle_t mutex) {
    uint64_t time_val = 0;
    if (xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE) {
        time_val = simulated_time;
        xSemaphoreGive(mutex);
    }
    return time_val;
}

// --- Tasks ---

// Task für Core 0 (Server und Taster-Handling)
void core0_task(void *pvParameters) {
    
    // Initialisierung der Zeit
    core0_last_update_us = esp_timer_get_time();
    
    // Statusvariable für Taster-Entprellung
    bool button_pressed_last = false;
    
    SyncRequest_t request;

    while (1) {
        // 1. Uhren-Update und Anzeige (Server)
        update_simulated_time(core0_simulated_time_us, core0_last_update_us, CORE0_DRIFT_PER_MS, core0_time_mutex);
        uint64_t time_c0 = get_simulated_time(core0_simulated_time_us, core0_time_mutex);
        
        Serial.printf("[Core 0 (Server)] Zeit: %llu us (Drift: +%llu us/ms)\n", time_c0, CORE0_DRIFT_PER_MS);
        
        // 2. Taster-Prüfung (Trigger für Client-Sync)
        bool button_current = (gpio_get_level(BUTTON_GPIO) == 1); // Taster geht auf HIGH bei Drücken (wegen PULLDOWN)
        
        if (button_current && !button_pressed_last) {
            Serial.println(">>> Taster gedrückt - Sende Sync-Anfrage an Core 1");
            
            // Client-Anfrage fälschen (Da Core 0 sie sendet, simuliert er den Client-Send-Timestamp)
            // Tatsächlich müsste diese Logik in Core 1 liegen. Wir lassen Core 1 die Anfrage stellen.
            // Der Taster triggert Core 1 über ein Semaphor.
            xSemaphoreGive(*((SemaphoreHandle_t*)pvParameters));
        }
        button_pressed_last = button_current;

        // 3. Queue-Verarbeitung (Christian's Server-Teil)
        if (xQueueReceive(syncRequestQueue, &request, 0) == pdTRUE) {
            uint64_t t_server = get_simulated_time(core0_simulated_time_us, core0_time_mutex); // T_server
            Serial.printf("[Core 0 (Server)] Sync-Anfrage von Core %d empfangen. Sende T_server=%llu\n", request.core_id, t_server);
            
            // Sende Antwort
            SyncResponse_t response = { .server_time_us = t_server };
            xQueueSend(syncResponseQueue, &response, portMAX_DELAY);
        }

        vTaskDelay(pdMS_TO_TICKS(1000)); // Anzeige alle 1s
    }
}

// Task für Core 1 (Client und Synchronisation)
void core1_task(void *pvParameters) {
    SemaphoreHandle_t sync_trigger = *((SemaphoreHandle_t*)pvParameters);

    // Initialisierung der Zeit
    core1_last_update_us = esp_timer_get_time();

    while (1) {
        // 1. Uhren-Update und Anzeige
        update_simulated_time(core1_simulated_time_us, core1_last_update_us, CORE1_DRIFT_PER_MS, core1_time_mutex);
        uint64_t time_c1 = get_simulated_time(core1_simulated_time_us, core1_time_mutex);
        
        Serial.printf("[Core 1 (Client)] Zeit: %llu us (Drift: +%llu us/ms)\n", time_c1, CORE1_DRIFT_PER_MS);

        // 2. Warte auf Sync-Trigger vom Taster (aus Core 0)
        if (xSemaphoreTake(sync_trigger, 0) == pdTRUE) {
            // --- Christian's Algorithmus (Client-Teil) ---
            Serial.println("[Core 1 (Client)] Starte Zeit-Synchronisation...");
            
            // Sende Anfrage: T_start (T1)
            uint64_t t1 = get_simulated_time(core1_simulated_time_us, core1_time_mutex);
            SyncRequest_t request = { .core_id = 1, .client_send_us = t1 };
            
            // Verwende echten High-Resolution Timer für RTT-Messung
            uint64_t rtt_start_us = esp_timer_get_time(); 
            xQueueSend(syncRequestQueue, &request, portMAX_DELAY);
            
            // Warte auf Antwort von Core 0
            SyncResponse_t response;
            if (xQueueReceive(syncResponseQueue, &response, pdMS_TO_TICKS(100)) == pdTRUE) {
                // T_ende (T2)
                uint64_t rtt_end_us = esp_timer_get_time();
                uint64_t t2 = get_simulated_time(core1_simulated_time_us, core1_time_mutex);

                uint64_t t_server = response.server_time_us; // T_server (T_s)
                
                // Round-Trip-Time (RTT)
                uint64_t rtt_us = rtt_end_us - rtt_start_us;
                
                // Geschätzte einseitige Verzögerung (Delay)
                // delay = RTT / 2 (Grundannahme des Algorithmus)
                uint64_t delay_us = rtt_us / 2;
                
                // Neue synchronisierte Zeit: T_sync = T_server + delay
                uint64_t t_sync = t_server + delay_us;

                // Uhr korrigieren: T_sync - T_client (T2)
                int64_t correction_us = (int64_t)t_sync - (int64_t)t2;
                
                // Anwendung der Korrektur auf die simulierte Core-Zeit
                if (xSemaphoreTake(core1_time_mutex, portMAX_DELAY) == pdTRUE) {
                    core1_simulated_time_us += correction_us;
                    xSemaphoreGive(core1_time_mutex);
                }

                Serial.printf("    RTT (real): %llu us\n", rtt_us);
                Serial.printf("    Verzögerung (Delay): %llu us\n", delay_us);
                Serial.printf("    T_client (Start T1): %llu us\n", t1);
                Serial.printf("    T_server (T_s): %llu us\n", t_server);
                Serial.printf("    T_client (Ende T2): %llu us\n", t2);
                Serial.printf("    Neue simulierte Zeit T_sync: %llu us\n", t_sync);
                Serial.printf("    Korrektur: %lld us\n", correction_us);
                Serial.println(">>> Synchronisation abgeschlossen.");
            } else {
                 Serial.println("[Core 1 (Client)] FEHLER: Keine Antwort vom Server (Core 0).");
            }
        }

        vTaskDelay(pdMS_TO_TICKS(50)); // Häufigeres Überprüfen für den Trigger
    }
}

// --- Arduino Setup und Loop ---

SemaphoreHandle_t syncTriggerSemaphore;

void setup() {
    Serial.begin(115200);
    delay(1000);
    Serial.println("--- FreeRTOS Christian's Algorithmus (ESP32 Multicore) ---");

    // Taster-Konfiguration
    gpio_set_direction(BUTTON_GPIO, GPIO_MODE_INPUT);
    gpio_set_pull_mode(BUTTON_GPIO, GPIO_PULLDOWN_ONLY); // PULLDOWN verwenden, da der Taster Pin mit Ground verbinden soll


    // Erstellen der FreeRTOS-Objekte
    syncRequestQueue = xQueueCreate(SYNC_QUEUE_LENGTH, sizeof(SyncRequest_t));
    syncResponseQueue = xQueueCreate(SYNC_QUEUE_LENGTH, sizeof(SyncResponse_t));
    syncTriggerSemaphore = xSemaphoreCreateBinary(); // Taster-Trigger

    core0_time_mutex = xSemaphoreCreateMutex();
    core1_time_mutex = xSemaphoreCreateMutex();
    
    if (syncRequestQueue == NULL || syncResponseQueue == NULL || syncTriggerSemaphore == NULL) {
        Serial.println("Fehler beim Erstellen der FreeRTOS-Objekte!");
        return;
    }

    // Erstellen und Binden der Tasks an Cores
    // Core 0 Task (Server)
    xTaskCreatePinnedToCore(
        core0_task,           // Funktion, die als Task ausgeführt werden soll
        "Core0_Task",         // Task-Name
        4096,                 // Stack-Größe (in Bytes)
        (void*)&syncTriggerSemaphore, // Übergabe des Semaphors
        1,                    // Priorität
        NULL,                 // Task-Handle
        0                     // Core ID (0)
    );

    // Core 1 Task (Client)
    xTaskCreatePinnedToCore(
        core1_task,           // Funktion, die als Task ausgeführt werden soll
        "Core1_Task",         // Task-Name
        4096,                 // Stack-Größe (in Bytes)
        (void*)&syncTriggerSemaphore, // Übergabe des Semaphors
        1,                    // Priorität
        NULL,                 // Task-Handle
        1                     // Core ID (1)
    );
}

void loop() {
    // Die Tasks übernehmen die Kontrolle. loop() sollte leer bleiben.
    vTaskDelete(NULL); 
}