const int leftStep = 2, leftDir = 3;
const int rightStep = 4, rightDir = 5;
const int trig = 6, echo = 7;

const int TARGET_DISTANCE = 30; // المسافة المطلوبة 30 سم
const int STEPS_PER_CM = 10;    // عدد الخطوات لقطع 1 سم

void setup() {
  pinMode(leftStep, OUTPUT);
  pinMode(leftDir, OUTPUT);
  pinMode(rightStep, OUTPUT);
  pinMode(rightDir, OUTPUT);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  Serial.begin(9600);
  Serial.println("🤖 روبوت ذكي - يصحح المسافة إلى 30 سم");
}

void loop() {
  int currentDistance = getDistance();
  
  Serial.print("📏 المسافة الحالية: ");
  Serial.print(currentDistance);
  Serial.println(" سم");
  
  // احسب كم يجب أن يتحرك ليكون على بعد 30 سم
  int distanceToMove = currentDistance - TARGET_DISTANCE;
  
  if (abs(distanceToMove) > 2) { // إذا الفرق أكثر من 2 سم
    moveToTarget(distanceToMove);
  }
  else {
    Serial.println("🎯 المسافة صحيحة 30 سم! ✅");
  }
  
  delay(2000); // انتظر 2 ثانية بين القراءات
}

int getDistance() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  
  long duration = pulseIn(echo, HIGH, 30000);
  if (duration == 0) return 400;
  return duration * 0.034 / 2;
}

void moveToTarget(int distanceToMove) {
  int stepsNeeded = distanceToMove * STEPS_PER_CM;
  
  Serial.print("🔄 يحتاج التحرك: ");
  Serial.print(distanceToMove);
  Serial.print(" سم (");
  Serial.print(stepsNeeded);
  Serial.println(" خطوة)");
  
  if (distanceToMove > 0) {
    // المسافة الحالية أكبر من 30 سم - تحرك للأمام
    Serial.println("➡️ أتحرك للأمام لأقترب من الهدف");
    moveForward(abs(stepsNeeded));
  }
  else {
    // المسافة الحالية أقل من 30 سم - تحرك للخلف
    Serial.println("⬅️ أتحرك للخلف لأبتعد عن الهدف");
    moveBackward(abs(stepsNeeded));
  }
  
  // بعد الحركة، اقرأ المسافة الجديدة
  delay(1000);
  int newDistance = getDistance();
  Serial.print("📏 المسافة الجديدة: ");
  Serial.print(newDistance);
  Serial.println(" سم");
}

void moveForward(int steps) {
  digitalWrite(leftDir, HIGH);
  digitalWrite(rightDir, HIGH);
  
  for (int i = 0; i < steps; i++) {
    digitalWrite(leftStep, HIGH);
    digitalWrite(rightStep, HIGH);
    delay(5);
    digitalWrite(leftStep, LOW);
    digitalWrite(rightStep, LOW);
    delay(5);
  }
}

void moveBackward(int steps) {
  digitalWrite(leftDir, LOW);
  digitalWrite(rightDir, LOW);
  
  for (int i = 0; i < steps; i++) {
    digitalWrite(leftStep, HIGH);
    digitalWrite(rightStep, HIGH);
    delay(5);
    digitalWrite(leftStep, LOW);
    digitalWrite(rightStep, LOW);
    delay(5);
  }
}