#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"

#define LED_OUT 6
#define ECHO_PIN 4
#define TRIG_PIN 5

int pico_led_init(void) {
    gpio_init(LED_OUT);
    gpio_set_dir(LED_OUT, GPIO_OUT);
    return PICO_OK;
}

// Turn the led on or off
void pico_set_led(bool led_on) {
    gpio_put(LED_OUT, led_on);
}

int pico_sr04_init(void) {
    gpio_init(ECHO_PIN);
    gpio_set_dir(ECHO_PIN, GPIO_IN);
    gpio_init(TRIG_PIN);
    gpio_set_dir(TRIG_PIN, GPIO_OUT);
    return PICO_OK;
}

void triggerSonar() {
  gpio_put(TRIG_PIN, false);
  sleep_ms(2);
  gpio_put(TRIG_PIN, true);
  sleep_us(10);
  gpio_put(TRIG_PIN, false);
}

float measure_distance() {
  triggerSonar();
  while (!gpio_get(ECHO_PIN)) sleep_us(1);
  absolute_time_t start_time = get_absolute_time();
  while (gpio_get(ECHO_PIN)) sleep_us(1);
  absolute_time_t end_time = get_absolute_time();

  printf("time diff (us): %lli\n", end_time - start_time);
  return (end_time - start_time) * 0.01715;
}

int main() {
    stdio_init_all();

    printf("Hello, Wokwi!\n");

    int rc = pico_led_init();
    hard_assert(rc == PICO_OK);

    if (cyw43_arch_init()) {
      printf("Wi-Fi init failed");
      return -1;
    }

    rc = pico_sr04_init();
    hard_assert(rc == PICO_OK);

    // Wait forever
    while (true) {
      sleep_ms(500);
      pico_set_led(true);
      float distance = measure_distance();
      printf("distance (us) %.2f\n", distance);
      pico_set_led(false);
      
      // bool isNearby = distance < 100;

      // pico_set_led(isNearby);

      // cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
      // sleep_ms(250);
      // pico_set_led(false);
      // cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 0);
      // sleep_ms(250);
    }
}