#include <stdio.h>
#include "pico/stdlib.h"

#define TRIG_PIN 3  // Pin connected to HC-SR04 TRIG
#define ECHO_PIN 2  // Pin connected to HC-SR04 ECHO

void hc_sr04_init() {
    // Initialize TRIG as output
    gpio_init(TRIG_PIN);
    gpio_set_dir(TRIG_PIN, GPIO_OUT);
    gpio_put(TRIG_PIN, 0); // Set TRIG low initially

    // Initialize ECHO as input
    gpio_init(ECHO_PIN);
    gpio_set_dir(ECHO_PIN, GPIO_IN);
}

float measure_distance() {
    // Trigger a 10µs pulse on TRIG
    gpio_put(TRIG_PIN, 1);
    sleep_us(10);
    gpio_put(TRIG_PIN, 0);

    // Measure the pulse width on ECHO
    uint64_t start_time = time_us_64();
    while (!gpio_get(ECHO_PIN)) { // Wait for ECHO to go HIGH
        if ((time_us_64() - start_time) > 2000000) return -1; // Timeout if no response
    }
    uint64_t echo_start = time_us_64();

    while (gpio_get(ECHO_PIN)) { // Wait for ECHO to go LOW
        if ((time_us_64() - echo_start) > 2000000) return -1; // Timeout if no response
    }
    uint64_t echo_end = time_us_64();

    // Calculate distance in cm
    uint64_t pulse_duration = echo_end - echo_start;
    float distance = (pulse_duration * 0.0343) / 2; // Speed of sound = 343 m/s

    return distance;
}

int main() {
    stdio_init_all();
    hc_sr04_init();
    printf("HC-SR04 Ultrasonic Sensor Initialized\n");

    while (true) {
        float distance = measure_distance();

        if (distance < 0) {
            printf("Timeout! No object detected.\n");
        } else {
            printf("Distance: %.2f cm\n", distance);
        }

        sleep_ms(1000); // Wait 1 second before the next reading
    }
}