#include <Arduino_FreeRTOS.h>

#include <task.h>


#define TRIGGER_PIN 7
#define ECHO_PIN 8
#define LED_PIN 3

void setup() {
  Serial.begin(9600);
  
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  
  xTaskCreate(TaskReadHC_SR04, "ReadHC_SR04", 100, NULL, 2, NULL);
  xTaskCreate(TaskBuzzer, "Buzzer", 100, NULL, 1, NULL);
  xTaskCreate(TaskBlinkLED, "BlinkLED", 100, NULL, 1, NULL);

  
  vTaskStartScheduler();
}

void TaskReadHC_SR04(void *pvParameters) {
  while (1) {
    long duration;
    float distance;

    digitalWrite(TRIGGER_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIGGER_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIGGER_PIN, LOW);

    duration = pulseIn(ECHO_PIN, HIGH);
    distance = (duration * 0.0343) / 2;

    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");

    delay(1000);
  }
}

void TaskBlinkLED(void *pvParameters) {
  while (1) {
    digitalWrite(LED_PIN, HIGH);
    delay(200);
    digitalWrite(LED_PIN, LOW);
    delay(200);
  }
}

void TaskBuzzer(void *pbParameters){
  while(1){
    tone(10, 1000, 1000);
    delay(1000);
    tone(10, 0, 1000);
    delay(1000);
  }
}


void loop() {
  
}