#include <stdio.h>
#include <stdbool.h>
#include "pico/stdlib.h"
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "hardware/i2c.h"
#include "hardware/gpio.h"

// Assume we have these libraries for LCD and DHT22
#include "lcd.h"
#include "dht22.h"

// Pin definitions
#define LED_PIN 16
#define YELLOW_LED_PIN 18
#define PIR_PIN 17
#define DHT_PIN 15

#define I2C_SDA_PIN 4
#define I2C_SCL_PIN 5

// LCD settings
#define LCD_ROWS 4
#define LCD_COLS 20

// Global variables for shared data
typedef struct {
    float temperature;
    float humidity;
    bool motion;
} SensorData;

SensorData sensor_data = {.temperature = 0, .humidity = 0, .motion = false};
SemaphoreHandle_t data_mutex;

LCD_HandleTypeDef lcd;
DHT22_HandleTypeDef dht22;

// Function prototypes
void sensor_reading_task(void *pvParameters);
void lcd_update_task(void *pvParameters);
void led_blink_task(void *pvParameters);
void yellow_led_blink_task(void *pvParameters);

int main() {
    stdio_init_all();

    // Initialize hardware
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
    gpio_init(YELLOW_LED_PIN);
    gpio_set_dir(YELLOW_LED_PIN, GPIO_OUT);
    gpio_init(PIR_PIN);
    gpio_set_dir(PIR_PIN, GPIO_IN);

    // Initialize I2C
    i2c_init(i2c0, 400000);
    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);

    // Initialize LCD (assuming a function like this exists)
    lcd_init(&lcd, i2c0, LCD_ROWS, LCD_COLS);

    // Initialize DHT22 (assuming a function like this exists)
    dht22_init(&dht22, DHT_PIN);

    // Create mutex
    data_mutex = xSemaphoreCreateMutex();

    // Create tasks
    xTaskCreate(sensor_reading_task, "Sensor_Reading_Task", configMINIMAL_STACK_SIZE, NULL, 1, NULL);
    xTaskCreate(lcd_update_task, "LCD_Update_Task", configMINIMAL_STACK_SIZE, NULL, 1, NULL);
    xTaskCreate(led_blink_task, "LED_Blink_Task", configMINIMAL_STACK_SIZE, NULL, 1, NULL);
    xTaskCreate(yellow_led_blink_task, "Yellow_LED_Blink_Task", configMINIMAL_STACK_SIZE, NULL, 1, NULL);

    // Start the scheduler
    vTaskStartScheduler();

    // We should never get here
    while(1);
}

void sensor_reading_task(void *pvParameters) {
    float temperature, humidity;
    bool motion_detected;

    while (1) {
        // Read DHT22 sensor
        dht22_read(&dht22, &temperature, &humidity);

        // Read PIR sensor
        motion_detected = gpio_get(PIR_PIN);

        // Update shared data
        if (xSemaphoreTake(data_mutex, portMAX_DELAY) == pdTRUE) {
            sensor_data.temperature = temperature;
            sensor_data.humidity = humidity;
            sensor_data.motion = motion_detected;
            xSemaphoreGive(data_mutex);
        }

        // Print sensor data to the shell
        printf("Sensor Reading Task - Temp: %.1fC, Humidity: %.1f%%, Motion: %s\n", 
               temperature, humidity, motion_detected ? "Yes" : "No");

        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}

void lcd_update_task(void *pvParameters) {
    SensorData prev_data = {.temperature = -1, .humidity = -1, .motion = false};
    char lcd_buffer[LCD_COLS + 1];

    while (1) {
        if (xSemaphoreTake(data_mutex, portMAX_DELAY) == pdTRUE) {
            if (memcmp(&sensor_data, &prev_data, sizeof(SensorData)) != 0) {
                prev_data = sensor_data;

                lcd_clear(&lcd);
                snprintf(lcd_buffer, sizeof(lcd_buffer), "Temp: %.1fC", prev_data.temperature);
                lcd_puts(&lcd, 0, 0, lcd_buffer);
                snprintf(lcd_buffer, sizeof(lcd_buffer), "Humidity: %.1f%%", prev_data.humidity);
                lcd_puts(&lcd, 0, 1, lcd_buffer);
                snprintf(lcd_buffer, sizeof(lcd_buffer), "Motion: %s", prev_data.motion ? "Yes" : "No");
                lcd_puts(&lcd, 0, 2, lcd_buffer);

                printf("LCD Update Task - Display Updated:\n%s\n%s\n%s\n", 
                       lcd_buffer, lcd_buffer + LCD_COLS + 1, lcd_buffer + 2*(LCD_COLS + 1));
            }
            xSemaphoreGive(data_mutex);
        }

        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}

void led_blink_task(void *pvParameters) {
    while (1) {
        if (xSemaphoreTake(data_mutex, portMAX_DELAY) == pdTRUE) {
            if (sensor_data.motion) {
                gpio_put(LED_PIN, 1);
                vTaskDelay(pdMS_TO_TICKS(100));
                gpio_put(LED_PIN, 0);
                vTaskDelay(pdMS_TO_TICKS(100));
            } else {
                gpio_put(LED_PIN, 0);
            }
            xSemaphoreGive(data_mutex);
        }

        printf("LED Blink Task - LED %s\n", sensor_data.motion ? "ON" : "OFF");

        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

void yellow_led_blink_task(void *pvParameters) {
    char lcd_buffer[LCD_COLS + 1];

    while (1) {
        gpio_put(YELLOW_LED_PIN, 1);

        lcd_puts(&lcd, 0, 3, "Yellow LED Blinks!");
        printf("Yellow LED Blink Task - Yellow LED Blinks!\n");

        vTaskDelay(pdMS_TO_TICKS(2000));

        gpio_put(YELLOW_LED_PIN, 0);
        lcd_clear(&lcd);

        vTaskDelay(pdMS_TO_TICKS(2000));
    }
}