/*
 * Dual button input with LED output and temperature set control + HVAC mode control
 * GPIO 11: Increase button input (with internal pull-down)
 * GPIO 12: Decrease button input (with internal pull-down)
 * GPIO 2: Increase LED output
 * GPIO 4: Decrease LED output
 * GPIO 13: Heating mode switch (with internal pull-down)
 * GPIO 14: Cooling mode switch (with internal pull-down)
 * GPIO 3: TMP36 temperature sensor (ADC input)
 * When increase button is pressed, increase LED turns on and temperature set increments
 * When decrease button is pressed, decrease LED turns on and temperature set decrements
 * Switch positions determine HVAC mode: OFF, HEATING, COOLING
 * TMP36 sensor provides actual temperature readings every 5 seconds
 */
#include <driver/gpio.h>
#include "esp_adc/adc_oneshot.h"
#include "esp_adc/adc_cali.h"
#include "esp_adc/adc_cali_scheme.h"
#include <esp_log.h>
#include <esp_err.h>
#include <freertos/FreeRTOS.h>
#include <freertos/queue.h>
#include <freertos/task.h>

#include "sdkconfig.h"

static char tag[] = "temperature_set";
static QueueHandle_t q1;

#define INCREMENT_TEMPERATURE_SET_BUTTON_GPIO (11)
#define DECREMENT_TEMPERATURE_SET_BUTTON_GPIO (12)
#define INCREASE_LED_GPIO (2)
#define DECREASE_LED_GPIO (4)

#define HEATING_SET_SWITCH_GPIO (13)
#define COOLING_SET_SWITCH_GPIO (14)
// HVAC indicator LEDs
#define HEATING_LED_GPIO (15)    // Heating mode indicator LED
#define COOLING_LED_GPIO (16)    // Cooling mode indicator LED

// HVAC system control outputs (relays/contactors)
#define HEATING_RELAY_GPIO (18)  // Heating system relay control
#define COOLING_RELAY_GPIO (17)  // Cooling system relay control

// Ventilation fan
#define VENTILATION_FAN_BUTTON_GPIO (9)
#define VENTILATION_FAN_RELAY_GPIO (10)

// TMP36 Temperature sensor on ADC
#define TMP36_ADC_CHANNEL ADC_CHANNEL_2  // GPIO 3 corresponds to ADC_CHANNEL_2
#define TMP36_ADC_ATTEN ADC_ATTEN_DB_12   // 0-3.9V range
//#define TMP36_ADC_WIDTH ADC_WIDTH_BIT_12  // 12-bit resolution (0-4095)

// Temperature sensor configuration
#define TEMP_DEADBAND 1          // Temperature deadband in degrees F
#define DEFAULT_VREF 1100        // Default reference voltage in mV
#define TEMP_SAMPLES 10          // Number of samples to average

typedef enum {
    HVAC_MODE_OFF,
    HVAC_MODE_HEATING,
    HVAC_MODE_COOLING
} hvac_mode_t;

typedef enum {
    SYSTEM_OFF,
    SYSTEM_HEATING,
    SYSTEM_COOLING
} system_state_t;

// Global variables
static int temperature_set = 73;
static int current_temperature = 73;  // Will be updated by TMP36 readings
static hvac_mode_t current_hvac_mode = HVAC_MODE_OFF;
static system_state_t current_system_state = SYSTEM_OFF;
static bool is_fan_on = false;

// ADC
static int adc_raw[2][10];
static int voltage[2][10];
adc_oneshot_unit_handle_t adc1_handle;
adc_cali_handle_t adc1_cali_tmp36_chan_handle = NULL;
bool do_calibration1_tmp36_chan = false;

// ADC calibration
//static esp_adc_cal_characteristics_t *adc_chars;

static void IRAM_ATTR button_isr_handler(void *args) {
    gpio_num_t gpio = (gpio_num_t)(int)args;
    
    // Disable interrupt immediately to prevent bounce
    gpio_intr_disable(gpio);
    
    xQueueSendToBackFromISR(q1, &gpio, NULL);
}

static void IRAM_ATTR switch_isr_handler(void *args) {
    gpio_num_t gpio = (gpio_num_t)(int)args;

    // Disable interrupt immediately to prevent bounce
    gpio_intr_disable(gpio);

    xQueueSendToBackFromISR(q1, &gpio, NULL);  
}

/*---------------------------------------------------------------
        ADC Calibration
---------------------------------------------------------------*/
static bool ckoch786_adc_calibration_init(adc_unit_t unit, adc_channel_t channel, adc_atten_t atten, adc_cali_handle_t *out_handle)
{
    adc_cali_handle_t handle = NULL;
    esp_err_t ret = ESP_FAIL;
    bool calibrated = false;

// TODO The S3 supports this option, not sure why when attempting to build/flash this without the defines it fails to find the curve_fitting_ stuff
#if ADC_CALI_SCHEME_CURVE_FITTING_SUPPORTED
    if (!calibrated)
    {
        ESP_LOGI(tag, "calibration scheme version is %s", "Curve Fitting");
        adc_cali_curve_fitting_config_t cali_config = {
            .unit_id = unit,
            .chan = channel,
            .atten = atten,
            .bitwidth = ADC_BITWIDTH_DEFAULT,
        };
        ret = adc_cali_create_scheme_curve_fitting(&cali_config, &handle);
        if (ret == ESP_OK)
        {
            calibrated = true;
        }
    }
#endif

#if ADC_CALI_SCHEME_LINE_FITTING_SUPPORTED
    if (!calibrated)
    {
        ESP_LOGI(tag, "calibration scheme version is %s", "Line Fitting");
        adc_cali_line_fitting_config_t cali_config = {
            .unit_id = unit,
            .atten = atten,
            .bitwidth = ADC_BITWIDTH_DEFAULT,
        };
        ret = adc_cali_create_scheme_line_fitting(&cali_config, &handle);
        if (ret == ESP_OK)
        {
            calibrated = true;
        }
    }
#endif

    *out_handle = handle;
    if (ret == ESP_OK)
    {
        ESP_LOGI(tag, "Calibration Success");
        printf("Calibration Success\n");
    }
    else if (ret == ESP_ERR_NOT_SUPPORTED || !calibrated)
    {
        ESP_LOGW(tag, "eFuse not burnt, skip software calibration");
        printf("eFuse not burnt, skip software calibration\n");
    }
    else
    {
        ESP_LOGE(tag, "Invalid arg or no memory");
        printf("Invalid arg or no memory\n");
    }

    return calibrated;
}

static void ckoch786_adc_calibration_deinit(adc_cali_handle_t handle)
{
#if ADC_CALI_SCHEME_CURVE_FITTING_SUPPORTED
    ESP_LOGI(tag, "deregister %s calibration scheme", "Curve Fitting");
    //ESP_ERROR_CHECK(adc_cali_delete_scheme_curve_fitting(handle));
    adc_cali_delete_scheme_curve_fitting(handle);

#elif ADC_CALI_SCHEME_LINE_FITTING_SUPPORTED
    ESP_LOGI(tag, "deregister %s calibration scheme", "Line Fitting");
    //ESP_ERROR_CHECK(adc_cali_delete_scheme_line_fitting(handle));
    adc_cali_delete_scheme_line_fitting(handle);
#endif
}

static void tmp36_adc_setup() {
    // Configure ADC1
    // adc1_config_width(TMP36_ADC_WIDTH);
    // adc1_config_channel_atten(TMP36_ADC_CHANNEL, TMP36_ADC_ATTEN);

    // // Characterize ADC
    // adc_chars = calloc(1, sizeof(esp_adc_cal_characteristics_t));
    // esp_adc_cal_value_t val_type = esp_adc_cal_characterize(ADC_UNIT_1, TMP36_ADC_ATTEN, TMP36_ADC_WIDTH, DEFAULT_VREF, adc_chars);
    
    // if (val_type == ESP_ADC_CAL_VAL_EFUSE_VREF) {
    //     ESP_LOGI(tag, "ADC characterized using eFuse Vref");
    // } else if (val_type == ESP_ADC_CAL_VAL_EFUSE_TP) {
    //     ESP_LOGI(tag, "ADC characterized using Two Point Value");
    // } else {
    //     ESP_LOGI(tag, "ADC characterized using Default Vref");
    // }

    // ADC1 Init
    
    adc_oneshot_unit_init_cfg_t init_config = {
      .unit_id = ADC_UNIT_1,
    };
    //ESP_ERROR_CHECK(adc_oneshot_new_unit(&init_config, &adc1_handle));
    adc_oneshot_new_unit(&init_config, &adc1_handle);

    // ADC1 Config
    adc_oneshot_chan_cfg_t config = {
      .atten = TMP36_ADC_ATTEN,
      .bitwidth = ADC_BITWIDTH_DEFAULT,
    };
    //ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, TMP36_ADC_CHANNEL, &config));
    adc_oneshot_config_channel(adc1_handle, TMP36_ADC_CHANNEL, &config);

    // ADC1 Calibration Init
    adc1_cali_tmp36_chan_handle = NULL;
    do_calibration1_tmp36_chan = ckoch786_adc_calibration_init(ADC_UNIT_1, TMP36_ADC_CHANNEL, TMP36_ADC_ATTEN , &adc1_cali_tmp36_chan_handle);
}

static int read_temperature() {
    // ESP_ERROR_CHECK(adc_oneshot_read(adc1_handle, TMP36_ADC_CHANNEL, &adc_raw[0][0]);
    adc_oneshot_read(adc1_handle, TMP36_ADC_CHANNEL, &adc_raw[0][0]);
    if (do_calibration1_tmp36_chan) {
      // ESP_ERROR_CHECK(adc_cali_raw_to_voltage(adc1_cali_tmp36_chan_handle, adc_raw[0][0], &voltage[0][0]));
      adc_cali_raw_to_voltage(adc1_cali_tmp36_chan_handle, adc_raw[0][0], &voltage[0][0]);
      ESP_LOGI(tag, "Temperature Sensor ADC%d Channel[%d] Cali Voltage: %d mV", ADC_UNIT_1 + 1, TMP36_ADC_CHANNEL, voltage[0][0]); 
      printf("Temperature Sensor ADC%d Channel[%d] Cali Voltage: %d mV", ADC_UNIT_1 + 1, TMP36_ADC_CHANNEL, voltage[0][0]);
    }

    return voltage;

    // uint32_t adc_reading = 0;
    
    // // Take multiple samples and average them
    // for (int i = 0; i < TEMP_SAMPLES; i++) {
    //     adc_reading += adc1_get_raw(TMP36_ADC_CHANNEL);
    // }
    // adc_reading /= TEMP_SAMPLES;
    
    // // Convert ADC reading to voltage in mV
    // uint32_t voltage_mv = esp_adc_cal_raw_to_voltage(adc_reading, adc_chars);
    
    // // Convert voltage to temperature
    // // TMP36 formula: Voltage (mV) = (Temperature °C × 10) + 500
    // // Rearranged: Temperature °C = (Voltage (mV) - 500) / 10
    // float temp_celsius = (voltage_mv - 500.0) / 10.0;
    
    // // Convert Celsius to Fahrenheit
    // float temp_fahrenheit = (temp_celsius * 9.0 / 5.0) + 32.0;
    
    // // Round to nearest integer
    // int temp_f = (int)(temp_fahrenheit + 0.5);
    
    // // Log detailed readings for debugging
    // ESP_LOGD(tag, "ADC Raw: %zu, Voltage: %zu mV, Temp: %.1f°C, %.1f°F", 
    //          (size_t)adc_reading, (size_t)voltage_mv, temp_celsius, temp_fahrenheit);
    
    // return temp_f;
}

static void IncreaseSetTempButtonSetup() {
    // Configure increase button GPIO (input with pull-down)
    gpio_config_t buttonConfig;
    buttonConfig.pin_bit_mask = (1ULL << INCREMENT_TEMPERATURE_SET_BUTTON_GPIO);
    buttonConfig.mode         = GPIO_MODE_INPUT;
    buttonConfig.pull_up_en   = GPIO_PULLUP_DISABLE;
    buttonConfig.pull_down_en = GPIO_PULLDOWN_ENABLE;
    buttonConfig.intr_type    = GPIO_INTR_POSEDGE;  // Trigger on rising edge
    gpio_config(&buttonConfig);
}

static void DecreaseSetTempButtonSetup() {
    // Configure decrease button GPIO (input with pull-down)
    gpio_config_t buttonConfig;
    buttonConfig.pin_bit_mask = (1ULL << DECREMENT_TEMPERATURE_SET_BUTTON_GPIO);
    buttonConfig.mode         = GPIO_MODE_INPUT;
    buttonConfig.pull_up_en   = GPIO_PULLUP_DISABLE;
    buttonConfig.pull_down_en = GPIO_PULLDOWN_ENABLE;
    buttonConfig.intr_type    = GPIO_INTR_POSEDGE;  // Trigger on rising edge
    gpio_config(&buttonConfig);
}

static void IncreaseSetTempLEDIndicatorSetup() {
    // Configure increase LED GPIO (output)
    gpio_config_t ledConfig;
    ledConfig.pin_bit_mask = (1ULL << INCREASE_LED_GPIO);
    ledConfig.mode         = GPIO_MODE_OUTPUT;
    ledConfig.pull_up_en   = GPIO_PULLUP_DISABLE;
    ledConfig.pull_down_en = GPIO_PULLDOWN_DISABLE;
    ledConfig.intr_type    = GPIO_INTR_DISABLE;
    gpio_config(&ledConfig);

    // Start with LED off
    gpio_set_level(INCREASE_LED_GPIO, 0);
}

static void DecreaseSetTempLEDIndicatorSetup() {
    // Configure decrease LED GPIO (output)
    gpio_config_t ledConfig;
    ledConfig.pin_bit_mask = (1ULL << DECREASE_LED_GPIO);
    ledConfig.mode         = GPIO_MODE_OUTPUT;
    ledConfig.pull_up_en   = GPIO_PULLUP_DISABLE;
    ledConfig.pull_down_en = GPIO_PULLDOWN_DISABLE;
    ledConfig.intr_type    = GPIO_INTR_DISABLE;
    gpio_config(&ledConfig);

    // Start with LED off
    gpio_set_level(DECREASE_LED_GPIO, 0);
}

static void HeatingOnSwitchSetup() {
    gpio_config_t switchConfig;
    switchConfig.pin_bit_mask = (1ULL << HEATING_SET_SWITCH_GPIO);
    switchConfig.mode         = GPIO_MODE_INPUT;
    switchConfig.pull_up_en   = GPIO_PULLUP_DISABLE;
    switchConfig.pull_down_en = GPIO_PULLDOWN_ENABLE;
    switchConfig.intr_type    = GPIO_INTR_ANYEDGE;  // Trigger on both edges
    gpio_config(&switchConfig);
}

static void CoolingOnSwitchSetup() {
    gpio_config_t switchConfig;
    switchConfig.pin_bit_mask = (1ULL << COOLING_SET_SWITCH_GPIO);
    switchConfig.mode         = GPIO_MODE_INPUT;
    switchConfig.pull_up_en   = GPIO_PULLUP_DISABLE;
    switchConfig.pull_down_en = GPIO_PULLDOWN_ENABLE;
    switchConfig.intr_type    = GPIO_INTR_ANYEDGE;  // Trigger on both edges
    gpio_config(&switchConfig);
}

static void HVACLEDSetup() {
    // Configure HVAC indicator LEDs
    gpio_config_t ledConfig;
    ledConfig.pin_bit_mask = (1ULL << HEATING_LED_GPIO) | (1ULL << COOLING_LED_GPIO);
    ledConfig.mode         = GPIO_MODE_OUTPUT;
    ledConfig.pull_up_en   = GPIO_PULLUP_DISABLE;
    ledConfig.pull_down_en = GPIO_PULLDOWN_DISABLE;
    ledConfig.intr_type    = GPIO_INTR_DISABLE;
    gpio_config(&ledConfig);

    // Start with LEDs off
    gpio_set_level(HEATING_LED_GPIO, 0);
    gpio_set_level(COOLING_LED_GPIO, 0);
}

static void HVACRelaySetup() {
    // Configure HVAC system relay outputs
    gpio_config_t relayConfig;
    relayConfig.pin_bit_mask = (1ULL << HEATING_RELAY_GPIO) | (1ULL << COOLING_RELAY_GPIO);
    relayConfig.mode         = GPIO_MODE_OUTPUT;
    relayConfig.pull_up_en   = GPIO_PULLUP_DISABLE;
    relayConfig.pull_down_en = GPIO_PULLDOWN_DISABLE;
    relayConfig.intr_type    = GPIO_INTR_DISABLE;
    gpio_config(&relayConfig);

    // Start with relays off (safety first)
    gpio_set_level(HEATING_RELAY_GPIO, 0);
    gpio_set_level(COOLING_RELAY_GPIO, 0);
}

static void VentilationFanRelaySetup() {
    gpio_config_t relayConfig;
    relayConfig.pin_bit_mask = (1ULL << VENTILATION_FAN_RELAY_GPIO);
    relayConfig.mode         = GPIO_MODE_OUTPUT;
    relayConfig.pull_up_en   = GPIO_PULLUP_DISABLE;
    relayConfig.pull_down_en = GPIO_PULLDOWN_DISABLE;
    relayConfig.intr_type    = GPIO_INTR_DISABLE;
    gpio_config(&relayConfig);

    // Start with relays off (safety first)
    gpio_set_level(VENTILATION_FAN_RELAY_GPIO, 0);
}

static void VentilationFanButtonButtonSetup() {
    // Configure increase button GPIO (input with pull-down)
    gpio_config_t buttonConfig;
    buttonConfig.pin_bit_mask = (1ULL << VENTILATION_FAN_BUTTON_GPIO);
    buttonConfig.mode         = GPIO_MODE_INPUT;
    buttonConfig.pull_up_en   = GPIO_PULLUP_DISABLE;
    buttonConfig.pull_down_en = GPIO_PULLDOWN_ENABLE;
    buttonConfig.intr_type    = GPIO_INTR_POSEDGE;  // Trigger on rising edge
    gpio_config(&buttonConfig);
}

static void control_hvac_system() {
    // Read current temperature from TMP36 sensor
    current_temperature = read_temperature();
    
    system_state_t new_system_state = SYSTEM_OFF;
    
    // Only control system if a mode is selected
    if (current_hvac_mode != HVAC_MODE_OFF) {
        
        if (current_hvac_mode == HVAC_MODE_HEATING) {
            // Heating logic with deadband
            if (current_system_state == SYSTEM_HEATING) {
                // Currently heating - turn off when we reach setpoint
                if (current_temperature >= temperature_set) {
                    new_system_state = SYSTEM_OFF;
                } else {
                    new_system_state = SYSTEM_HEATING;  // Keep heating
                }
            } else {
                // Not currently heating - turn on if below setpoint minus deadband
                if (current_temperature < (temperature_set - TEMP_DEADBAND)) {
                    new_system_state = SYSTEM_HEATING;
                } else {
                    new_system_state = SYSTEM_OFF;
                }
            }
        } 
        else if (current_hvac_mode == HVAC_MODE_COOLING) {
            // Cooling logic with deadband
            if (current_system_state == SYSTEM_COOLING) {
                // Currently cooling - turn off when we reach setpoint
                if (current_temperature <= temperature_set) {
                    new_system_state = SYSTEM_OFF;
                } else {
                    new_system_state = SYSTEM_COOLING;  // Keep cooling
                }
            } else {
                // Not currently cooling - turn on if above setpoint plus deadband
                if (current_temperature > (temperature_set + TEMP_DEADBAND)) {
                    new_system_state = SYSTEM_COOLING;
                } else {
                    new_system_state = SYSTEM_OFF;
                }
            }
        }
    }
    
    // Update system state and relays if changed
    if (new_system_state != current_system_state) {
        current_system_state = new_system_state;
        
        // Control the relay outputs
        switch (current_system_state) {
            case SYSTEM_HEATING:
                gpio_set_level(HEATING_RELAY_GPIO, 1);
                gpio_set_level(COOLING_RELAY_GPIO, 0);
                ESP_LOGI(tag, "HEATING SYSTEM ON - Current: %d°F, Set: %d°F", current_temperature, temperature_set);
                printf("🔥 HEATING SYSTEM ON - Current: %d°F, Set: %d°F\n", current_temperature, temperature_set);
                break;
                
            case SYSTEM_COOLING:
                gpio_set_level(HEATING_RELAY_GPIO, 0);
                gpio_set_level(COOLING_RELAY_GPIO, 1);
                ESP_LOGI(tag, "COOLING SYSTEM ON - Current: %d°F, Set: %d°F", current_temperature, temperature_set);
                printf("❄️  COOLING SYSTEM ON - Current: %d°F, Set: %d°F\n", current_temperature, temperature_set);
                break;
                
            case SYSTEM_OFF:
                gpio_set_level(HEATING_RELAY_GPIO, 0);
                gpio_set_level(COOLING_RELAY_GPIO, 0);
                ESP_LOGI(tag, "HVAC SYSTEM OFF - Current: %d°F, Set: %d°F", current_temperature, temperature_set);
                printf("⏸️  HVAC SYSTEM OFF - Current: %d°F, Set: %d°F\n", current_temperature, temperature_set);
                break;
        }
    }
}

static void temperature_control_task(void *pvParameters) {
    TickType_t last_wake_time = xTaskGetTickCount();
    
    while (1) {
        // Run temperature control every 5 seconds
        control_hvac_system();
        
        // Log current status every minute (12 cycles)
        static int log_counter = 0;
        if (++log_counter >= 12) {
            log_counter = 0;
            const char* mode_str = (current_hvac_mode == HVAC_MODE_HEATING) ? "HEAT" : 
                                   (current_hvac_mode == HVAC_MODE_COOLING) ? "COOL" : "OFF";
            const char* system_str = (current_system_state == SYSTEM_HEATING) ? "HEATING" : 
                                     (current_system_state == SYSTEM_COOLING) ? "COOLING" : "OFF";
            
            ESP_LOGI(tag, "Status - Mode: %s, System: %s, Current: %d°F, Set: %d°F", 
                     mode_str, system_str, current_temperature, temperature_set);
        }
        
        // Wait for next cycle (5 seconds)
        vTaskDelayUntil(&last_wake_time, pdMS_TO_TICKS(5000));
    }
}

static void update_hvac_mode() {
    int heating_level = gpio_get_level(HEATING_SET_SWITCH_GPIO);
    int cooling_level = gpio_get_level(COOLING_SET_SWITCH_GPIO);
    
    hvac_mode_t new_mode = HVAC_MODE_OFF;
    
    // Determine mode based on switch positions (active-HIGH logic)
    if (heating_level == 1 && cooling_level == 0) {
        new_mode = HVAC_MODE_HEATING;
    } else if (heating_level == 0 && cooling_level == 1) {
        new_mode = HVAC_MODE_COOLING;
    } else {
        new_mode = HVAC_MODE_OFF;  // Both off or both on = off mode
    }
    
    // Update mode if changed
    if (new_mode != current_hvac_mode) {
        current_hvac_mode = new_mode;
        
        // Update HVAC mode indicator LEDs
        gpio_set_level(HEATING_LED_GPIO, (current_hvac_mode == HVAC_MODE_HEATING) ? 1 : 0);
        gpio_set_level(COOLING_LED_GPIO, (current_hvac_mode == HVAC_MODE_COOLING) ? 1 : 0);
        
        // Force immediate temperature control update
        control_hvac_system();
        
        switch (current_hvac_mode) {
            case HVAC_MODE_HEATING:
                ESP_LOGI(tag, "HVAC Mode: HEATING ENABLED (Set: %d°F, Current: %d°F)", temperature_set, current_temperature);
                printf("HVAC Mode: HEATING ENABLED (Set: %d°F, Current: %d°F)\n", temperature_set, current_temperature);
                break;
            case HVAC_MODE_COOLING:
                ESP_LOGI(tag, "HVAC Mode: COOLING ENABLED (Set: %d°F, Current: %d°F)", temperature_set, current_temperature);
                printf("HVAC Mode: COOLING ENABLED (Set: %d°F, Current: %d°F)\n", temperature_set, current_temperature);
                break;
            case HVAC_MODE_OFF:
                ESP_LOGI(tag, "HVAC Mode: DISABLED");
                printf("HVAC Mode: DISABLED\n");
                break;
        }
    }
}

void app_main() {
    ESP_LOGI(tag, "Starting HVAC temperature control application with TMP36 sensor");
    gpio_num_t gpio;
    
    // Create queue to handle interrupt events
    q1 = xQueueCreate(10, sizeof(gpio_num_t));

    // Setup all GPIO
    IncreaseSetTempButtonSetup();
    DecreaseSetTempButtonSetup();
    IncreaseSetTempLEDIndicatorSetup();
    DecreaseSetTempLEDIndicatorSetup();
    HeatingOnSwitchSetup();
    CoolingOnSwitchSetup();
    HVACLEDSetup();
    HVACRelaySetup();
    VentilationFanButtonButtonSetup();
    VentilationFanRelaySetup();
    
    // Setup TMP36 temperature sensor ADC
    tmp36_adc_setup();

    // Install interrupt service and add handlers
    gpio_install_isr_service(0);
    gpio_isr_handler_add(INCREMENT_TEMPERATURE_SET_BUTTON_GPIO, button_isr_handler, (void*)INCREMENT_TEMPERATURE_SET_BUTTON_GPIO);
    gpio_isr_handler_add(DECREMENT_TEMPERATURE_SET_BUTTON_GPIO, button_isr_handler, (void*)DECREMENT_TEMPERATURE_SET_BUTTON_GPIO);
    gpio_isr_handler_add(HEATING_SET_SWITCH_GPIO, switch_isr_handler, (void*)HEATING_SET_SWITCH_GPIO);
    gpio_isr_handler_add(COOLING_SET_SWITCH_GPIO, switch_isr_handler, (void*)COOLING_SET_SWITCH_GPIO);
    gpio_isr_handler_add(VENTILATION_FAN_BUTTON_GPIO, button_isr_handler, (void*)VENTILATION_FAN_BUTTON_GPIO);

    // Initial HVAC mode check
    update_hvac_mode();
    
    // Take initial temperature reading
    current_temperature = read_temperature();
    ESP_LOGI(tag, "Initial temperature reading: %d°F", current_temperature);

    ESP_LOGI(tag, "System ready. Use buttons to adjust temperature and switches for HVAC mode");
    ESP_LOGI(tag, "Switch Logic: Heating pin 13 HIGH = Heat, Cooling pin 14 HIGH = Cool");
    ESP_LOGI(tag, "Relay Outputs: Pin 17 = Heating, Pin 18 = Cooling");
    ESP_LOGI(tag, "Temperature deadband: ±%d°F", TEMP_DEADBAND);
    ESP_LOGI(tag, "TMP36 sensor connected to GPIO 3 (ADC1_CHANNEL_2)");

    // Create temperature control task
    xTaskCreate(temperature_control_task, "temp_control", 2048, NULL, 5, NULL);

    while(1) {
        ESP_LOGI(tag, "Waiting for input... (Set: %d°F, Current: %d°F, Mode: %s, Fan: %s)", 
                 temperature_set, 
                 current_temperature,
                 (current_hvac_mode == HVAC_MODE_HEATING) ? "HEAT" : 
                 (current_hvac_mode == HVAC_MODE_COOLING) ? "COOL" : "OFF",
                 is_fan_on ? "ON" : "OFF");
        
        // Wait for interrupt
        BaseType_t rc = xQueueReceive(q1, &gpio, portMAX_DELAY);
        
        if (rc == pdTRUE) {
            if (gpio == INCREMENT_TEMPERATURE_SET_BUTTON_GPIO) {
                // Increment temperature set
                temperature_set++;
                
                // Turn on increase LED briefly
                gpio_set_level(INCREASE_LED_GPIO, 1);
                
                printf("Increase button pressed! Temperature set: %d°F (Current: %d°F)\n", temperature_set, current_temperature);
                ESP_LOGI(tag, "Temperature increased to: %d°F", temperature_set);
                
                // Update HVAC control immediately when setpoint changes
                control_hvac_system();
                
                // Brief LED flash
                vTaskDelay(50 / portTICK_PERIOD_MS);
                gpio_set_level(INCREASE_LED_GPIO, 0);
                
                // Wait for button release and debounce
                while(gpio_get_level(INCREMENT_TEMPERATURE_SET_BUTTON_GPIO) == 1) {
                    vTaskDelay(10 / portTICK_PERIOD_MS);
                }
                vTaskDelay(30 / portTICK_PERIOD_MS);
                
                // Re-enable interrupt
                gpio_intr_enable(INCREMENT_TEMPERATURE_SET_BUTTON_GPIO);
                
            } else if (gpio == DECREMENT_TEMPERATURE_SET_BUTTON_GPIO) {
                // Decrement temperature set
                temperature_set--;
                
                // Turn on decrease LED briefly
                gpio_set_level(DECREASE_LED_GPIO, 1);
                
                printf("Decrease button pressed! Temperature set: %d°F (Current: %d°F)\n", temperature_set, current_temperature);
                ESP_LOGI(tag, "Temperature decreased to: %d°F", temperature_set);
                
                // Update HVAC control immediately when setpoint changes
                control_hvac_system();
                
                // Brief LED flash
                vTaskDelay(50 / portTICK_PERIOD_MS);
                gpio_set_level(DECREASE_LED_GPIO, 0);
                
                // Wait for button release and debounce
                while(gpio_get_level(DECREMENT_TEMPERATURE_SET_BUTTON_GPIO) == 1) {
                    vTaskDelay(10 / portTICK_PERIOD_MS);
                }
                vTaskDelay(30 / portTICK_PERIOD_MS);
                
                // Re-enable interrupt
                gpio_intr_enable(DECREMENT_TEMPERATURE_SET_BUTTON_GPIO);
                
            } else if (gpio == HEATING_SET_SWITCH_GPIO || gpio == COOLING_SET_SWITCH_GPIO) {
                // HVAC switch changed - update mode
                vTaskDelay(50 / portTICK_PERIOD_MS);  // Debounce delay
                update_hvac_mode();
                
                // Re-enable interrupts for both switches
                gpio_intr_enable(HEATING_SET_SWITCH_GPIO);
                gpio_intr_enable(COOLING_SET_SWITCH_GPIO);
            } else if (gpio == VENTILATION_FAN_BUTTON_GPIO) {
                is_fan_on = !is_fan_on;
                
                // Turn on increase LED briefly
                gpio_set_level(VENTILATION_FAN_RELAY_GPIO, is_fan_on ? 1 : 0);
                
                printf("Ventilation Fan power button touched %s \n", is_fan_on ? "ON" : "OFF");
                ESP_LOGI(tag, "Ventilation Fan is %s", is_fan_on ? "ON" : "OFF");

                // Wait for button release and debounce
                while(gpio_get_level(VENTILATION_FAN_BUTTON_GPIO) == 1) {
                    vTaskDelay(10 / portTICK_PERIOD_MS);
                }
                vTaskDelay(30 / portTICK_PERIOD_MS);
                
                // Re-enable interrupt
                gpio_intr_enable(VENTILATION_FAN_BUTTON_GPIO);
            }
        }
    }

    // Tear Down
    ESP_ERROR_CHECK(adc_oneshot_del_unit(adc1_handle));
    if (do_calibration1_tmp36_chan) {
      ckoch786_adc_calibration_deinit(adc1_cali_tmp36_chan_handle);
    }
    
    vTaskDelete(NULL);
}