#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "esp_timer.h"
#include "esp_random.h"

#define NUM_SENSORS          5
#define SENSOR_PERIOD_MS     200
#define READING_TIME_MS      40
#define SENDING_TIME_MS      40
#define TOLERANCE_MS         5

SemaphoreHandle_t bus_mutex;

void sensor_task(void *pvParameters) {
    int id = (int)pvParameters;
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = pdMS_TO_TICKS(SENSOR_PERIOD_MS);
    
    int64_t last_start_ms = 0;

    // Offset awal untuk mengatur antrean Bus (0, 40, 80, 120, 160ms)
    vTaskDelay(pdMS_TO_TICKS((id - 1) * READING_TIME_MS));
    while (1) {
        int64_t now_ms = esp_timer_get_time() / 1000;
        int64_t interval = (last_start_ms == 0) ? 0 : (now_ms - last_start_ms);
        
        //bus set for task
        if (xSemaphoreTake(bus_mutex, portMAX_DELAY) == pdTRUE) {
            printf("[%06lld ms] [S%d] BACA SENSOR (Interval: %lld ms)\n", now_ms, id, interval);
            vTaskDelay(pdMS_TO_TICKS(READING_TIME_MS)); //reading process
            esp_random(); // Simulasi data
            xSemaphoreGive(bus_mutex); // bus release, used for other reading process task
        }

        int64_t send_start = esp_timer_get_time() / 1000;
        vTaskDelay(pdMS_TO_TICKS(SENDING_TIME_MS)); // send process
        int64_t send_done = esp_timer_get_time() / 1000;
        // Log Deadline Miss jika interval > 205ms
        if (interval > (SENSOR_PERIOD_MS + TOLERANCE_MS)) {
            printf("[%06lld ms] [S%d] !!! DEADLINE MISS !!! Kirim Selesai (Interval: %lld ms)\n", send_done, id, interval);
        } else {
            printf("[%06lld ms] [S%d] KIRIM SELESAI (Durasi Total: %lld ms)\n", send_done, id, (send_done - now_ms));
        }
        last_start_ms = now_ms;
        // Menjaga interval 200ms presisi
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
    }
}

void app_main(void) {
    bus_mutex = xSemaphoreCreateMutex();
    if (bus_mutex != NULL) {
        for (int i = 1; i <= NUM_SENSORS; i++) {
            char name[10];
            sprintf(name, "S%d", i);
            xTaskCreate(sensor_task, name, 2048, (void *)i, 5, NULL);
        }
    }
}