#include <stdio.h>
#include <sys/time.h>
#include <esp_timer.h>
#include <driver/gpio.h>
#include <rom/ets_sys.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

#define ECHO_PIN 5
#define TRIG_PIN 6

float previous = 100000;

float read_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);

  while (!gpio_get_level(ECHO_PIN)){};
  int64_t start = esp_timer_get_time();
  while (gpio_get_level(ECHO_PIN)){};
  int64_t finish = esp_timer_get_time();
  int64_t duration = finish - start;
  return duration * 0.034 / 2.0;
}

void app_main() 
{
  gpio_pad_select_gpio(ECHO_PIN);
  gpio_pad_select_gpio(TRIG_PIN);
  gpio_set_direction(TRIG_PIN, GPIO_MODE_OUTPUT);
  gpio_set_direction(ECHO_PIN, GPIO_MODE_INPUT);
  while(1)
  {
    float distance = read_distance();

    if (distance < previous * 1000)
    {
      previous = distance;
      printf("Measured distance: %0.0f\n", distance);
    }
    
    vTaskDelay(100 / portTICK_PERIOD_MS);
  }
}