#include "Arduino_FreeRTOS.h"
#include "task.h"
#include <Servo.h> // Include Servo Library
#include <NewPing.h> // Include Newping Library

//Our L298N control pins
const int Left_MotorForward = 8;
const int Left_MotorBackward = 9;
const int Right_MotorForward = 10;
const int Right_MotorBackward = 11;
int enA = 5;
int enB = 6;

//Sensor pins
#define trig_pin 4 //Analog input 1
#define echo_pin 3 //Analog input 2

#define distance_maximum 200
int distance = 100;
boolean is_Forward = false;

NewPing sonar(trig_pin, echo_pin, distance_maximum); //Sensor function
Servo servo_motor; //Our servo name

int lookRight() // Look Right Function for Servo Motor
{
servo_motor.write(50);
delay(500);
int distance = readPing();
delay(100);
servo_motor.write(115);
return distance;
}

int lookLeft() // Look Left Function for Servo Motor
{
servo_motor.write(180);
delay(500);
int distance = readPing();
delay(100);
servo_motor.write(115);
return distance;
}

int readPing() // Read Ping Function for Ultrasonic Sensor.
{
delay(100); // Wait 100ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
int cm = sonar.ping_cm(); //Send ping, get ping distance in centimeters (cm).
if (cm==0)
{
cm=250;
}
return cm;
}
// Fungsi wrapper untuk task pembacaan jarak oleh ultrasonic
void readUltrasonic(void *pvParameters) {
//servo_motor.attach(12); //Our servo pin
//servo_motor.write(95);
//delay(2000);
distance = readPing(); // Get Ping Distance.
delay(100); // Wait for 100ms.
distance = readPing();
delay(100);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
}

void moveStop() // Move Stop Function for Motor Driver.
{
digitalWrite(Right_MotorForward, LOW);
digitalWrite(Right_MotorBackward, LOW);
digitalWrite(Left_MotorForward, LOW);
digitalWrite(Left_MotorBackward, LOW);
}

void moveForward() // Move Forward Function for Motor Driver.
{
if(!is_Forward){

is_Forward=true;

digitalWrite(Left_MotorForward, HIGH);
digitalWrite(Right_MotorForward, HIGH);
digitalWrite(Left_MotorBackward, LOW);
digitalWrite(Right_MotorBackward, LOW); 
analogWrite(enA, 80);
analogWrite(enB, 80);

}
}

void moveBackward() // Move Backward Function for Motor Driver.
{
is_Forward=false;

digitalWrite(Left_MotorBackward, HIGH);
digitalWrite(Right_MotorBackward, HIGH);
digitalWrite(Left_MotorForward, LOW);
digitalWrite(Right_MotorForward, LOW);
analogWrite(enA, 80);
}

void turnRight() // Turn Right Function for Motor Driver.
{
digitalWrite(Left_MotorForward, HIGH);
digitalWrite(Right_MotorBackward, HIGH);
digitalWrite(Left_MotorBackward, LOW);
digitalWrite(Right_MotorForward, LOW);
analogWrite(enA, 80);
analogWrite(enB, 80);
delay(500);

digitalWrite(Left_MotorForward, HIGH);
digitalWrite(Right_MotorForward, HIGH);
digitalWrite(Left_MotorBackward, LOW);
digitalWrite(Right_MotorBackward, LOW);
analogWrite(enA, 80);
analogWrite(enB, 80);
delay(500);
}

void turnLeft() // Turn Left Function for Motor Driver.
{
digitalWrite(Left_MotorBackward, HIGH);
digitalWrite(Right_MotorForward, HIGH);
digitalWrite(Left_MotorForward, LOW);
digitalWrite(Right_MotorBackward, LOW);
analogWrite(enA, 80);
analogWrite(enB, 80);
delay(500);

digitalWrite(Left_MotorForward, HIGH);
digitalWrite(Right_MotorForward, HIGH);
digitalWrite(Left_MotorBackward, LOW);
digitalWrite(Right_MotorBackward, LOW);
analogWrite(enA, 80);
analogWrite(enB, 80);
delay(500);
}

// Fungsi wrapper untuk task pergerakan motor
void moveMotor(void *pvParameters) {
int distanceRight = 0;
int distanceLeft = 0;
delay(50);

if (distance <= 20)
{
moveStop();
delay(300);
moveBackward();
delay(400);
moveStop();
delay(300);
distanceRight = lookRight();
delay(300);
distanceLeft = lookLeft();
delay(300);

if (distanceRight >= distanceLeft)
{
turnRight();
delay(300);
moveStop();
}
else
{
turnLeft();
delay(300);
moveStop();
}

}
else
{
moveForward();
}

distance = readPing();
}

void setup()
{
// Set L298N Control Pins as Output
pinMode(Right_MotorForward, OUTPUT);
pinMode(Left_MotorForward, OUTPUT);
pinMode(Left_MotorBackward, OUTPUT);
pinMode(Right_MotorBackward, OUTPUT);
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);

servo_motor.attach(12); // Attachs the servo on pin 9 to servo object.
servo_motor.write(115); // Set at 115 degrees
delay(2000); // Wait for 2s.
distance = readPing(); // Get Ping Distance.
delay(100); // Wait for 100ms.
distance = readPing();
delay(100);
distance = readPing();
delay(100);
distance = readPing();
delay(100);

// Buat task untuk pembacaan jarak oleh ultrasonic dengan prioritas tertinggi
xTaskCreate(readUltrasonic, "ReadUltrasonic", configMINIMAL_STACK_SIZE, NULL, 0, NULL);

// Buat task untuk pergerakan motor dengan prioritas kedua
xTaskCreate(moveMotor, "MoveMotor", configMINIMAL_STACK_SIZE, NULL, 1, NULL);

// Jalankan scheduler
vTaskStartScheduler();

}

void loop()
{
// Kosongkan loop karena semua task akan dijalankan oleh scheduler
}