/*
X_DIR LOW = to endstop
Y_DIR HIGH = to endstop
*/

#include <Servo.h>

// Pin Definitions
#define X_STEP_PIN 17
#define X_DIR_PIN 16
#define X_EN_PIN 18
#define X_ENDSTOP_PIN 9

#define Y_STEP_PIN 20
#define Y_DIR_PIN 19
#define Y_EN_PIN 21
#define Y_ENDSTOP_PIN 8

//Speed settings
#define SPEED 600
#define HOMING_DELAY 1000

//Step settings
#define MM_PER_STEP 0.02
#define STEP_PER_REV 1600
#define STEPS_PER_TILE 1090
#define STEPS_TILE_ONE 100

bool initialHome = false;

//Initiate Servo
Servo myServo;
#define SERVO_SPEED 100

// Setup
void setup() {
  Serial.begin(115200);
  // X axis
  pinMode(X_STEP_PIN, OUTPUT);
  pinMode(X_DIR_PIN, OUTPUT);
  pinMode(X_EN_PIN, OUTPUT);
  digitalWrite(X_EN_PIN, HIGH);  // Disable motor by default

  // Y axis
  pinMode(Y_STEP_PIN, OUTPUT);
  pinMode(Y_DIR_PIN, OUTPUT);
  pinMode(Y_EN_PIN, OUTPUT);
  digitalWrite(Y_EN_PIN, HIGH);  // Disable motor by default

  // Endstop inputs
  pinMode(X_ENDSTOP_PIN, INPUT);
  pinMode(Y_ENDSTOP_PIN, INPUT);

  //Servo Setup
  myServo.attach(22);  // Attach the servo to digital pin 9
  myServo.write(0);    //Sets the servo to my "Neutral Position"



  //Solonoid Setup
  pinMode(10, OUTPUT);
}

void loop() {

  if (!initialHome) {
    home();
  }

  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    command.trim();  // Remove whitespace/newlines

    if (command.length() == 0) return;

    if (isAxisCommand(command)) {
      char axis = command.charAt(0);
      char direction = command.charAt(1);
      int value = command.substring(2).toInt();

      Serial.print("Axis: ");
      Serial.print(axis);
      Serial.print(" | Direction: ");
      Serial.print(direction);
      Serial.print(" | Steps: ");
      Serial.println(value);

      if (axis == 'x') {
        if (direction == '+') {
          stepX(value, HIGH);
        } else {
          stepX(value, LOW);
        }
      } else {
        if (direction == '+') {
          stepY(value, LOW);
        } else {
          stepY(value, HIGH);
        }
      }
    } else {
      // Word command handling
      command.toLowerCase();
      if (command == "d1") {
        Serial.println("Building Dungeon 1");
        dungeon1();
      } else if (command == "raise") {
        Serial.println("raise the tile");
        raise();
      } else if (command == "lower") {
        Serial.println("lower the tile");
        lower();
      } else if (command == "pos1") {
        Serial.println("Moving to position 1");
        pos1();
      } else if (command == "home") {
        Serial.println("Homing...");
        home();
      } else {
        Serial.println("Unknown command.");
      }
    }
  }
}

void pos1() {
  //Move to first tile
  delay(500);
  home();
  delay(500);
  stepX(110, HIGH);
  stepY(160, LOW);
}

void dungeon1() {
  delay(500);
  home();

  //Move to  tile 1 and raise it
  delay(500);
  stepX(110, HIGH);
  stepY(170, LOW);
  delay(500);
  raise();

  //Move to  tile 2 and raise it
  delay(500);
  stepY(1120, LOW);
  delay(500);
  raise();

  //Move to  tile 3 and raise it
  delay(500);
  stepY(1120, LOW);
  delay(500);
  raise();

  //Move to  tile 4 and raise it
  delay(500);
  stepY(1120, LOW);
  delay(500);
  raise();

  //Move to  tile 5 and raise it
  delay(500);
  stepY(1120, LOW);
  delay(500);
  raise();

  //Move to  tile 6 and raise it
  delay(500);
  stepX(1100, HIGH);
  delay(500);
  raise();

  //Move to  tile 7 and raise it
  delay(500);
  stepX(1100, HIGH);
  delay(500);
  raise();

  //Move to  tile 8and raise it
  delay(500);
  stepX(1100, HIGH);
  delay(500);
  raise();

  //Move to  tile 9 and raise it
  delay(500);
  stepX(1100, HIGH);
  delay(500);
  raise();

  //Move to  tile 10 and raise it
  delay(500);
  stepY(1150, HIGH);
  delay(500);
  raise();

  //Move to  tile 11 and raise it
  delay(500);
  stepY(2200, HIGH);
  delay(500);
  raise();

  //Move to  tile 12 and raise it
  delay(500);
  stepY(1100, HIGH);
  delay(500);
  raise();

  //Move to  tile 13 and raise it
  delay(500);
  stepX(1100, LOW);
  delay(500);
  raise();

  //Move to  tile 14 and raise it
  delay(500);
  stepX(1100, LOW);
  delay(500);
  raise();

  //Move to  tile 15 and raise it
  delay(500);
  stepX(1100, LOW);
  delay(500);
  raise();
}


// Step function for X-axis
void stepX(int steps, bool dir) {
  digitalWrite(X_EN_PIN, LOW);   // Enable motor
  digitalWrite(X_DIR_PIN, dir);  // Set direction

  for (int i = 0; i < abs(steps); i++) {
    digitalWrite(X_STEP_PIN, HIGH);
    delayMicroseconds(SPEED);  // Adjust for speed
    digitalWrite(X_STEP_PIN, LOW);
    delayMicroseconds(SPEED);
  }

  digitalWrite(X_EN_PIN, HIGH);  // Disable motor
}

// Step function for Y-axis
void stepY(int steps, bool dir) {
  digitalWrite(Y_EN_PIN, LOW);   // Enable motor
  digitalWrite(Y_DIR_PIN, dir);  // Set direction

  for (int i = 0; i < abs(steps); i++) {
    digitalWrite(Y_STEP_PIN, HIGH);
    delayMicroseconds(SPEED);  // Adjust for speed
    digitalWrite(Y_STEP_PIN, LOW);
    delayMicroseconds(SPEED);
  }

  digitalWrite(Y_EN_PIN, HIGH);  // Disable motor
}

void home() {
  Serial.println("Starting Homing...");

  // === Home X Axis ===
  digitalWrite(X_EN_PIN, LOW);   // Enable X motor
  digitalWrite(X_DIR_PIN, LOW);  // Move X toward endstop

  Serial.println("Homing X Axis...");
  while (digitalRead(X_ENDSTOP_PIN) == HIGH) {  // Wait until triggered (LOW)
    digitalWrite(X_STEP_PIN, HIGH);
    delayMicroseconds(HOMING_DELAY);
    digitalWrite(X_STEP_PIN, LOW);
    delayMicroseconds(HOMING_DELAY);
  }
  Serial.println("X Axis Homed");
  delay(100);

  // === Home Y Axis ===
  digitalWrite(Y_EN_PIN, LOW);    // Enable Y motor
  digitalWrite(Y_DIR_PIN, HIGH);  // Move Y toward endstop

  Serial.println("Homing Y Axis...");
  while (digitalRead(Y_ENDSTOP_PIN) == HIGH) {  // Wait until triggered (LOW)
    digitalWrite(Y_STEP_PIN, HIGH);
    delayMicroseconds(HOMING_DELAY);
    digitalWrite(Y_STEP_PIN, LOW);
    delayMicroseconds(HOMING_DELAY);
  }
  Serial.println("Y Axis Homed");
  delay(100);

  Serial.println("All Axes Homed");

  digitalWrite(X_EN_PIN, HIGH);
  digitalWrite(Y_EN_PIN, HIGH);

  initialHome = true;
}

void raise() {

  delay(500);

  myServo.write(180);

  delay(500);

  digitalWrite(10, HIGH);  //Activate Solonoid

  delay(500);
  // release solonoid if stuck
  myServo.write(170);
  delay(200);
  myServo.write(180);
  delay(500);


  // Turn slowly
  for (int pos = 180; pos >= 0; pos--) {
    myServo.write(pos);  // Move to 'pos' degrees
    delay(15);           // Wait 15 ms for servo to reach position
  }

  digitalWrite(10, LOW);  //Deactivate Solonoid

  // release solonoid if stuck
  delay(300);
  myServo.write(10);
  delay(100);
  myServo.write(0);
  delay(200);
}

void lower() {
  delay(500);

  myServo.write(0);

  delay(500);

  digitalWrite(10, HIGH);  //Activate Solonoid

  delay(500);

  // release solonoid if stuck
  myServo.write(10);
  delay(100);
  myServo.write(0);

  delay(500);

  // Turn slowly
  for (int pos = 0; pos <= 180; pos++) {
    myServo.write(pos);  // Move to 'pos' degrees
    delay(15);           // Wait 15 ms for servo to reach position
  }

  delay(500);

  digitalWrite(10, LOW);  //Deactivate Solonoid

  // release solonoid if stuck
  delay(200);
  myServo.write(170);
  delay(200);
  myServo.write(180);
  delay(200);
}

bool isAxisCommand(String cmd) {
  if (cmd.length() < 3) return false;
  char axis = cmd.charAt(0);
  char dir = cmd.charAt(1);

  return (axis == 'x' || axis == 'y') && (dir == '+' || dir == '-') && isDigit(cmd.charAt(2));
}