#include <stdio.h>
#include "Arduino_FreeRTOS.h"
#include "task.h"
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>

const int trigPinRight = 15;
const int echoPinRight = 16;
const int trigPinLeft = 10;
const int echoPinLeft = 11;
const int ledGreen = 14;
const int ledYellow = 12;
const int ledRed = 13;


LiquidCrystal_I2C lcd(0x27, 16, 2);

const byte ROWS = 4;
const byte COLS = 4;

char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};

byte rowPins[ROWS] = {9, 8, 7, 6};
byte colPins[COLS] = {5, 4, 3, 2};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

int safeDistance;

void vTaskReadUltrasonic(void *pvParameters) {
  while (true) {
    // Membaca jarak kanan
    long durationRight, distanceRight;
    digitalWrite(trigPinRight, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPinRight, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPinRight, LOW);
    durationRight = pulseIn(echoPinRight, HIGH);
    distanceRight = (durationRight/2) / 29.1;

    // Membaca jarak kiri
    long durationLeft, distanceLeft;
    digitalWrite(trigPinLeft, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPinLeft, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPinLeft, LOW);
    durationLeft = pulseIn(echoPinLeft, HIGH);
    distanceLeft = (durationLeft/2) / 29.1;

    // Menampilkan jarak kanan dan kiri di LCD
    lcd.setCursor(0, 0);
    lcd.print("Right: ");
    lcd.print(distanceRight);
    lcd.print(" cm");
    lcd.setCursor(0, 1);
    lcd.print("Left: ");
    lcd.print(distanceLeft);
    lcd.print(" cm");

    // Memeriksa apakah jarak kanan atau kiri lebih besar dari jarak aman
    if (distanceRight > safeDistance || distanceLeft > safeDistance) {
      // Menyalakan lampu LED Kuning
      digitalWrite(ledYellow, HIGH);

      if (distanceRight < distanceLeft || distanceRight < safeDistance) {
        // Menyalakan lampu LED Merah
        digitalWrite(ledRed, HIGH);
        // Mematikan lampu LED Hijau
        digitalWrite(ledGreen, LOW);
}
else if (distanceLeft < distanceRight || distanceLeft < safeDistance) {
// Menyalakan lampu LED Hijau
digitalWrite(ledGreen, HIGH);
// Mematikan lampu LED Merah
digitalWrite(ledRed, LOW);
}
}
else {
// Mematikan semua lampu LED
digitalWrite(ledYellow, LOW);
digitalWrite(ledGreen, LOW);
digitalWrite(ledRed, LOW);
}

vTaskDelay(100);

}
}

void vTaskGetKeyInput(void *pvParameters) {
while (true) {
char key = keypad.getKey();
if (key != NO_KEY) {
if (key == 'A') {
// Jika tombol A ditekan, maka jarak aman akan diset ke 10 cm
safeDistance = 10;
lcd.setCursor(0, 0);
lcd.print("Safe distance: ");
lcd.print(safeDistance);
lcd.print(" cm ");
}
else if (key == 'B') {
// Jika tombol B ditekan, maka jarak aman akan diset ke 20 cm
safeDistance = 20;
lcd.setCursor(0, 0);
lcd.print("Safe distance: ");
lcd.print(safeDistance);
lcd.print(" cm ");
}
else if (key == 'C') {
// Jika tombol C ditekan, maka jarak aman akan diset ke 30 cm
safeDistance = 30;
lcd.setCursor(0, 0);
lcd.print("Safe distance: ");
lcd.print(safeDistance);
lcd.print(" cm ");
}
else if (key == 'D') {
// Jika tombol D ditekan, maka jarak aman akan diset ke 40 cm
safeDistance = 40;
lcd.setCursor(0, 0);
lcd.print("Safe distance: ");
lcd.print(safeDistance);
lcd.print(" cm ");
}
else {
// Jika tombol angka ditekan, maka jarak aman akan diset ke nilai yang sesuai
safeDistance = key - '0';
lcd.setCursor(0, 0);
lcd.print("Safe distance: ");
lcd.print(safeDistance);
lcd.print(" cm ");
}
}

vTaskDelay(100);

}
}

void setup() {
Serial.begin(9600);
lcd.init();
pinMode(trigPinRight, OUTPUT);
pinMode(echoPinRight, INPUT);
pinMode(trigPinLeft, OUTPUT);
pinMode(echoPinLeft, INPUT);
pinMode(ledGreen, OUTPUT);
pinMode(ledYellow, OUTPUT);
pinMode(ledRed, OUTPUT);

xTaskCreate(vTaskReadUltrasonic, "TaskReadUltrasonic", 128, NULL, 0, NULL);
xTaskCreate(vTaskGetKeyInput, "TaskGetKeyInput", 128, NULL, 1, NULL);

vTaskStartScheduler();
}

void loop() {
// Kosong, karena semua taks telah ditangani oleh freeRTOS
}