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

// Definisi pin
const int potPin = A0;
const int ledPin = 9;
const int trigPin = 7;
const int echoPin = 8;
const int servoPin = 10;
const int buzzerPin = 6;
const int buttonPin = 2;

// Objek
Servo myServo;
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Variabel global
int potValue = 0;
int ledBrightness = 0;
long ultrasonicDistance = 0;
int servoAngle = 0;
int buzzerSettingValue = 500;
int buttonState = 0;
int displayMode = 0;
bool buzzerOn = false;

// Fungsi untuk membaca potensiometer
void TaskReadPot(void *pvParameters) {
  for (;;) {
    potValue = analogRead(potPin);
    Serial.print("Potentiometer Value: ");
    Serial.println(potValue);
    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

// Fungsi untuk menyesuaikan kecerahan LED
void TaskLEDControl(void *pvParameters) {
  for (;;) {
    ledBrightness = map(potValue, 0, 1023, 0, 255);
    analogWrite(ledPin, ledBrightness);
    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

// Fungsi untuk membaca sensor ultrasonik
void TaskUltrasonic(void *pvParameters) {
  for (;;) {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    
    long duration = pulseIn(echoPin, HIGH);
    ultrasonicDistance = duration * 0.034 / 2;
    
    Serial.print("Ultrasonic Distance: ");
    Serial.print(ultrasonicDistance);
    Serial.println(" cm");
    
    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

// Fungsi untuk menggerakkan servo
void TaskServoControl(void *pvParameters) {
  for (;;) {
    servoAngle = map(ultrasonicDistance, 0, 200, 0, 180); // Rentang bebas
    myServo.write(servoAngle);
    
    Serial.print("Servo Angle: ");
    Serial.println(servoAngle);
    
    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

// Fungsi untuk mengontrol buzzer
void TaskBuzzerControl(void *pvParameters) {
  for (;;) {
    if (potValue > buzzerSettingValue) {
      digitalWrite(buzzerPin, HIGH);
      buzzerOn = true;
    } else {
      digitalWrite(buzzerPin, LOW);
      buzzerOn = false;
    }
    
    Serial.print("Buzzer: ");
    Serial.println(buzzerOn ? "ON" : "OFF");
    
    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

// Fungsi untuk menampilkan nilai pada LCD
void TaskLCDControl(void *pvParameters) {
  for (;;) {
    buttonState = digitalRead(buttonPin);
    
    if (buttonState == HIGH) {
      displayMode = (displayMode + 1) % 5; // Berpindah mode setiap tombol ditekan
      vTaskDelay(300 / portTICK_PERIOD_MS); // Debounce
    }
    
    lcd.clear();
    switch (displayMode) {
      case 0:
        lcd.print("Pot: ");
        lcd.print(potValue);
        break;
      case 1:
        lcd.print("LED: ");
        lcd.print(ledBrightness);
        break;
      case 2:
        lcd.print("Ultrasonic: ");
        lcd.print(ultrasonicDistance);
        lcd.print(" cm");
        break;
      case 3:
        lcd.print("Servo: ");
        lcd.print(servoAngle);
        break;
      case 4:
        lcd.print("Buzzer: ");
        lcd.print(buzzerOn ? "ON" : "OFF");
        break;
    }
    
    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

void setup() {
  Serial.begin(9600);
  
  // Pin setup
  pinMode(potPin, INPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(buttonPin, INPUT);
  
  // Servo setup
  myServo.attach(servoPin);
  
  // LCD setup
  lcd.init();
  lcd.backlight();
  
  // Task creation
  xTaskCreate(TaskReadPot, "Read Pot", 128, NULL, 1, NULL);
  xTaskCreate(TaskLEDControl, "LED Control", 128, NULL, 1, NULL);
  xTaskCreate(TaskUltrasonic, "Ultrasonic", 128, NULL, 1, NULL);
  xTaskCreate(TaskServoControl, "Servo Control", 128, NULL, 1, NULL);
  xTaskCreate(TaskBuzzerControl, "Buzzer Control", 128, NULL, 1, NULL);
  xTaskCreate(TaskLCDControl, "LCD Control", 128, NULL, 1, NULL);
  
  vTaskStartScheduler(); // Mulai scheduler FreeRTOS
}

void loop() {
  // Tidak digunakan karena FreeRTOS menjalankan task-task
}