use std::time::Instant;
use esp_idf_hal::gpio::*;
use esp_idf_hal::delay::FreeRtos;
use esp_idf_hal::peripherals::Peripherals;

fn main() -> anyhow::Result<()> {
    let peripherals = Peripherals::take().unwrap();
    let mut trig = PinDriver::output(peripherals.pins.gpio3)?;
    let echo = PinDriver::input(peripherals.pins.gpio2)?;

    loop {
        trig.set_low()?;
        FreeRtos::delay_us(2);
        trig.set_high()?;
        FreeRtos::delay_us(10);
        trig.set_low()?;
        while echo.is_low(){}
        let start = Instant::now();
        while echo.is_high(){}
        let duration: f32 = start.elapsed().as_micros() as f32;
        let distance: f32 = (duration * 0.034) / 2.0;
        println!("distance = {} cm", distance as i32);
        FreeRtos::delay_ms(1000);
    }
}