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

#define TRIGGER_GPIO          (GPIO_NUM_4)
#define TRIGGER_LOW_DELAY     (4)
#define TRIGGER_HIGH_DELAY    (10)
#define ECHO_GPIO             (GPIO_NUM_5)
#define MAX_DISTANCE_CM       (400)
#define ROUNDTRIP_CM          (58)
#define TIMEOUT_US            (MAX_DISTANCE_CM * ROUNDTRIP_CM)

typedef struct {
    gpio_num_t trigger_pin;
    gpio_num_t echo_pin;
} ultrasonic_handle_t;


static const char *TAG = "ULTRASONIC";

#define CONFIG_LOG_MAXIMUM_LEVEL 5

esp_err_t ultrasonic_init(const ultrasonic_handle_t *dev)
{
  gpio_reset_pin( dev->trigger_pin );
  gpio_reset_pin( dev->echo_pin );
  ESP_ERROR_CHECK(gpio_set_direction(dev->trigger_pin, GPIO_MODE_OUTPUT));
  ESP_ERROR_CHECK(gpio_set_direction(dev->echo_pin, GPIO_MODE_INPUT));
  ESP_ERROR_CHECK(gpio_set_level(dev->trigger_pin, 0));
  return ESP_OK;
}

esp_err_t ultrasonic_measure_cm(const ultrasonic_handle_t *dev, uint32_t *distance)
{
  ESP_ERROR_CHECK(gpio_set_level(dev->trigger_pin, 0));
  esp_rom_delay_us(TRIGGER_LOW_DELAY);
  ESP_ERROR_CHECK(gpio_set_level(dev->trigger_pin, 1));
  esp_rom_delay_us(TRIGGER_HIGH_DELAY);
  ESP_ERROR_CHECK(gpio_set_level(dev->trigger_pin, 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 time_us  = (esp_timer_get_time() - start);
  *distance = time_us / ROUNDTRIP_CM;
  return ESP_OK;
}


void app_main(void) {

  esp_log_level_set(TAG, ESP_LOG_VERBOSE);

  uint32_t distance;

    ultrasonic_handle_t sensor = {
        .trigger_pin = TRIGGER_GPIO,
        .echo_pin = ECHO_GPIO,
    };

  ultrasonic_init(&sensor);
  while (1) {
    esp_err_t res = ultrasonic_measure_cm(&sensor, &distance);
    if (res == ESP_OK) {
      printf("Distance: %ld cm\n",distance);
      }
    vTaskDelay(pdMS_TO_TICKS(500));
  }
}