/* --------------------------------------------------------------
   Application: Filament Diameter Sensor
   Release Type: 1st attempt
   Class: Real Time Systems - Su 2025
   AI Use: Commented inline
---------------------------------------------------------------*/
#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
// #include "driver/adc.h"
#include "math.h"
#include "freertos/semphr.h"
#include <esp_log.h>
#include "esp_adc/adc_oneshot.h" // ADD
#include "hal/adc_types.h"
#include "esp_timer.h"
#include <inttypes.h>

#include "esp_adc/adc_cali_scheme.h" // required for calibration scheme of the ADC channel
#include "esp_adc/adc_cali.h"        // required for calibration of the ADC channel

#include "driver/i2c.h"
#include "esp_ssd1306/include/ssd1306.h"

#define LED_PIN GPIO_NUM_2          // changed per challenge to GPIO2
#define WARNING_LED_PIN GPIO_NUM_15 // low-lux threshold warning yellow LED connected to GPIO15
#define CALIBRATION_BUTTON_GPIO GPIO_NUM_0      // Boot Button used to calibrate
#define LED_OFF 0
#define LED_ON 1
#define SHORT_DURATION 250 // tested to work as low as 150, but for 125, period indicated is 120 !
#define LONG_DELAY 1000
#define TINY_DELAY 50
#define ADC_DELAY 100

#define priorityX 1
#define priorityY 2
#define priorityZ 3
#define priorityT 4

// I2C configuration
#define I2C_MASTER_NUM          I2C_NUM_0
#define I2C_MASTER_SDA_IO       21
#define I2C_MASTER_SCL_IO       22
#define I2C_MASTER_FREQ_HZ      400000
#define I2C_MASTER_TX_BUF_DISABLE 0
#define I2C_MASTER_RX_BUF_DISABLE 0

// I2C scanner
#define I2C_PORT        0
#define I2C_SDA_PIN     21
#define I2C_SCL_PIN     22
#define I2C_FREQ_HZ     400000

// OLED I2C Configuration
#define OLED_SDA_PIN GPIO_NUM_21
#define OLED_SCL_PIN GPIO_NUM_22
#define OLED_I2C_PORT I2C_NUM_0
#define OLED_I2C_ADDR 0x3C
#define OLED_WIDTH 128
#define OLED_HEIGHT 64

#define ANALOG_PIN1 GPIO_NUM_32 // GPIO32 connected to LDR AO PIN of sensor1
#define LDR_ADC_CHANNEL1 ADC_CHANNEL_4
#define ANALOG_PIN2 GPIO_NUM_33 // GPIO33 connected to LDR AO PIN of sensor2
#define LDR_ADC_CHANNEL2 ADC_CHANNEL_5
#define LOG_BUFFER_SIZE 20 // Number of samples for averaging and Lux statistics

// BONUS Adding in some examples of using the built in ESP Logging capabilties!
const static char *TAG = "LOG-DEMO>";

// Synchronization
static SemaphoreHandle_t xLogMutex;  // Hand off access to the buffer!
static SemaphoreHandle_t xCalibButtonSem;

int ledState = LED_OFF; // consistent starting state of LED

// calibration data for filament diameter
const float cal_dia1 = 1.5900f;
const float cal_dia2 = 1.8900f;

const int32_t raw_dia1 = 5282;
const int32_t raw_dia2 = 5175;

int32_t user_raw_dia1 = -1;
int32_t user_raw_dia2 = -1;

float compute_slope(int32_t raw1, int32_t raw2)
{
    if (raw1 == raw2) return 0.0f;
    return (cal_dia2 - cal_dia1) / (float)(raw2 - raw1);
}

const float fallback_slope = (cal_dia2 - cal_dia1) / (raw_dia2 - raw_dia1);
const float default_nominal_diameter = 1.7500f;
const float max_difference_diameter = 0.200f;
const float min_diameter = 1.0000f;
const float max_diameter = 2.0000f;

const int16_t AnalogRange = (1 << 12) - 1; // = 4095

// button press time and state
volatile int64_t button_press_time = 0;
volatile bool button_was_pressed = false;

const float VS = 3.3; // ESP32 operating voltage
int analogValueCalibrated;
double VoutSum = 0.0;

// Handle definitions
static i2c_master_bus_handle_t i2c_bus = NULL;
static ssd1306_handle_t oled = NULL;
TaskHandle_t ADCTaskHandle = NULL;
esp_timer_handle_t debounce_timer;

// helper functions
void setup_debounce_timer(esp_timer_handle_t *handle, esp_timer_cb_t cb, const char *name)
{
    const esp_timer_create_args_t args = {
        .callback = cb,
        .name = name
    };
    ESP_ERROR_CHECK(esp_timer_create(&args, handle));
}

void i2c_master_init()
{
    i2c_master_bus_config_t bus_config = {
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .i2c_port = I2C_MASTER_NUM,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .glitch_ignore_cnt = 7,
        .flags.enable_internal_pullup = true
    };

    ESP_ERROR_CHECK(i2c_new_master_bus(&bus_config, &i2c_bus));
}

void oled_init()
{
    ssd1306_config_t oled_cfg = I2C_SSD1306_128x64_CONFIG_DEFAULT;

    esp_err_t ret = ssd1306_init(i2c_bus, &oled_cfg, &oled);
    if (ret != ESP_OK || oled == NULL) {
        ESP_LOGE(TAG, "Failed to initialize SSD1306 OLED display");
        return;
    }

    ssd1306_clear_display(oled, false);
    ssd1306_display_text(oled, 0, "Display Ready!", false);
}

// === ISR: Triggered on calibration button press === use BOOT button
void IRAM_ATTR calibration_button_isr_handler(void *arg)
{
    gpio_isr_handler_remove(CALIBRATION_BUTTON_GPIO); // Disable ISR during debounce

    int level = gpio_get_level(CALIBRATION_BUTTON_GPIO);
    int64_t now = esp_timer_get_time();

    if (level == 0) {
        button_press_time = now;  // button down
    } else {
        int64_t duration = (now - button_press_time) / 1000; // ms
        button_was_pressed = true;

        if (duration >= 100) {
            BaseType_t xHigherPriorityTaskWoken = pdFALSE;
            xSemaphoreGiveFromISR(xCalibButtonSem, &xHigherPriorityTaskWoken);
            portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
        }
    }

    // Always restart the debounce timer after release
    esp_timer_start_once(debounce_timer, 50000); // 50 ms debounce
}

void debounce_timer_callback(void *arg)
{
    gpio_isr_handler_add(CALIBRATION_BUTTON_GPIO, calibration_button_isr_handler, NULL); 
}

void i2c_scan_ng()
{
    ESP_LOGI("I2C_SCAN", "Initializing I2C master on port %d", I2C_PORT);

    i2c_master_bus_handle_t i2c_bus = NULL;
    i2c_master_bus_config_t bus_cfg = {
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .i2c_port = I2C_PORT,
        .scl_io_num = I2C_SCL_PIN,
        .sda_io_num = I2C_SDA_PIN,
        .glitch_ignore_cnt = 7,
        .flags.enable_internal_pullup = true,
    };

    ESP_ERROR_CHECK(i2c_new_master_bus(&bus_cfg, &i2c_bus));
    ESP_LOGI("I2C_SCAN", "Scanning addresses 0x03 to 0x77...");

    for (uint8_t addr = 0x03; addr <= 0x77; addr++) {
    i2c_master_dev_handle_t dev_handle = NULL;
    i2c_device_config_t dev_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = addr,
        .scl_speed_hz = I2C_FREQ_HZ,
    };

    if (i2c_master_bus_add_device(i2c_bus, &dev_cfg, &dev_handle) == ESP_OK) {
            uint8_t dummy = 0x00;
            esp_err_t ret = i2c_master_transmit(dev_handle, &dummy, 1, 100);  // Send 1-byte dummy

            if (ret == ESP_OK) {
                ESP_LOGI(TAG, "Found device at 0x%02X", addr);
            }

            i2c_master_bus_rm_device(dev_handle); // clean up device
        }
    }
    
    i2c_del_master_bus(i2c_bus); // clean up bus

    ESP_LOGI("I2C_SCAN", "Scan complete.");
}

// Utility function to log with timestamp
void log_task_event(const char *taskName, const char *eventType, TickType_t periodTicks)
{
    TickType_t now = xTaskGetTickCount();
    unsigned long time_ms = now * portTICK_PERIOD_MS;
    unsigned long period_ms = periodTicks * portTICK_PERIOD_MS;

    printf("[%8lu ms] %s: %s | Period: %lu ms\n",
           time_ms, taskName, eventType, period_ms);
}

void Beacon_xmit_task(void *pvParameters)
{
    TickType_t lastWakeTime = xTaskGetTickCount(); // Initial timestamp

    while (1)
    {
        ledState = !ledState;
        gpio_set_level(LED_PIN, ledState);

        TickType_t now = xTaskGetTickCount();
        // printf("Beacon Transmit Task period: %lu ms\t", (unsigned long)((now - lastWakeTime) * portTICK_PERIOD_MS));
        log_task_event("XmitTask", ledState ? "LED ON" : "LED OFF", now - lastWakeTime);
        lastWakeTime = now;

        vTaskDelay(pdMS_TO_TICKS(SHORT_DURATION));
    }
}

void Monitor_task(void *pvParameters)
{
    TickType_t lastWakeTime = xTaskGetTickCount(); // Initial timestamp

    while (1)
    {
        TickType_t now = xTaskGetTickCount();
        // printf("\n");
        // printf("MonitorTask period: %lu ms | System time: %lu ms | LED = %s\n",
        //            (unsigned long)((now - lastWakeTime) * portTICK_PERIOD_MSpow(Rm , - GAMMA) * pow(10,-GAMMA) /RL10),
        //            (unsigned long)(now * portTICK_PERIOD_MS),
        //            ledState ? "ON" : "OFF");
        char msg[64];
        snprintf(msg, sizeof(msg), "Alive | LED = %s", ledState ? "ON" : "OFF");

        log_task_event("MonitorTask", msg, now - lastWakeTime);
        lastWakeTime = now;
        vTaskDelay(pdMS_TO_TICKS(LONG_DELAY));
        // vTaskDelay(pdMS_TO_TICKS(LONG_DELAY));
    }
}

void Sensor_task(void *arg)
{
    typedef struct {
        adc_channel_t channel;
        const char *name;
    } sensor_config_t;

    sensor_config_t sensors[2] = {
        {LDR_ADC_CHANNEL1, "Sensor1"},
        {LDR_ADC_CHANNEL2, "Sensor2"}
    };

    adc_oneshot_unit_handle_t adc_handle;

    adc_oneshot_unit_init_cfg_t init_cfg = {
        .unit_id = ADC_UNIT_1,
    };

    adc_oneshot_new_unit(&init_cfg, &adc_handle);

    adc_oneshot_chan_cfg_t chan_cfg = {
        .bitwidth = ADC_BITWIDTH_12,
        .atten = ADC_ATTEN_DB_12,
    };

    adc_cali_handle_t cali_handle = NULL;
    adc_cali_line_fitting_config_t cali_config = {
        .unit_id = ADC_UNIT_1,
        .atten = ADC_ATTEN_DB_12,
        .bitwidth = ADC_BITWIDTH_12,
    };

    ESP_ERROR_CHECK(adc_cali_create_scheme_line_fitting(&cali_config, &cali_handle));

    TickType_t lastWakeTime = xTaskGetTickCount();

    double rolling_max_dia = 0.0;
    double rolling_min_dia = 2.5; // start high to catch early min   
    double adcSumBuffer[LOG_BUFFER_SIZE] = {0};
    int bufferIndex = 0;
    int bufferFilled = 0;
    int oled_update_counter = 0;
    const int oversample_count = 16;

    TickType_t last_fluct_check = xTaskGetTickCount();
    
    while (1)
    {
        VoutSum = 0.0;

        for (int i = 0; i < 2; ++i)
        {
            int raw_total = 0;
            for (int j = 0; j < oversample_count; ++j) {
                int raw;
                adc_oneshot_config_channel(adc_handle, sensors[i].channel, &chan_cfg);
                ESP_ERROR_CHECK(adc_oneshot_read(adc_handle, sensors[i].channel, &raw));
                raw_total += raw;
            }
            int raw_avg = raw_total / oversample_count;
            int voltage;
            ESP_ERROR_CHECK(adc_cali_raw_to_voltage(cali_handle, raw_avg, &voltage));
            VoutSum += voltage;
        }

        float effective_slope;
        int32_t ref_raw1 = (user_raw_dia1 > 0) ? user_raw_dia1 : raw_dia1;
        int32_t ref_raw2 = (user_raw_dia2 > 0) ? user_raw_dia2 : raw_dia2;

        if (user_raw_dia1 > 0 && user_raw_dia2 > 0) {
            effective_slope = compute_slope(ref_raw1, ref_raw2);
        } else {
            effective_slope = fallback_slope;
        }

        double Diameter = cal_dia1 + effective_slope * (VoutSum - ref_raw1);

        if (Diameter > rolling_max_dia) rolling_max_dia = Diameter;
        if (Diameter < rolling_min_dia) rolling_min_dia = Diameter;

        adcSumBuffer[bufferIndex] = Diameter;
        bufferIndex = (bufferIndex + 1) % LOG_BUFFER_SIZE;
        if (bufferFilled < LOG_BUFFER_SIZE) bufferFilled++;

        double sum = 0.0, sumSq = 0.0;
        for (int j = 0; j < bufferFilled; ++j)
        {
            sum += adcSumBuffer[j];
            sumSq += adcSumBuffer[j] * adcSumBuffer[j];
        }

        double meanDia = sum / bufferFilled;
        double variance = (sumSq / bufferFilled) - (meanDia * meanDia);
        double stdDevDia = sqrt(variance);  // this turns out to be always 0.00000 -> eliminate from display ???

        if (meanDia < min_diameter)
        {
            log_task_event("ALERT", "⚠️ MEAN DIAMETER BELOW THRESHOLD!", 0);
            gpio_set_level(WARNING_LED_PIN, LED_ON);
        }
        else
        {
            gpio_set_level(WARNING_LED_PIN, LED_OFF);
        }

        char msg[128];
        snprintf(msg, sizeof(msg), "VoutSum=%.2f | Dia=%.4f | µ=%.4f | σ=%.4f",
                 VoutSum, Diameter, meanDia, stdDevDia);

        TickType_t now = xTaskGetTickCount();
        
        log_task_event("SensorSUM", msg, xTaskGetTickCount() - now);
       
        // OLED update once per second
        if (++oled_update_counter >= (1000 / ADC_DELAY))
        {
            oled_update_counter = 0;

            char line0[32], line1[32], line2[32], line3[32], line4[32], line6[32], line7[32];

            snprintf(line0, sizeof(line0), "DIA  : %.3f mm", Diameter);

            const char *cal_status;
            if (user_raw_dia1 > 0 && user_raw_dia2 > 0) {
                cal_status = "YES";
            } else if (user_raw_dia1 > 0) {
                cal_status = "1";
            } else if (user_raw_dia2 > 0) {
                cal_status = "2";
            } else {
                cal_status = "NO";
            }

            snprintf(line1, sizeof(line1), "Calibrated: %s", cal_status);
            snprintf(line2, sizeof(line2), "Mean : %.3f mm", meanDia);
            snprintf(line3, sizeof(line3), "Sigma: %.3f mm", stdDevDia);
            snprintf(line4, sizeof(line4), "Var  : %.3f", variance);  // may not be useful 

            // Warning evaluation
            bool diameter_warning = false;
            bool fluctuation_warning = false;

            if (Diameter < min_diameter || Diameter > max_diameter) {
                diameter_warning = true;
            }
            
            TickType_t now = xTaskGetTickCount();
        
            if ((now - last_fluct_check) >= pdMS_TO_TICKS(10000)) {
                double fluctuation = rolling_max_dia - rolling_min_dia;
                if (fluctuation > max_difference_diameter) {
                    fluctuation_warning = true;
                }

                // Reset for next 10 seconds
                rolling_max_dia = Diameter;
                rolling_min_dia = Diameter;
                last_fluct_check = now;
            }      
            
            bool invert_warning = false;

            if (diameter_warning) {
                snprintf(line7, sizeof(line6), "! OUT OF RANGE !");
                invert_warning = true;
            } else if (fluctuation_warning) {
                snprintf(line7, sizeof(line6), "! FLUCTUATION !");
                invert_warning = true;
            } else {
                snprintf(line7, sizeof(line6), "STATUS: OK");
            }

            ssd1306_clear_display(oled, false);
            ssd1306_display_text(oled, 0, line0, false);
            ssd1306_display_text(oled, 2, line1, false);
            ssd1306_display_text(oled, 3, line2, false);
            ssd1306_display_text(oled, 4, line3, false);
            ssd1306_display_text(oled, 5, line4, false);
            ssd1306_display_text(oled, 7, line7, invert_warning);
        }

        vTaskDelayUntil(&lastWakeTime, pdMS_TO_TICKS(ADC_DELAY));
    }

    adc_oneshot_del_unit(adc_handle);
    adc_cali_delete_scheme_line_fitting(cali_handle);
    vTaskDelete(NULL);
}

void CalibrateButton_task(void *arg)
{
    while (1)
    {
        if (xSemaphoreTake(xCalibButtonSem, portMAX_DELAY) == pdTRUE)
        {
            int64_t press_duration = (esp_timer_get_time() - button_press_time) / 1000;

            if (press_duration >= 5000) {
                // Very long press: clear calibration
                if (xSemaphoreTake(xLogMutex, pdMS_TO_TICKS(20))) {
                    user_raw_dia1 = -1;
                    user_raw_dia2 = -1;
                    xSemaphoreGive(xLogMutex);
                }
                ESP_LOGW(TAG, "[CALIBRATION] VERY LONG PRESS: Calibration CLEARED");
                
                char line1[32];
                if (oled) {

                    snprintf(line1, sizeof(line1), "Calibrated: RESET");
                    ssd1306_display_text(oled, 2, line1, false);
                }

            } else if (press_duration >= 1000) {
                // Long press = Step 2/2
                if (xSemaphoreTake(xLogMutex, pdMS_TO_TICKS(20))) {
                    user_raw_dia2 = (int32_t)VoutSum;
                    xSemaphoreGive(xLogMutex);
                }
                ESP_LOGI(TAG, "[CALIBRATION] Long press: raw_dia2 = %" PRId32, user_raw_dia2);

            } else {
                // Short press = Step 1/2
                if (xSemaphoreTake(xLogMutex, pdMS_TO_TICKS(20))) {
                    user_raw_dia1 = (int32_t)VoutSum;
                    xSemaphoreGive(xLogMutex);
                }
                ESP_LOGI(TAG, "[CALIBRATION] Short press: raw_dia1 = %" PRId32, user_raw_dia1);
            }
        }
    }
}

void app_main()
{
    esp_log_level_set(TAG, ESP_LOG_INFO);

    gpio_config_t io_conf = {
        .intr_type = GPIO_INTR_ANYEDGE,
        .mode = GPIO_MODE_INPUT,
        .pin_bit_mask = (1ULL << CALIBRATION_BUTTON_GPIO),
        .pull_up_en = GPIO_PULLUP_ENABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE
    };
    gpio_config(&io_conf);
    
    // Create debounce timer
    setup_debounce_timer(&debounce_timer, debounce_timer_callback, "buttonDebounce");
 
    // Create synchronization primitives
    xLogMutex = xSemaphoreCreateMutex();
    xCalibButtonSem = xSemaphoreCreateBinary();
    assert(xLogMutex && xCalibButtonSem);

    // Button setup for isr
    gpio_install_isr_service(0);

    ESP_LOGV(TAG, "configuring button\n");
    //gpio_reset_pin(CALIBRATION_BUTTON_GPIO);
    //gpio_set_direction(CALIBRATION_BUTTON_GPIO, GPIO_MODE_INPUT);
    //gpio_pullup_en(CALIBRATION_BUTTON_GPIO);
    //gpio_set_intr_type(CALIBRATION_BUTTON_GPIO, GPIO_INTR_ANYEDGE);
    gpio_isr_handler_add(CALIBRATION_BUTTON_GPIO, calibration_button_isr_handler, NULL);

    // LED setup
    gpio_reset_pin(LED_PIN);
    gpio_reset_pin(WARNING_LED_PIN);

    gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);
    gpio_set_direction(WARNING_LED_PIN, GPIO_MODE_OUTPUT);

    // Initially start with all LED_OFF
    gpio_set_level(LED_PIN, ledState);
    gpio_set_level(WARNING_LED_PIN, ledState); // Ensure it starts in the OFF state

    // Setup Analog inputs
    gpio_reset_pin(ANALOG_PIN1);
    gpio_set_direction(ANALOG_PIN1, GPIO_MODE_INPUT); // configure input for analogRead
    // adc1_config_width(ADC_WIDTH_BIT_12); // configure ADC 12-bit representation between 0 and 2^12-1=4095
    // adc1_config_channel_atten(LDR_ADC_CHANNEL1, ADC_ATTEN_DB_11)
    gpio_reset_pin(ANALOG_PIN2);
    gpio_set_direction(ANALOG_PIN2, GPIO_MODE_INPUT); // configure input for analogRead
    // adc1_config_width(ADC_WIDTH_BIT_12); // configure ADC 12-bit representation between 0 and 2^12-1=4095
    // adc1_config_channel_atten(LDR_ADC_CHANNEL2, ADC_ATTEN_DB_11)
    
    // scan for oled, initialize and clear display
    i2c_scan_ng();
    i2c_master_init();   
    oled_init();

    if (oled == NULL) {
        ESP_LOGI(TAG, "ssd1306 handle init failed");
        assert(oled);
    }

    ssd1306_clear_display(oled,false);
    ssd1306_display_text(oled, 0, "OLED INITIALIZED !",false);

    ESP_LOGI(TAG, "Display updated.");

    // Optional contrast setting
    ssd1306_set_contrast(oled, 0xFF);

    // === TASK STARTUP ===
    xTaskCreatePinnedToCore(Beacon_xmit_task, "BeaconXmit_task", 2048, NULL, priorityX, NULL, 1);
    xTaskCreatePinnedToCore(Monitor_task, "Monitor_task", 2048, NULL, priorityY, NULL, 1);
    xTaskCreatePinnedToCore(Sensor_task, "Sensor_task", 4096, NULL, priorityZ, &ADCTaskHandle, 1);
    xTaskCreatePinnedToCore(CalibrateButton_task, "Calibrate_task", 4096, NULL, priorityT,NULL, 1);

    ESP_LOGI(TAG, "System ready. Press the button to dump the log.");
}