#include <Arduino_FreeRTOS.h>
#include <Servo.h>

const int trigPin = 2; // Pin trigger sensor ultrasonik
const int echoPin = 3; // Pin echo sensor ultrasonik
const int potPin = A0;
const int ledPin = 9;
const int buzzerPin = 10;
Servo servoMotor;
int distance = 0; // Variabel global untuk menyimpan jarak
int potValue = 0;
TaskHandle_t servoControlTaskHandle;

void setup() {
  servoMotor.attach(8); // Menghubungkan servo motor ke pin 11
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);

  xTaskCreate(TaskUltrasonic, "Ultrasonic Task", 100, NULL, 1, NULL);
  xTaskCreate(taskServoControl, "ServoControlTask",100, NULL ,2 ,&servoControlTaskHandle);
  xTaskCreate(TaskReadPot, "ReadPot", 128, NULL, 1, NULL);
  xTaskCreate(TaskControlLED, "ControlLED", 128, NULL, 2, NULL);
  xTaskCreate(TaskControlBuzzer, "ControlBuzzer", 128, NULL, 3, NULL);
  vTaskStartScheduler();
}

void loop() {
  // Tidak ada kode dalam loop karena tugas-tugas dijadwalkan oleh RTOS
}


void TaskUltrasonic(void *pvParameters) {
  (void)pvParameters;
  for (;;) {
    // Baca jarak dari sensor ultrasonik
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    long duration = pulseIn(echoPin, HIGH);
    distance = duration * 0.034 / 2;

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

    vTaskDelay(pdMS_TO_TICKS(2000)); // Ukuran delay sesuai kebutuhan
  }
}

void taskServoControl(void *pvParameters) {
  (void)pvParameters;
  for (;;) {
    // Kontrol servo berdasarkan nilai jarak yang diperoleh
    if (distance > 100) {
      servoMotor.write(90);
      Serial.println("Servo              : 90 Derajat");
    } else {
      servoMotor.write(0);  
      Serial.println("Servo              :  0 Derajat");
    }
    
    vTaskDelay(pdMS_TO_TICKS(2000)); // Ukuran delay sesuai kebutuhan
  }
}

void TaskReadPot(void *pvParameters) {
  (void)pvParameters;
  int potValue = 0;
  for (;;) {
    potValue = analogRead(potPin);
    Serial.print("Potentiometer Value: ");
    Serial.println(potValue);
    vTaskDelay(pdMS_TO_TICKS(2000)); // Read pot every 1 second
  }
}

void TaskControlLED(void *pvParameters) {
  (void)pvParameters;
  int potValue = 0;
  for (;;) {
    potValue = analogRead(potPin);
    int ledBrightness = map(potValue, 0, 1023, 0, 255);
    analogWrite(ledPin, ledBrightness);
    vTaskDelay(pdMS_TO_TICKS(100)); // Control LED every 0.1 second
  }
}

void TaskControlBuzzer(void *pvParameters) {
  (void)pvParameters;
  int potValue = 0;
  for (;;) {
    potValue = analogRead(potPin);
    if (potValue > 900) {
      digitalWrite(buzzerPin, HIGH);
      Serial.println("Buzzer             : Menyala");
    } else {
      digitalWrite(buzzerPin, LOW);
      Serial.println("Buzzer             : Mati");
    }
    vTaskDelay(pdMS_TO_TICKS(2000)); // Control buzzer every 0.1 second
  }
}