#include <AccelStepper.h>

// Define IR sensor connections
const int left_IR = 6;
const int right_IR = 5;
const int mid_IR = 7;

// Define the stepper motor connections
const int motorLeftStepPin = 13;  // Connect to the STEP pin of the left motor
const int motorLeftDirPin = 12;   // Connect to the DIR pin of the left motor
const int motorRightStepPin = 4;  // Connect to the STEP pin of the right motor
const int motorRightDirPin = 2;   // Connect to the DIR pin of the right motor

// Constants
const int speedFactor = 5;

// Create a stepper motor instances
AccelStepper leftMotor(AccelStepper::DRIVER, motorLeftStepPin, motorLeftDirPin);
AccelStepper rightMotor(AccelStepper::DRIVER, motorRightStepPin, motorRightDirPin);

void setup() {
  Serial.begin(9600);
  pinMode(left_IR, INPUT);
  pinMode(right_IR, INPUT);
  pinMode(mid_IR, INPUT);

  // Set maximum speed for both stepper motors
  leftMotor.setMaxSpeed(25 * speedFactor);
  rightMotor.setMaxSpeed(25 * speedFactor);
}

void forward() {
  leftMotor.setSpeed(10 * speedFactor);
  rightMotor.setSpeed(10 * speedFactor);
  leftMotor.runSpeed();
  rightMotor.runSpeed();
}

void left() {
  leftMotor.setSpeed(0);
  rightMotor.setSpeed(25 * speedFactor);
  leftMotor.runSpeed();
  rightMotor.runSpeed();
}

void right() {
  leftMotor.setSpeed(25 * speedFactor);
  rightMotor.setSpeed(0);
  leftMotor.runSpeed();
  rightMotor.runSpeed();
}

void stop() {
  leftMotor.setSpeed(0);
  rightMotor.setSpeed(0);
  leftMotor.runSpeed();
  rightMotor.runSpeed();
}


void loop() {
  // Read sensor values
  int leftSensor = digitalRead(left_IR);
  int rightSensor = digitalRead(right_IR);
  int midSensor = digitalRead(mid_IR);

  // Determine the direction based on sensor readings
  if (midSensor) {
    //Serial.println("forward");
    forward();
  } else if (!rightSensor && leftSensor) {
    Serial.println("right");
    right();
  } else if (rightSensor && !leftSensor) {
    Serial.println("left");
    left();
  } else {
    Serial.println("stop");
    stop();
  }
}