// Multitasking demo for ESP32 + FreeRTOS (ESP-IDF)
// - Blinker task toggles LED (higher priority)
// - Sensor task "produces" samples and pushes to a queue
// - Logger task "consumes" from the queue and prints running stats
// - A mutex protects a shared heartbeat counter
//
// Works with ESP-IDF v4.x/v5.x

#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "driver/gpio.h"

// ====== Hardware config (change to match your dev board) ======
#define LED_GPIO        23

// ====== Task parameters ======
#define BLINK_PERIOD_MS       500
#define SENSOR_PERIOD_MS      200
#define LOGGER_PERIOD_MS      1000

// ====== Queue configuration ======
#define SAMPLE_QUEUE_LENGTH   16

typedef struct {
    uint32_t tick;   // FreeRTOS tick count at sample time
    float     value; // simulated sensor value
} sample_t;

// ====== Globals ======
static QueueHandle_t sample_queue;
static SemaphoreHandle_t heartbeat_mutex;
static uint32_t heartbeat = 0;

static void safe_inc_heartbeat(void) {
    if (xSemaphoreTake(heartbeat_mutex, pdMS_TO_TICKS(10)) == pdTRUE) {
        heartbeat++;
        xSemaphoreGive(heartbeat_mutex);
    }
}

static uint32_t safe_get_heartbeat(void) {
    uint32_t hb = 0;
    if (xSemaphoreTake(heartbeat_mutex, pdMS_TO_TICKS(10)) == pdTRUE) {
        hb = heartbeat;
        xSemaphoreGive(heartbeat_mutex);
    }
    return hb;
}

// ====== Tasks ======

static void blinker_task(void *arg) {
    gpio_reset_pin(LED_GPIO);
    gpio_set_direction(LED_GPIO, GPIO_MODE_OUTPUT);

    bool level = false;
    while (1) {
        gpio_set_level(LED_GPIO, level);
        level = !level;

        safe_inc_heartbeat(); // show that this task runs periodically

        vTaskDelay(pdMS_TO_TICKS(BLINK_PERIOD_MS));
    }
}

// Simulated sensor: produces a smooth-ish value (no hardware needed)
static float next_sensor_value(void) {
    // A tiny LUT-based waveform + slow drift
    static const float lut[] = {0.0f, 0.25f, 0.5f, 0.75f, 1.0f, 0.75f, 0.5f, 0.25f};
    static size_t idx = 0;
    static float drift = 0.0f;

    float v = lut[idx] + drift;
    idx = (idx + 1) % (sizeof(lut)/sizeof(lut[0]));
    drift += 0.01f;
    if (drift > 0.2f) drift = -0.2f;

    return v;
}

static void sensor_task(void *arg) {
    (void)arg;
    TickType_t last = xTaskGetTickCount();

    while (1) {
        // Periodic activation (soft real-time)
        vTaskDelayUntil(&last, pdMS_TO_TICKS(SENSOR_PERIOD_MS));

        sample_t s = {
            .tick = xTaskGetTickCount(),
            .value = next_sensor_value()
        };

        // Non-blocking send: if queue is full, drop the oldest by receiving once
        if (xQueueSend(sample_queue, &s, 0) != pdPASS) {
            sample_t throwaway;
            (void)xQueueReceive(sample_queue, &throwaway, 0);
            (void)xQueueSend(sample_queue, &s, 0);
        }
    }
}

static void logger_task(void *arg) {
    (void)arg;
    const TickType_t period = pdMS_TO_TICKS(LOGGER_PERIOD_MS);
    TickType_t last = xTaskGetTickCount();

    // simple running stats
    uint32_t count = 0;
    float sum = 0.0f, minv =  1e9f, maxv = -1e9f;

    while (1) {
        // Drain queue for this window
        sample_t s;
        uint32_t drained = 0;
        while (xQueueReceive(sample_queue, &s, 0) == pdTRUE) {
            drained++;
            count++;
            sum += s.value;
            if (s.value < minv) minv = s.value;
            if (s.value > maxv) maxv = s.value;
        }

        float mean = (count > 0) ? (sum / (float)count) : 0.0f;

        printf("[LOGGER] hb=%lu drained=%lu total=%lu mean=%.3f min=%.3f max=%.3f\n",
               (unsigned long)safe_get_heartbeat(),
               (unsigned long)drained,
               (unsigned long)count,
               mean, minv, maxv);

        vTaskDelayUntil(&last, period);
    }
}

// ====== app_main ======
void app_main(void) {
    // Create queue & mutex first
    sample_queue = xQueueCreate(SAMPLE_QUEUE_LENGTH, sizeof(sample_t));
    heartbeat_mutex = xSemaphoreCreateMutex();

    if (!sample_queue || !heartbeat_mutex) {
        printf("Failed to create queue or mutex.\n");
        return;
    }

    // Create tasks (optionally pin to cores 0/1)
    BaseType_t ok;

    ok = xTaskCreatePinnedToCore(blinker_task, "blinker",
                                 2048, NULL, 5, NULL, 1); // higher prio, Core 1
    if (ok != pdPASS) printf("Failed to create blinker_task\n");

    ok = xTaskCreatePinnedToCore(sensor_task, "sensor",
                                 3072, NULL, 4, NULL, 1); // producer, Core 1
    if (ok != pdPASS) printf("Failed to create sensor_task\n");

    ok = xTaskCreatePinnedToCore(logger_task, "logger",
                                 4096, NULL, 3, NULL, 0); // consumer/logger, Core 0
    if (ok != pdPASS) printf("Failed to create logger_task\n");

    // Nothing else to do in app_main (tasks run forever)
}