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

// #define redPin GPIO_NUM_4
// #define fiolPin GPIO_NUM_5
// #define whitePin GPIO_NUM_6
#define trigPin GPIO_NUM_7
#define echoPin GPIO_NUM_15

const short ledPins[3] = {GPIO_NUM_4, GPIO_NUM_5, GPIO_NUM_6};
const int dists[3] = {100, 200, 300};

short kol_vo = 0;

void leds(short a) {
  for(short i = 0; i < 3; i++) {
    if(i < a) {
      gpio_set_level(ledPins[i], true);
    }
    else {
      gpio_set_level(ledPins[i], false);
    }
  }

  // for(short i = 0; i < 3 - a; i++) {
  //   gpio_set_level(ledPins[i], false);
  // }
}


void app_main() {
  printf("Hello, Wokwi!\n");
  for(short i = 0; i < 3; i++) {
    gpio_reset_pin(ledPins[i]);
    gpio_set_direction(ledPins[i], GPIO_MODE_OUTPUT); 
  }

  gpio_reset_pin(trigPin);
  gpio_set_direction(trigPin, GPIO_MODE_OUTPUT);
  gpio_reset_pin(echoPin);
  gpio_set_direction(echoPin, GPIO_MODE_INPUT); 


  while (true) {
    gpio_set_level(trigPin, false);
    esp_rom_delay_us(2);
    gpio_set_level(trigPin, true);
    esp_rom_delay_us(10);
    gpio_set_level(trigPin, false);

    while(!gpio_get_level(echoPin)) {}
    uint64_t start_time = esp_timer_get_time();
    while(gpio_get_level(echoPin)) {}
    long dist = (esp_timer_get_time() - start_time) / 2 * 0.0343;

    for(short i = 0; i < 3; i++) {
      if(dist > dists[i]) {
        kol_vo++;
      }
    }

    leds(kol_vo);
    kol_vo = 0;


    //vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}