#include <AccelStepper.h>

const int trigPins0 = 10;
const int echoPins0 = A3;
const int trigPins1= 11;
const int echoPins1= A1;
const int trigPins2= 12;
const int echoPins2= A2;
const int trigPins3= 13;
const int echoPins3= A0;

AccelStepper stepperFL(AccelStepper::DRIVER, 3, 7); // Front Left
AccelStepper stepperFR(AccelStepper::DRIVER, 2, 6); // Front Right
AccelStepper stepperBL(AccelStepper::DRIVER, 4, 8); // Back Left
AccelStepper stepperBR(AccelStepper::DRIVER, 5, 9); // Back Right

const int stepPins0 = 2;
const int dirPins0 = 6;
const int stepPins1 = 3;
const int dirPins1 =  7;
const int stepPins2 = 4;
const int dirPins2 =  8;
const int stepPins3 = 5;
const int dirPins3 =  9;
#define warning 50
// Joystick pins
const int joyXPin = A4;
const int joyYPin = A5;
const int speedPin = A6;

// Variables to hold joystick values
int joyX, joyY, speed;

void setup() 
{
  Serial.begin(9600);
 
    pinMode(trigPins0, OUTPUT);
    pinMode(echoPins0, INPUT);
        pinMode(trigPins1, OUTPUT);
    pinMode(echoPins1, INPUT);
        pinMode(trigPins2, OUTPUT);
    pinMode(echoPins2, INPUT);
        pinMode(trigPins3, OUTPUT);
    pinMode(echoPins3, INPUT);

 pinMode(stepPins0, OUTPUT);
    pinMode(dirPins0, OUTPUT);
     pinMode(stepPins1, OUTPUT);
    pinMode(dirPins1, OUTPUT);
     pinMode(stepPins2, OUTPUT);
    pinMode(dirPins2, OUTPUT);
     pinMode(stepPins3, OUTPUT);
    pinMode(dirPins3, OUTPUT);

  stepperFL.setMaxSpeed(1000);
  stepperFL.setAcceleration(500);
  
  stepperFR.setMaxSpeed(1000);
  stepperFR.setAcceleration(500);
  
  stepperBL.setMaxSpeed(1000);
  stepperBL.setAcceleration(500);
  
  stepperBR.setMaxSpeed(1000);
  stepperBR.setAcceleration(500);

}


long readDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.034 / 2;
 return distance;


}
void loop() 
{
   long distanceLeft = readDistance(trigPins2, echoPins2);
  long distanceRight = readDistance(trigPins0, echoPins0);
  long distanceFront = readDistance(trigPins1, echoPins1);
  long distanceBack = readDistance(trigPins3, echoPins3);
 

 joyX = analogRead(joyXPin);
  joyY = analogRead(joyYPin);
  speed = analogRead(speedPin);
  int baseSpeed = map(speed, 0, 1023, 0, 1023);
  int xSpeed = map(joyX, 0, 1023, -baseSpeed, baseSpeed);
  int ySpeed = map(joyY, 0, 1023, -baseSpeed, baseSpeed);

  // Initialize motor speeds
  int speedFL = baseSpeed;
  int speedFR = baseSpeed;
  int speedBL = baseSpeed;
  int speedBR = baseSpeed;



 
if (distanceLeft <= warning) {
    Serial.println("Warning: Object too close on the LEFT side!");
    // Activate warning signal (LED/Buzzer)
  }
  if (distanceRight <= warning) {
    Serial.println("Warning: Object too close on the RIGHT side!");
    // Activate warning signal (LED/Buzzer)
  }
  if (distanceFront <= warning) {
    Serial.println("Warning: Object too close at the FRONT!");
    // Activate warning signal (LED/Buzzer)
  }
  if (distanceBack <= warning) {
    Serial.println("Warning: Object too close at the BACK!");
    // Activate warning signal (LED/Buzzer)
  }
     // Adjust speeds based on joystick movements
  // Forward/Backward control
  if (joyY > 512 + 50) { // Move forward

    for (int x = 0; x < 200; x++) 
    {
      digitalWrite(stepPins2, HIGH);
      digitalWrite(stepPins3, HIGH);
    }
     speedBL += ySpeed;
    speedBR += ySpeed;
  } else if (joyY < 512 - 50) { // Move backward

    for (int x = 0; x < 200; x++) 
    {
      digitalWrite(stepPins0, HIGH);
      digitalWrite(stepPins1, HIGH);
    }
    speedFL += -ySpeed;
    speedFR += -ySpeed;
  }
  
  // Left/Right control
  if (joyX > 512 + 50) { // Move right

    for (int x = 0; x < 200; x++) 
    {
      digitalWrite(stepPins1, HIGH);
      digitalWrite(stepPins2, HIGH);

    }
    speedFL += xSpeed;
    speedBL += xSpeed;
  } else if (joyX < 512 - 50) { // Move left

    for (int x = 0; x < 200; x++) 
    {
      digitalWrite(stepPins0, HIGH);
      digitalWrite(stepPins3, HIGH);
      
    }
speedFR += -xSpeed;
    speedBR += -xSpeed;
  }

  // Up/Down control (not used)
  // int zSpeed = map(joyZ, 0, 1023, -baseSpeed, baseSpeed);
  // Not implementing up/down control in this configuration
  
  // Set stepper speeds
 stepperFL.setSpeed(speedFL);
  stepperFR.setSpeed(speedFR);
  stepperBL.setSpeed(speedBL);
  stepperBR.setSpeed(speedBR);

  // Move steppers
  stepperFL.runSpeed();
  stepperFR.runSpeed();
  stepperBL.runSpeed();
  stepperBR.runSpeed();

  delay(100); 
}