/*
  2 Stepper Motors + Ultrasonic + Line Follower (Switch)
  - Stepper 1: Step 19, Dir 21 (Left Motor)
  - Stepper 2: Step 22, Dir 23 (Right Motor)
  - Ultrasonic: Trig 5, Echo 18
  - Left Sensor (Switch): Pin 25
  - Right Sensor (Switch): Pin 26
*/

// --- Pin Definitions ---
const int trigPin = 5;
const int echoPin = 18;
const int leftSensorPin = 25;  // Switch 1
const int rightSensorPin = 26; // Switch 2

// 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; // cm

// --- Helper Functions ---

void moveForward() {
  digitalWrite(dirPin1, HIGH); // Left Forward
  digitalWrite(dirPin2, HIGH); // Right Forward
  
  digitalWrite(stepPin1, HIGH);
  digitalWrite(stepPin2, HIGH);
  delayMicroseconds(1000); 
  digitalWrite(stepPin1, LOW);
  digitalWrite(stepPin2, LOW);
  delayMicroseconds(1000);
}

void turnRight() {
  digitalWrite(dirPin1, HIGH); // Left Forward
  digitalWrite(dirPin2, LOW);  // Right Backward (Spin)
  
  digitalWrite(stepPin1, HIGH);
  digitalWrite(stepPin2, HIGH);
  delayMicroseconds(2000); // Slower for turning
  digitalWrite(stepPin1, LOW);
  digitalWrite(stepPin2, LOW);
  delayMicroseconds(2000);
}

void turnLeft() {
  digitalWrite(dirPin1, LOW);  // Left Backward (Spin)
  digitalWrite(dirPin2, HIGH); // Right Forward
  
  digitalWrite(stepPin1, HIGH);
  digitalWrite(stepPin2, HIGH);
  delayMicroseconds(2000); 
  digitalWrite(stepPin1, LOW);
  digitalWrite(stepPin2, LOW);
  delayMicroseconds(2000);
}

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

// --- Main Setup ---

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

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

  // Line Sensors (Switches)
  pinMode(leftSensorPin, INPUT); 
  pinMode(rightSensorPin, INPUT);

  // Motors
  pinMode(stepPin1, OUTPUT);
  pinMode(dirPin1, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);

  Serial.println("System Started");
}

// --- Main Loop ---

void loop() {
  // 1. Check Distance
  int distance = getDistance();
  
  // 2. Read Line Sensors (Switches)
  // In simulation: HIGH means Switch is ON (Simulating "Line Detected")
  int leftVal = digitalRead(leftSensorPin);
  int rightVal = digitalRead(rightSensorPin);

  // Debugging info
  Serial.print("Dist: "); Serial.print(distance);
  Serial.print("cm | L: "); Serial.print(leftVal);
  Serial.print(" R: "); Serial.println(rightVal);

  // 3. Logic Control
  
  if (distance < obstacleThreshold) {
    // PRIORITY 1: Obstacle Detected -> Turn Right to avoid
    turnRight();
    
  } else if (leftVal == HIGH && rightVal == LOW) {
    // Line is on the Left -> Turn Left
    turnLeft();
    
  } else if (rightVal == HIGH && leftVal == LOW) {
    // Line is on the Right -> Turn Right
    turnRight();
    
  } else {
    // Path clear, no line detected (or centered) -> Go Forward
    moveForward();
  }
}