#include <stdio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <driver/gpio.h>
#include <driver/i2c.h>
#include <esp_log.h>

// OLED configuration (using SSD1306)
#define I2C_MASTER_SCL_IO 22        // SCL pin
#define I2C_MASTER_SDA_IO 21        // SDA pin
#define I2C_MASTER_NUM I2C_NUM_0    // I2C port
#define I2C_MASTER_FREQ_HZ 100000   // 100kHz
#define OLED_I2C_ADDRESS 0x3C       // Typical SSD1306 I2C address

// HC-SR04 pins
#define TRIG_PIN GPIO_NUM_12
#define ECHO_PIN GPIO_NUM_14

static const char *TAG = "PARKING";

// OLED initialization function
void oled_init() {
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };
    i2c_param_config(I2C_MASTER_NUM, &conf);
    i2c_driver_install(I2C_MASTER_NUM, conf.mode, 0, 0, 0);
}

// Simple function to write to OLED (basic example, requires SSD1306 driver)
void oled_write(const char *text) {
    // Note: This is a placeholder. You'll need a full SSD1306 library or driver.
    // For simplicity, this example assumes a basic I2C write.
    uint8_t data[] = {0x40, text[0]}; // Example command/data
    i2c_master_write_to_device(I2C_MASTER_NUM, OLED_I2C_ADDRESS, data, sizeof(data), 1000 / portTICK_PERIOD_MS);
}

// Measure distance with HC-SR04
int measure_distance() {
    gpio_set_level(TRIG_PIN, 0);
    ets_delay_us(2);
    gpio_set_level(TRIG_PIN, 1);
    ets_delay_us(10);
    gpio_set_level(TRIG_PIN, 0);

    uint32_t time = xthal_get_ccount();
    while (gpio_get_level(ECHO_PIN) == 0 && (xthal_get_ccount() - time) < 1000000);
    time = xthal_get_ccount();
    while (gpio_get_level(ECHO_PIN) == 1 && (xthal_get_ccount() - time) < 1000000);
    uint32_t duration = (xthal_get_ccount() - time) / 240; // Convert CPU cycles to microseconds

    return (duration * 0.034) / 2; // Distance in cm
}

void app_main() {
    // Initialize GPIO
    gpio_config_t io_conf = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_OUTPUT,
        .pin_bit_mask = (1ULL << TRIG_PIN),
    };
    gpio_config(&io_conf);
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pin_bit_mask = (1ULL << ECHO_PIN);
    gpio_config(&io_conf);

    // Initialize OLED
    oled_init();

    while (1) {
        int distance = measure_distance();
        int parkingSpot = (distance > 10) ? 1 : 0; // Threshold 10 cm

        // Display result (placeholder)
        char buffer[16];
        snprintf(buffer, sizeof(buffer), "Spot: %d", parkingSpot);
        oled_write(buffer);

        ESP_LOGI(TAG, "Distance: %d cm, Spot: %d", distance, parkingSpot);
        vTaskDelay(500 / portTICK_PERIOD_MS);
    }
}