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

Servo myservo;
const int potPin = A0;  // Pin analog untuk potensiometer
const int ledPin = 6;   // Pin digital untuk LED
Ultrasonic ultrasonic(12, 13);
int sensorValueUlt;
int sensorValuePot;
int countval = 0;
int countval2 = 0;
const int servoPin = 9;
const int buzzerPin = 7;
int settingValue = 512;

void setup() {
  myservo.attach(servoPin);
  Serial.begin(9600);
  pinMode(ledPin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);

  xTaskCreate(TaskAnalogRead, "AnalogRead", 128, NULL, 1, NULL);
  xTaskCreate(LEDpot, "LEDpot", 128, NULL, 1, NULL);
  xTaskCreate(Ultrason, "Ultrason", 128, NULL, 1, NULL);
  xTaskCreate(servo, "servo", 128, NULL, 1, NULL);
  xTaskCreate(buzzer, "buzzer", 128, NULL, 1, NULL);
}

void loop() {}

void TaskAnalogRead(void *pvParameters) {
  (void) pvParameters;
  pinMode(A0, OUTPUT);
  for (;;) {
    int sensorValue = analogRead(A0);
    if (sensorValue > 500) {
      countval++;
      sensorValuePot = 1;
    } else if (sensorValue < 500) {
      sensorValuePot = 0;
      countval = 0;
    }
    Serial.print("PotVal: ");
    Serial.println(sensorValue);
    vTaskDelay(50);
  }
}

void LEDpot(void *pvParameters) {
  (void) pvParameters;
  for (;;) {
    int potValue = analogRead(potPin);
    int brightness = map(potValue, 0, 1023, 0, 255);
    analogWrite(ledPin, brightness);
  }
}

void Ultrason(void *pvParameters) {
  (void) pvParameters;
  for (;;) {
    int distance = ultrasonic.read();
    Serial.print("Distance in CM: ");
    Serial.println(distance);
    if (distance <= 100) {
      countval++;
      sensorValueUlt = 1;
    } else if (distance > 100) {
      sensorValueUlt = 0;
      countval = 0;
    }
    vTaskDelay(50);
  }
}

void servo(void *pvParameters) {
  (void) pvParameters;
  for (;;) {
     if (sensorValueUlt == 1) {
      myservo.write(90);
      Serial.print("Sudut Servo: 90");
      Serial.println(" derajat");
    } else {
      myservo.write(0);
      Serial.print("Sudut Servo: 0");
      Serial.println(" derajat");
     }
     vTaskDelay(50);
    }
  }

void buzzer(void *pvParameters) {
  (void) pvParameters;
  for (;;) {
    int sensorValue = analogRead(A0);
    if (sensorValue > 500) {
      tone(buzzerPin, 440);
      Serial.println("Buzzer: Nyala");
    } else {
      noTone(buzzerPin);
      Serial.println("Buzzer: Mati");
    }
    vTaskDelay(50);
  }
}