#include <Arduino.h>
#include <ESP32Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define RED_LED_PIN  19  // LED merah
#define GREEN_LED_PIN  18  // LED hijau
#define SERVO_PIN 5
#define SCREEN_WIDTH 128  // Lebar layar OLED
#define SCREEN_HEIGHT 64  // Tinggi layar OLED

// Deklarasi handle semaphore
SemaphoreHandle_t xSemaphore;

Servo myservo;  // Objek servo
int statServo = 0;

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Task handles
TaskHandle_t Task1;
TaskHandle_t Task2;
TaskHandle_t Task3;
TaskHandle_t Task4;

void clearStatServo(){
  statServo=0;
}

void checkMotor() {
  if (statServo < 5) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.print("Warning Motor");
    display.display();
    vTaskDelay(2000 / portTICK_PERIOD_MS);  // Update layar setiap 2 detik
  }
  else{
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.print("");
    display.display();
    vTaskDelay(2000 / portTICK_PERIOD_MS);  // Update layar setiap 2 detik
  }
}

// Task 1: Blink Red LED
void Task1code(void * parameter) {
  pinMode(RED_LED_PIN, OUTPUT);
  while(1) {
    Serial.println("Task 1 try to take semaphore"); 
    if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) {
      Serial.println("Task 1 get semaphore");
      digitalWrite(RED_LED_PIN, HIGH);
      vTaskDelay(500 / portTICK_PERIOD_MS);  // delay 500 ms
      digitalWrite(RED_LED_PIN, LOW);
      vTaskDelay(500 / portTICK_PERIOD_MS);  // delay 500 ms

      statServo++;
      Serial.println("Task 1: RED LED Blink");
      xSemaphoreGive(xSemaphore);
      Serial.println("Task 1 release semaphore");     
      vTaskDelay(2000 / portTICK_PERIOD_MS);  // delay 500 ms 
    }
  }
}

// Task 2: Blink Green LED
void Task2code(void * parameter) {
  pinMode(GREEN_LED_PIN, OUTPUT);
  for (;;) {
    Serial.println("Task 2 try to take semaphore"); 
    if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) {
      Serial.println("Task 2 get semaphore");
      digitalWrite(GREEN_LED_PIN, HIGH);
      vTaskDelay(500 / portTICK_PERIOD_MS);  // delay 1000 ms
      digitalWrite(GREEN_LED_PIN, LOW);
      vTaskDelay(500 / portTICK_PERIOD_MS);  // delay 1000 ms

      statServo++;
      Serial.println("Task 2: GREEN LED Blink");  // Fixed message
      xSemaphoreGive(xSemaphore);
      Serial.println("Task 2 release semaphore"); 
      vTaskDelay(1000 / portTICK_PERIOD_MS);  // delay 1000 ms
    }
  }
}

// Task 3: Rotate Servo between 30 and 60 degrees
void Task3code(void * parameter) {
  for (;;) {
    Serial.println("Task 3 try to take semaphore"); 
    if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) {
      Serial.println("Task 3 get semaphore");
      myservo.write(30);
      vTaskDelay(500 / portTICK_PERIOD_MS);  // delay 1000 ms
      myservo.write(60);
      vTaskDelay(500 / portTICK_PERIOD_MS);  // delay 1000 ms

      clearStatServo();

      Serial.print("Task 3: motor move");
      xSemaphoreGive(xSemaphore);
      Serial.println("Task 3 release semaphore"); 
      vTaskDelay(500 / portTICK_PERIOD_MS);  // delay 1000 ms
    }
  }
}

// Task 4: OLED Display
void Task4code(void * parameter) {
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {  // Address 0x3C for OLED
    for (;;);  // Jika gagal, berhenti
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  for (;;) {
    checkMotor();
  }
}

void setup() {
  // Initialize serial communication
  Serial.begin(115200);
  delay(1000); // Give time for serial monitor to initialize

  // Membuat binary semaphore
  xSemaphore = xSemaphoreCreateBinary();  // Create a binary semaphore

  // Pastikan semaphore berhasil dibuat
  if (xSemaphore != NULL) {
    // Berikan semaphore pertama kali
    xSemaphoreGive(xSemaphore);

    // Create tasks
    xTaskCreatePinnedToCore(
      Task1code,    // Function to implement the task
      "Task1",      // Name of the task
      1000,         // Stack size in words
      NULL,         // Task input parameter
      3,            // Priority of the task
      &Task1,       // Task handle
      0);           // Core where the task should run

    xTaskCreatePinnedToCore(
      Task2code,
      "Task2",
      1000,
      NULL,
      2,
      &Task2,
      0);

    xTaskCreatePinnedToCore(
      Task3code,
      "Task3",
      1000,
      NULL,
      1,
      &Task3,
      0);

    xTaskCreatePinnedToCore(
      Task4code,
      "Task4",
      2000,         // Lebih besar untuk OLED
      NULL,
      1,
      &Task4,
      0);
  }
}

void loop() {
  // Tidak ada kode di loop, semua dilakukan oleh FreeRTOS
  delay(10);
}