#include <stdio.h>
#include <driver/gpio.h>
#include <esp_err.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <esp_timer.h>
#include <esp32c3/rom/ets_sys.h>
#include <esp_task_wdt.h>

#define ESP_ERR_ULTRASONIC_PING         0x200
#define ESP_ERR_ULTRASONIC_PING_TIMEOUT 0x201
#define ESP_ERR_ULTRASONIC_ECHO_TIMEOUT 0x202

typedef struct
{
   gpio_num_t trigger_pin; //!< GPIO output pin for trigger
   gpio_num_t echo_pin;    //!< GPIO input pin for echo
} ultrasonic_sensor_t;

esp_err_t ultrasonic_init();
esp_err_t ultrasonic_measure_raw(const ultrasonic_sensor_t *dev, uint32_t max_time_us, uint32_t *time_us);
esp_err_t ultrasonic_measure_cm(const ultrasonic_sensor_t *dev, float max_distance, float *distance);

#define TRIGGER_LOW_DELAY 4
#define TRIGGER_HIGH_DELAY 10
#define PING_TIMEOUT 50000
#define ROUNDTRIP_CM 58
#define TRIGGER_GPIO 4
#define ECHO_GPIO 5
#define MAX_DISTANCE_CM 500 // 5m max

static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
#define PORT_ENTER_CRITICAL portENTER_CRITICAL(&mux)
#define PORT_EXIT_CRITICAL portEXIT_CRITICAL(&mux)

#define timeout_expired(start, len) ((esp_timer_get_time() - (start)) >= (len))
#define CHECK_ARG(VAL) do { if (!(VAL)) return ESP_ERR_INVALID_ARG; } while (0)
#define CHECK(x) do { esp_err_t __; if ((__ = x) != ESP_OK) return __; } while (0)
#define RETURN_CRITICAL(RES) do { PORT_EXIT_CRITICAL; return RES; } while(0)

ultrasonic_init()
{
   gpio_set_direction(TRIGGER_GPIO, GPIO_MODE_OUTPUT);
   gpio_set_direction(ECHO_GPIO, GPIO_MODE_INPUT);

   return gpio_set_level(TRIGGER_GPIO, 0);
}

esp_err_t ultrasonic_measure_raw()
{
  uint32_t time_us;

   PORT_ENTER_CRITICAL;

   // Ping: Low for 2..4 us, then high 10 us
   gpio_set_level(TRIGGER_GPIO, 0);
   ets_delay_us(TRIGGER_LOW_DELAY);
   gpio_set_level(TRIGGER_GPIO, 1);
   ets_delay_us(TRIGGER_HIGH_DELAY);
   gpio_set_level(TRIGGER_GPIO, 0);

   // Previous ping isn't ended
   if (gpio_get_level(ECHO_GPIO))
      RETURN_CRITICAL(ESP_ERR_ULTRASONIC_PING);

   // Wait for echo
   int64_t start = esp_timer_get_time();
   while (!gpio_get_level(ECHO_GPIO))
   {
      if (timeout_expired(start, PING_TIMEOUT))
         RETURN_CRITICAL(ESP_ERR_ULTRASONIC_PING_TIMEOUT);
   }

    // got echo, measuring
   int64_t echo_start = esp_timer_get_time();
   int64_t time = echo_start;
   while (gpio_get_level(ECHO_GPIO))
   {
      time = esp_timer_get_time();
      if (timeout_expired(echo_start, MAX_DISTANCE_CM))
         RETURN_CRITICAL(ESP_ERR_ULTRASONIC_ECHO_TIMEOUT);
   }
   PORT_EXIT_CRITICAL;
   //esp_task_wdt_reset();

   time_us = time - echo_start;

   return time_us;
}
    
void read_distance()
{
    ultrasonic_init();

    while (true)
    {
      float distance;
      uint32_t time_us;
      time_us = ultrasonic_measure_raw();
   distance = time_us / ROUNDTRIP_CM;
      if (res != ESP_OK)
      {
         printf("Error %d: ", res);
         switch (res)
         {
            case ESP_ERR_ULTRASONIC_PING:
               printf("Cannot ping (device is in invalid state)\n");
               break;
            case ESP_ERR_ULTRASONIC_PING_TIMEOUT:
               printf("Ping timeout (no device found)\n");
               break;
            case ESP_ERR_ULTRASONIC_ECHO_TIMEOUT:
               printf("Echo timeout (i.e. distance too big)\n");
               break;
            default:
               printf("%s\n", esp_err_to_name(res));
         }
      }
      else
         printf("Distance: %0.04f m\n", distance);

      vTaskDelay(pdMS_TO_TICKS(500));
   }
}
    
void app_main()
{
read_distance();

   //xTaskCreate(read_distance, "read_distance", configMINIMAL_STACK_SIZE * 3, NULL, 5, NULL);
}