#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"


#define TRIGGER_GPIO 4
#define ECHO_GPIO 5
#define MAX_DISTANCE_CM 400 // 4m max
#define TIMEOUT_US (MAX_DISTANCE_CM * 58)

static const char *TAG = "lec2";

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 << TRIGGER_GPIO,
    };

    gpio_config(&io_conf);

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

    while (true) {
      // Calculate speed of sound in cm/us
      //float speed_of_sound = 331.4/10000;
      // Send trigger pulse
      gpio_set_level(TRIGGER_GPIO, 0);
      esp_rom_delay_us(4);
      gpio_set_level(TRIGGER_GPIO, 1);
      esp_rom_delay_us(8);
      gpio_set_level(TRIGGER_GPIO, 0);

      // Wait for echo start
      uint32_t start = esp_timer_get_time();
      while(gpio_get_level(ECHO_GPIO) == 0 && esp_timer_get_time() - start < TIMEOUT_US);

      // Measure echo time
      start = esp_timer_get_time();
      while(gpio_get_level(ECHO_GPIO) == 1 && esp_timer_get_time() - start < TIMEOUT_US);

      // Calculate distance in cm
      uint32_t distance;
      uint32_t time  = (esp_timer_get_time() - start);
      distance = time/58;
      printf("Distance: %d cm\n", (int)distance);

        // Wait for 1 second 
      vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
}