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

#define LED                     (18)
#define LEDC_OUTPUT_IO          (5)
#define LEDC_TIMER              LEDC_TIMER_0
#define LEDC_MODE               LEDC_LOW_SPEED_MODE
#define LEDC_CHANNEL            LEDC_CHANNEL_0
#define LEDC_DUTY_RES           LEDC_TIMER_12_BIT
#define LEDC_DUTY               (4095)
#define LEDC_FREQUENCY          (9000)

#define ECHO_PIN 19
#define TRIG_PIN 18

float previous = 100000;

float read_distance() 
{
  gpio_set_level(TRIG_PIN, 0);
  ets_delay_us(2);
  gpio_set_level(TRIG_PIN, 1);
  ets_delay_us(10);
  gpio_set_level(TRIG_PIN, 0);

  while (!gpio_get_level(ECHO_PIN)){};
  int64_t start = esp_timer_get_time();
  while (gpio_get_level(ECHO_PIN)){};
  int64_t finish = esp_timer_get_time();
  int64_t duration = finish - start;
  return duration * 0.034 / 2.0;
}

long map(long x, long in_min, long in_max, long out_min, long out_max) 
{
    const long dividend = out_max - out_min;
    const long divisor = in_max - in_min;
    const long delta = x - in_min;
    return (delta * dividend + (divisor / 2)) / divisor + out_min;
}

static void analogOutput(ledc_channel_t channel, uint32_t value, uint32_t valueMax)
{
    value = value > valueMax ? valueMax : value;
    uint32_t duty = (4095 / valueMax) * value;
    ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, channel, duty));
    ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, channel));
}

static void ledc_init(void)
{
    ledc_timer_config_t ledc_timer = {
        .speed_mode       = LEDC_MODE,
        .timer_num        = LEDC_TIMER,
        .duty_resolution  = LEDC_DUTY_RES,
        .freq_hz          = LEDC_FREQUENCY,
        .clk_cfg          = LEDC_AUTO_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

    ledc_channel_config_t ledc_channel = {
        .speed_mode     = LEDC_MODE,
        .channel        = LEDC_CHANNEL,
        .timer_sel      = LEDC_TIMER,
        .intr_type      = LEDC_INTR_DISABLE,
        .gpio_num       = LEDC_OUTPUT_IO,
        .duty           = 0,
        .hpoint         = 0
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
}

void app_main(void)
{
    gpio_pad_select_gpio(ECHO_PIN);
    gpio_pad_select_gpio(TRIG_PIN);
    gpio_set_direction(TRIG_PIN, GPIO_MODE_OUTPUT);
    gpio_set_direction(ECHO_PIN, GPIO_MODE_INPUT);
    ledc_init();

    while(1)
    {
      float distance = read_distance();

      if (distance < previous * 1000)
      {
        previous = distance;
      }

      uint32_t dutyCycle = map(distance, 0, 400, 0, 255);
      analogOutput(0, dutyCycle, 255); 
    }
}