#include <LiquidCrystal.h>

// Initialize the library by associating any needed LCD interface pin
// with the Arduino pin number it is connected to
const int rs = 7, en = 8, d4 = 9, d5 = 10, d6 = 11, d7 = 12;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

// Size of the field
const int fieldWidth = 4;
const int fieldHeight = 4;

// Drone's position
int droneX = 0;
int droneY = 0;

void setup() {
  // Set up the LCD's number of columns and rows
  lcd.begin(16, 2);
  lcd.print("Drone Sim Init");
  delay(2000);
  lcd.clear();
  lcd.print("Starting...");
  delay(1000);
  lcd.clear();
  
  // Initialize the starting position of the drone
  updateLCD();
}

void loop() {
  // Move the drone in a zigzag pattern
  for (int y = 0; y < fieldHeight; y++) {
    if (y % 2 == 0) {
      for (int x = 0; x < fieldWidth; x++) {
        moveDroneTo(x, y);
      }
    } else {
      for (int x = fieldWidth - 1; x >= 0; x--) {
        moveDroneTo(x, y);
      }
    }
  }
}

void moveDroneTo(int x, int y) {
  droneX = x;
  droneY = y;
  updateLCD();
  delay(500); // Wait for half a second
}

void updateLCD() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Drone Position:");
  lcd.setCursor(0, 1);
  lcd.print("(");
  lcd.print(droneX);
  lcd.print(", ");
  lcd.print(droneY);
  lcd.print(")");
}