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

void app_main(){

    // Configure GPIO for SR04 sensor
    gpio_config_t io_conf = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_OUTPUT,
        .pin_bit_mask = 1ULL << 16,
    };

    gpio_config(&io_conf);

    io_conf.intr_type = GPIO_INTR_POSEDGE;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pin_bit_mask = 1ULL << 4;
    gpio_config(&io_conf);

    while (true) {
      //float velocidade do som = 331.4/10000;
      gpio_set_level(16, 0);
      esp_rom_delay_us(4);
      gpio_set_level(16, 1);
      esp_rom_delay_us(8);
      gpio_set_level(16, 0);

      uint32_t start = esp_timer_get_time();
      while(gpio_get_level(4) == 0 && esp_timer_get_time() - start < 400*58);

      start = esp_timer_get_time();
      while(gpio_get_level(4) == 1 && esp_timer_get_time() - start < 400*58);

      uint32_t distance;
      uint32_t time  = (esp_timer_get_time() - start);
      distance = time/58;
      printf("Distance: %d cm\n", (int)distance);
      
      vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
}