// Pin assignments for ultrasonic sensor
const int trigPin = 9; 
const int echoPin = 10; 

// Pin assignments for stepper motor drivers
const int stepPin1 = 3;
const int dirPin1 = 4;
const int stepPin2 = 5;
const int dirPin2 = 6;

void setup() {
  // Initialize pins for ultrasonic sensor
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  // Initialize pins for stepper motor drivers
  pinMode(stepPin1, OUTPUT);
  pinMode(dirPin1, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  
  Serial.begin(9600);
}

long readUltrasonicDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  long duration = pulseIn(echoPin, HIGH);
  long distance = (duration / 2) / 29.1;
  
  return distance;
}

void moveForward() {
  digitalWrite(dirPin1, HIGH);
  digitalWrite(dirPin2, HIGH);
  for(int x = 0; x < 200; x++) {
    digitalWrite(stepPin1, HIGH);
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(1000);
    digitalWrite(stepPin1, LOW);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(1000);
  }
}

void turnRight() {
  digitalWrite(dirPin1, HIGH);
  digitalWrite(dirPin2, LOW);
  for(int x = 0; x < 100; x++) {
    digitalWrite(stepPin1, HIGH);
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(1000);
    digitalWrite(stepPin1, LOW);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(1000);
  }
}

void turnLeft() {
  digitalWrite(dirPin1, LOW);
  digitalWrite(dirPin2, HIGH);
  for(int x = 0; x < 100; x++) {
    digitalWrite(stepPin1, HIGH);
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(1000);
    digitalWrite(stepPin1, LOW);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(1000);
  }
}

void loop() {
  long distance = readUltrasonicDistance();
  Serial.println(distance);
  
  if (distance < 20) { // if obstacle is closer than 20 cm
    // Turn away
    turnRight();
    delay(500);
  } else {
    // Move forward
    moveForward();
  }
  delay(100);
}