#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <AccelStepper.h>

// LCD ekran
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Motor pinleri
#define LEFT_DIR_PIN 10
#define LEFT_STEP_PIN 11
#define RIGHT_DIR_PIN 12
#define RIGHT_STEP_PIN 13
AccelStepper leftMotor(AccelStepper::DRIVER, LEFT_STEP_PIN, LEFT_DIR_PIN);
AccelStepper rightMotor(AccelStepper::DRIVER, RIGHT_STEP_PIN, RIGHT_DIR_PIN);

// Ultrasonik sensör pinleri
#define TRIG_FRONT 2
#define ECHO_FRONT 3
#define TRIG_BACK 4
#define ECHO_BACK 5
#define TRIG_RIGHT 6
#define ECHO_RIGHT 7
#define TRIG_LEFT 8
#define ECHO_LEFT 9

// Harita boyutu ve adım
const int roomWidth = 7000;
const int roomHeight = 4000;
const int lcdCols = 20;
const int lcdRows = 4;
int robotX = 0;
int robotY = 0;
const int stepSizeX = 350;
const int stepSizeY = 1000;
const int SAFE_DISTANCE = 30; // cm

bool visited[lcdCols][lcdRows];

void setup() {
  lcd.init();
  lcd.backlight();

  leftMotor.setMaxSpeed(500);
  rightMotor.setMaxSpeed(500);
  leftMotor.setAcceleration(200);
  rightMotor.setAcceleration(200);

  pinMode(TRIG_FRONT, OUTPUT); pinMode(ECHO_FRONT, INPUT);
  pinMode(TRIG_BACK, OUTPUT);  pinMode(ECHO_BACK, INPUT);
  pinMode(TRIG_RIGHT, OUTPUT); pinMode(ECHO_RIGHT, INPUT);
  pinMode(TRIG_LEFT, OUTPUT);  pinMode(ECHO_LEFT, INPUT);

  lcd.setCursor(0, 0);
  lcd.print("Harita Basliyor...");
  delay(1000);

  // İlk pozisyonu ziyaret edilmiş olarak işaretle
  visited[robotX / stepSizeX][robotY / stepSizeY] = true;
}

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

void updateLCD() {
  lcd.clear();
  for (int row = 0; row < lcdRows; row++) {
    for (int col = 0; col < lcdCols; col++) {
      lcd.setCursor(col, row);
      lcd.print(visited[col][row] ? "#" : ".");
    }
  }
  int lcdX = constrain(robotX / stepSizeX, 0, lcdCols - 1);
  int lcdY = constrain(robotY / stepSizeY, 0, lcdRows - 1);
  lcd.setCursor(lcdX, lcdY);
  lcd.print("R");
}

bool move(int dx, int dy, const char* direction) {
  int nextX = robotX + dx;
  int nextY = robotY + dy;
  int col = nextX / stepSizeX;
  int row = nextY / stepSizeY;

  if (col >= 0 && col < lcdCols && row >= 0 && row < lcdRows && !visited[col][row]) {
    leftMotor.move(dx > 0 || dy > 0 ? 200 : -200);
    rightMotor.move(dx > 0 ? 200 : dy < 0 ? 200 : -200);
    while (leftMotor.isRunning() || rightMotor.isRunning()) {
      leftMotor.run();
      rightMotor.run();
    }

    robotX = nextX;
    robotY = nextY;
    visited[col][row] = true;

    lcd.setCursor(0, 0);
    lcd.print(direction);
    delay(500);
    return true;
  }
  return false;
}

void loop() {
  long front = readDistance(TRIG_FRONT, ECHO_FRONT);
  long back = readDistance(TRIG_BACK, ECHO_BACK);
  long rightD = readDistance(TRIG_RIGHT, ECHO_RIGHT);
  long leftD = readDistance(TRIG_LEFT, ECHO_LEFT);

  if (front > SAFE_DISTANCE && move(stepSizeX, 0, "Geri ")) {
  } else if (rightD > SAFE_DISTANCE && move(0, stepSizeY, "Saga  ")) {
  } else if (leftD > SAFE_DISTANCE && move(0, -stepSizeY, "Sola  ")) {
  } else if (back > SAFE_DISTANCE && move(-stepSizeX, 0,"Ileri  " )) {
  } else {
    lcd.setCursor(0, 0);
    lcd.print("Cikis yok!      ");
  }

  updateLCD();
  delay(300);
}