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

#define LED_PIN GPIO_NUM_2  // Using GPIO2 for the LED
#define LDR_PIN GPIO_NUM_32 // LDR_PIN to gpio pin 32
#define LDR_ADC_CHANNEL ADC1_CHANNEL_4 // LDR_ADC_CHANNEL
#define BUTTON_GPIO GPIO_NUM_4  // usinf GPIO4 for the LED

#define AVG_WINDOW 10
#define SENSOR_THRESHOLD 500

#define LOG_BUFFER_SIZE 50

// synchronization
static SemaphoreHandle_t xLogMutex;  // Hand off access to the buffer
static SemaphoreHandle_t xButtonSem;  // Hand off on button press

// Shared buffer and index
static uint16_t sensor_log[LOG_BUFFER_SIZE];
static int log_index = 0;

// device heartbeat task
void heartbeat_task(void *pvParameters) {
    int heartbeatState = 0;
    TickType_t currentTime = pdTICKS_TO_MS( xTaskGetTickCount() );

    while (1) {
        currentTime = pdTICKS_TO_MS( xTaskGetTickCount() );
        gpio_set_level(LED_PIN, heartbeatState);  
        heartbeatState = !heartbeatState;  
        
        //turned off printing for this task
        //printf("Heart %s @ Time %lu\n", heartbeatState ? "Systole" : "Diastole", currentTime);
        vTaskDelay(pdMS_TO_TICKS(1400)); 
    }
    vTaskDelete(NULL); 
}

//prints vitals to console
void vitals_task(void *pvParameters) {
    TickType_t currentTime = pdTICKS_TO_MS( xTaskGetTickCount() );
    TickType_t previousTime = 0;

    static bool systole = false;
    while (1) {
        previousTime = currentTime;
        currentTime = pdTICKS_TO_MS( xTaskGetTickCount() );
        systole = !systole; 
        
        printf("Heart %s @ Time %lu\n", systole ? "Systole" : "Diastole", currentTime);
        vTaskDelay(pdMS_TO_TICKS(7000)); // Delay for 7s
    }
    vTaskDelete(NULL); 
}

// simulates a photoplethysmography (PPG) senor that measures heart rate
void PPG_task(void *pvParameters) {
    printf("Sensor active\n");
    adc1_config_width(ADC_WIDTH_BIT_12);
    adc1_config_channel_atten(LDR_ADC_CHANNEL, ADC_ATTEN_DB_11); 

 // Variables to compute LUX
    float raw;
    float Vmeasure = 0.;
    float Rmeasure = 0.;
    float lux = 0.;
    // Variables for moving average
    int luxreadings[AVG_WINDOW] = {0};
    int idx = 0;
    float sum = 0;

    for(int i = 0; i < AVG_WINDOW; ++i) {
        raw =  adc1_get_raw(LDR_ADC_CHANNEL);
        Vmeasure = (raw/4096)*3.3; 
        Rmeasure = (10000*Vmeasure)/(3.3-Vmeasure); 
        lux = pow((50*1000*pow(10, 0.7))/Rmeasure, (1/0.7)); 
        sum += luxreadings[i];
    }

    const TickType_t periodTicks = pdMS_TO_TICKS(200); // 200 ms period
    TickType_t lastWakeTime = xTaskGetTickCount(); // initialize last wake time

    printf("Sensor Aquiring Data \n");
    while (1) {
        // Read current sensor value
        raw = adc1_get_raw(LDR_ADC_CHANNEL);

        // Compute LUX
        Vmeasure = (raw/4096)*3.3; 
        Rmeasure = (10000*Vmeasure)/(3.3-Vmeasure);
        lux = pow((50*1000*pow(10, 0.7))/Rmeasure, (1/0.7)); 
       
        //removed average lux reading code

        uint16_t val = (uint16_t) raw; //analog value

        //lock Mutex to write value to buffer
        if(xSemaphoreTake(xLogMutex, pdMS_TO_TICKS(10))){
           //printf("Sample Aquired - saving to database \n");
            sensor_log[log_index] = val;
            log_index = (log_index + 1) % LOG_BUFFER_SIZE;
            xSemaphoreGive(xLogMutex); // gives up semaphore
            //printf("Sample Stored Successfully - ready for next sample \n");
        }

       vTaskDelayUntil(&lastWakeTime, periodTicks);
    }
}

//ISR: Triggered on button press 
void IRAM_ATTR button_isr_handler(void *arg){
   //BONUS: debouncing by checking the elapsed time since last press
    static TickType_t PrevTime = 0; 
    TickType_t CurrentTime = xTaskGetTickCountFromISR();
    if((CurrentTime - PrevTime) > pdMS_TO_TICKS(50)){
      BaseType_t xHigherPrioTaskWoken = pdFALSE;
      xSemaphoreGiveFromISR(xButtonSem, &xHigherPrioTaskWoken);
      portYIELD_FROM_ISR(xHigherPrioTaskWoken);
      PrevTime = CurrentTime;
    }
}

//waits for button press event
void PPGlogger_task(void *pvParameters){
    while(1){
      //blocks til ISR signals button press
        if(xSemaphoreTake(xButtonSem, portMAX_DELAY) == pdTRUE){
            printf("photoplethysmography logger activated; session will close on completion\n");
            uint16_t copy[LOG_BUFFER_SIZE];
            int count = 0;

            // Lock mutex to read from buffer
            if (xSemaphoreTake(xLogMutex, pdMS_TO_TICKS(100))) {
                memcpy(copy, sensor_log, sizeof(copy));
                count = log_index;
                xSemaphoreGive(xLogMutex);
            }

            //compression
            uint16_t min = 4095, max = 0;
            uint32_t sum = 0;
            for (int i = 0; i < LOG_BUFFER_SIZE; i++) {
                if (copy[i] < min) min = copy[i];
                if (copy[i] > max) max = copy[i];
                sum += copy[i];
            }

            uint16_t avg = sum / LOG_BUFFER_SIZE;
            //dump data
            printf("PPG Log compressed last %d readings: min=%d, max=%d, avg=%d \n", LOG_BUFFER_SIZE, min, max, avg);
            vTaskDelay(pdMS_TO_TICKS(200));
            printf("Dump complete.\n");
        }
    }
}

void app_main() {
    // Initialize LED GPIO     
    gpio_reset_pin(LED_PIN);
    gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);

    // Initialize LDR PIN as INPUT 
    gpio_reset_pin(LDR_PIN);
    gpio_set_direction(LDR_PIN, GPIO_MODE_INPUT);

    //Initialize Button GPIO
    gpio_reset_pin(BUTTON_GPIO);
    gpio_set_direction(BUTTON_GPIO, GPIO_MODE_INPUT);
    gpio_pullup_en(BUTTON_GPIO);
    gpio_set_intr_type(BUTTON_GPIO, GPIO_INTR_NEGEDGE);

    gpio_install_isr_service(0);
    gpio_isr_handler_add(BUTTON_GPIO, button_isr_handler, NULL);
    

    // Set ADC1's resolution 
    adc1_config_width(ADC_WIDTH_BIT_12);

    // Set the the input channel to 11 DB Attenuation
    adc1_config_channel_atten(LDR_ADC_CHANNEL, ADC_ATTEN_DB_11);

    //create binary sempahpore & mutex for log buffer
    xButtonSem = xSemaphoreCreateBinary();
    xLogMutex = xSemaphoreCreateMutex();
    xSemaphoreTake(xButtonSem, 0); // take so it starts in a blocking state

    // . pointer to task function, 
    // . descriptive name, [has a max length; located in the FREERTOS_CONFIG.H]
    // . stack depth, 
    // . parameters [optional] = NULL 
    // . priority [0 = low], 
    // . pointer referencing this created task [optional] = NULL
    // . core [0,1] to pin task too 
    // https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/freertos_additions.html#_CPPv423xTaskCreatePinnedToCore14TaskFunction_tPCKcK8uint32_tPCv11UBaseType_tPC12TaskHandle_tK10BaseType_t
    xTaskCreatePinnedToCore(heartbeat_task, "LED", 2048, NULL, 1, NULL, 1);
    xTaskCreatePinnedToCore(vitals_task, "STATUS", 2048, NULL, 1, NULL, 1);
    xTaskCreatePinnedToCore(PPG_task, "SENSOR", 4096, NULL, 2, NULL, 1);
    xTaskCreatePinnedToCore(PPGlogger_task, "LOGGER",4096, NULL, 3, NULL, 1);
}