#include <Servo.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

Servo frontLeftLeg;
Servo frontRightLeg;
Servo backLeftLeg;
Servo backRightLeg;

#define FRONT_LEFT_PIN 9
#define FRONT_RIGHT_PIN 10
#define BACK_LEFT_PIN 11
#define BACK_RIGHT_PIN 12
#define LED_LEFT_PIN 7
#define LED_RIGHT_PIN 8

const int STAND = 90;
const int UP = 60;
const int FORWARD = 120;

void setup() {
  lcd.init();
  lcd.backlight();
  
  frontLeftLeg.attach(FRONT_LEFT_PIN);
  frontRightLeg.attach(FRONT_RIGHT_PIN);
  backLeftLeg.attach(BACK_LEFT_PIN);
  backRightLeg.attach(BACK_RIGHT_PIN);
  
  pinMode(LED_LEFT_PIN, OUTPUT);
  pinMode(LED_RIGHT_PIN, OUTPUT);
  
  standStill();
  
  lcd.setCursor(0, 0);
  lcd.print("Quadruped Robot");
  lcd.setCursor(0, 1);
  lcd.print("Standing...");
}

void loop() {
  walkForward();
  delay(2000);
  standStill();
  delay(1000);
}

void standStill() {
  digitalWrite(LED_LEFT_PIN, LOW);
  digitalWrite(LED_RIGHT_PIN, LOW);
  
  frontLeftLeg.write(STAND);
  frontRightLeg.write(STAND);
  backLeftLeg.write(STAND);
  backRightLeg.write(STAND);
  
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Status:");
  lcd.setCursor(0, 1);
  lcd.print("Standing Still");
}

void walkForward() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Status:");
  lcd.setCursor(0, 1);
  lcd.print("Walking Forward");
  
  // Step 1: Left side LED and diagonal legs
  digitalWrite(LED_LEFT_PIN, HIGH);
  digitalWrite(LED_RIGHT_PIN, LOW);
  frontLeftLeg.write(UP);
  backRightLeg.write(UP);
  delay(200);
  frontLeftLeg.write(FORWARD);
  backRightLeg.write(FORWARD);
  delay(200);
  
  frontLeftLeg.write(STAND);
  backRightLeg.write(STAND);
  delay(200);
  
  // Step 2: Right side LED and diagonal legs
  digitalWrite(LED_LEFT_PIN, LOW);
  digitalWrite(LED_RIGHT_PIN, HIGH);
  frontRightLeg.write(UP);
  backLeftLeg.write(UP);
  delay(200);
  frontRightLeg.write(FORWARD);
  backLeftLeg.write(FORWARD);
  delay(200);
  
  frontRightLeg.write(STAND);
  backLeftLeg.write(STAND);
  delay(200);
}