// Apply the supplementary act
#include <Wire.h> 
#include <Servo.h>
#include <LiquidCrystal_I2C.h> 

//set up pins para sa act 1
Servo servo1;  
Servo servo2;  
int led_horizontal = 6;
int led_vertical = 7;
int x_key = A1;  // X-axis of the joystick
int y_key = A0;  // Y-axis of the joystick
int x_pos;  
int y_pos;  
int servo1_pin = 6;  
int servo2_pin = 5;  
int initial_position = 90;  
int initial_position1 = 90;
bool turning = false;

//set up para sa pins for act 2
LiquidCrystal_I2C lcd(0x27, 16, 2); 
const int front_trigPin = 13; 
const int front_echoPin = 12; 
const int back_trigPin = 11; 
const int back_echoPin = 10;
const int left_trigPin = 9; 
const int left_echoPin = 8;
const int right_trigPin = 4; 
const int right_echoPin = 3;   
long duration; 
int distance; 
const int thresholdDistance = 100;  // Threshold distance in cm

//Functions for the servo motors
// Function declarations with proper names
void updateXaxis(int value, int high_value, int &XinitialPos, int x_pos, bool &isTurning);
void updateXaxis(int value, int high_value, int &XinitialPos, int x_pos, bool &isTurning);
void blinking(int led_direction);

//Function for the ultrasonic sensors and LED
void runningTheSensors(int trigerPin, int echoPin, char direction[50]);
void printingTheDistance(int distance , int thresholdDistance, char directionText[50]);

void setup() { 
  Serial.begin(9600); 
  //setup for LED and Sensor 
  lcd.begin(16, 2); // Initializes the interface to the LCD display
  lcd.backlight(); 
  pinMode(front_trigPin, OUTPUT); 
  pinMode(front_echoPin, INPUT);
  pinMode(back_trigPin, OUTPUT); 
  pinMode(back_echoPin, INPUT);
  pinMode(left_trigPin, OUTPUT); 
  pinMode(left_echoPin, INPUT);
  pinMode(right_trigPin, OUTPUT); 
  pinMode(right_echoPin, INPUT);




  //setup for servomotor and joystick
      servo1.attach(servo1_pin); 
      servo2.attach(servo2_pin); 
      
      servo1.write(initial_position); 
      servo2.write(initial_position1); 
      
      pinMode(led_horizontal, OUTPUT);
      pinMode(led_vertical, OUTPUT);
      pinMode(x_key, INPUT); 
      pinMode(y_key, INPUT);  
} 

void loop() { 
  x_pos = analogRead(x_key); // for servo motor x
  y_pos = analogRead(y_key); // for servo motor y

  // running the function for LCD and sensor
  runningTheSensors(front_trigPin, front_echoPin, "Front");
  delay(500);
  runningTheSensors(back_trigPin, back_echoPin, "Back");
  delay(500);
  runningTheSensors(left_trigPin, left_echoPin, "Left");
  delay(500);
  runningTheSensors(right_trigPin, right_echoPin, "Right");


  //running the function for servo motors
  // Allow the servo motors to turn
  if (distance > thresholdDistance) {
    // Horizontal updates for the X-axis servo
    updateXaxis(300, 700, initial_position, x_pos, turning);

    // Vertical updates for the Y-axis servo
    updateYaxis(300, 700, initial_position1, y_pos, turning);
  } else {
    // Threshold distance reached, prevent servo movement
    Serial.println("Threshold distance reached. Servos are disabled.");
  }
  delay(800); // Delay to avoid rapid updates
}

void runningTheSensors(int trigPin, int echoPin, char direction[50]) {
    digitalWrite(trigPin, LOW); 
    delayMicroseconds(2); 

    digitalWrite(trigPin, HIGH); 
    delayMicroseconds(10); 
    digitalWrite(trigPin, LOW); 

    duration = pulseIn(echoPin, HIGH); 
    distance = duration * 0.0340 / 2;

    printingTheDistance(distance ,thresholdDistance, direction);
}

void printingTheDistance(int distance, int thresholdDistance, char directionText[50]) {
      lcd.clear(); 
  if (distance <= thresholdDistance) {
      lcd.setCursor(0, 0);
      lcd.print("Stop");

    } else {
      lcd.setCursor(0, 0);
      lcd.print("Distance: ");
      lcd.print(distance);
      lcd.print("cm");

      lcd.setCursor(0, 1);
      lcd.print(directionText);
    };
}

// Function to update the X-axis servo
void updateXaxis(int value, int high_value, int &XinitialPos, int x_pos, bool &isTurning) {
  if (x_pos < value || x_pos > high_value) {  // Check if joystick is moved outside neutral range
    isTurning = true;
  } else {
    isTurning = false;  // Joystick is in neutral range
  }

  while (isTurning) {
    if (x_pos < value) { 
      if (XinitialPos >= 10) {
        XinitialPos -= 20; 
        servo1.write(XinitialPos); 
        blinking(led_horizontal);
      }
    } else if (x_pos > high_value) { 
      if (XinitialPos <= 180) {
        XinitialPos += 20; 
        servo1.write(XinitialPos);
        blinking(led_horizontal);
      }
    }

    // Break out of the loop if joystick returns to neutral position
    x_pos = analogRead(x_key);
    if (x_pos >= value && x_pos <= high_value) {
      isTurning = false;
    }
  }
}

void updateYaxis(int value, int high_value, int &YinitialPos, int y_pos, bool &isTurning) {
  if (y_pos < value || y_pos > high_value) {  // Check if joystick is moved outside neutral range
    isTurning = true;
  } else {
    isTurning = false;  // Joystick is in neutral range
  }

  while (isTurning) {
    if (y_pos < value) { 
      if (YinitialPos >= 10) {
        YinitialPos -= 20; 
        servo2.write(YinitialPos); 
        blinking(led_vertical);
      }
    } else if (y_pos > high_value) { 
      if (YinitialPos <= 180) {
        YinitialPos += 20; 
        servo2.write(YinitialPos); 
        blinking(led_vertical);
      }
    }

    // Break out of the loop if joystick returns to neutral position
    y_pos = analogRead(y_key);
    if (y_pos >= value && y_pos <= high_value) {
      isTurning = false;
    }
  }
}

void blinking(int led_direction){
  for (int i = 0; i < 5; i++) {
    digitalWrite(led_direction, HIGH);
    delay(100);
    digitalWrite(led_direction, LOW);
  }
}