/*
  2 Stepper Motors + Ultrasonic Sensor Demo
  - Stepper 1: Step Pin 19, Dir Pin 21 (Left)
  - Stepper 2: Step Pin 22, Dir Pin 23 (Right)
  - Ultrasonic: Trig Pin 5, Echo Pin 18
*/

// --- Pin Definitions ---
const int trigPin = 5;
const int echoPin = 18;

// Motor 1 (Left)
const int stepPin1 = 19;
const int dirPin1 = 21;

// Motor 2 (Right)
const int stepPin2 = 22;
const int dirPin2 = 23;

// --- Settings ---
const int obstacleThreshold = 20; // Turn if object is closer than 20cm

// --- Helper Functions ---

// Function to move both motors forward
void moveForward() {
  // Set Both Motors to Forward
  digitalWrite(dirPin1, HIGH); 
  digitalWrite(dirPin2, HIGH); 

  // Perform Step
  digitalWrite(stepPin1, HIGH);
  digitalWrite(stepPin2, HIGH);
  delayMicroseconds(1000); 
  
  digitalWrite(stepPin1, LOW);
  digitalWrite(stepPin2, LOW);
  delayMicroseconds(1000);
}

// Function to Turn Right (Spin Turn)
void turnRight() {
  // Left Motor moves Forward (HIGH)
  digitalWrite(dirPin1, HIGH); 
  
  // Right Motor moves Backward (LOW) - This creates the spin
  digitalWrite(dirPin2, LOW); 

  // Perform Step (Slightly slower for turning to prevent stalling)
  digitalWrite(stepPin1, HIGH);
  digitalWrite(stepPin2, HIGH);
  delayMicroseconds(2000); 
  
  digitalWrite(stepPin1, LOW);
  digitalWrite(stepPin2, LOW);
  delayMicroseconds(2000);
}

void stopMotors() {
  digitalWrite(stepPin1, LOW);
  digitalWrite(stepPin2, LOW);
}

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

// --- Main Setup and Loop ---

void setup() {
  Serial.begin(115200);

  // Setup Ultrasonic pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Setup Motor pins
  pinMode(stepPin1, OUTPUT);
  pinMode(dirPin1, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);

  Serial.println("System Started");
}

void loop() {
  // 1. Get Distance
  int distance = getDistance();
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // 2. Logic: Move Forward if clear, Turn Right if obstacle
  if (distance > obstacleThreshold) {
    moveForward();
  } else {
    // Determine obstacle! Turning Right until path is clear.
    turnRight(); 
  }
}