#include <stdio.h>
#include "driver/gpio.h"
#include "freertos/FreeRTOS.h"
#include <stdlib.h>
#include "driver/adc.h"
#include "esp_adc_cal.h"
#include "timer_u32.h"
#include <cmath>

#define LED_RED GPIO_NUM_32
#define LED_YELLOW GPIO_NUM_33
#define LED_GREEN GPIO_NUM_25

#define TRIG GPIO_NUM_21
#define ECHO GPIO_NUM_19

#define DELAY_TIME 200

int curr_time = 0;

static void gpio_isr_handler(void* arg)
{
    printf("Pozvan sam\n");
    uint32_t t0,t1,dt;
    t0 = timer_u32();
    while (gpio_get_level(ECHO) == 1){
    }
    t1 = timer_u32();
    dt = t1 - t0;
    curr_time = dt;
}

void sensor_config()
{
    
    printf("configuring sensor\n");

    // gpio_reset_pin(TRIG);
    gpio_set_direction(TRIG, GPIO_MODE_OUTPUT);
    gpio_pullup_en(TRIG);

    gpio_set_level(TRIG, 0);

    uint32_t t0,t1,dt;
    t0 = timer_u32();
    do {
        t1 = timer_u32();
        dt = t1 - t0;
    } while (timer_delta_us(dt) < 5);    

    gpio_set_level(TRIG, 1);

    // uint32_t t0,t1,dt;
    t0 = timer_u32();
    do {
        t1 = timer_u32();
        dt = t1 - t0;
    } while (timer_delta_us(dt) < 11);    

    gpio_set_level(TRIG, 0);

    gpio_install_isr_service(0);

    gpio_reset_pin(ECHO);
    gpio_set_direction(ECHO, GPIO_MODE_INPUT);
    gpio_pullup_en(ECHO);
    gpio_set_intr_type(ECHO, GPIO_INTR_POSEDGE);
    gpio_isr_handler_add(ECHO, gpio_isr_handler, NULL);

    printf("config complete\n");
}


extern "C" void app_main()
{   
    sensor_config();

    while (1) {

    }
}