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

// Constants
const int ENCODER_CLK = 2;
const int ENCODER_DT = 4;
const int ENCODER_SW = 6;
const int BIG_BUTTON = 7;
const int SMALL_BUTTON = 8;
const int LIMIT_SWITCH_UP = 13;
const int LIMIT_SWITCH_DOWN = 10;
const int STEPPER_X_STEP = 3;
const int STEPPER_X_DIR = 5;
const int STEPPER_Y_STEP = 0;
const int STEPPER_Y_DIR = 1;
const int SERVO_PIN = 11;

// Variables
volatile int encoderValue = 0;
int lastEncoderValue = 0;
float canvasLeft = 0;
float canvasRight = 0;
bool canvasSet = false;
bool jogging = false;
int joggingProgress = 0;
const float stepPerMeter = 4000.0; // Assuming 4000 steps per meter for quarter steps

// LCD and Stepper setup
LiquidCrystal_I2C lcd(0x27, 20, 4);
AccelStepper stepperX(AccelStepper::DRIVER, STEPPER_X_STEP, STEPPER_X_DIR);
AccelStepper stepperY(AccelStepper::DRIVER, STEPPER_Y_STEP, STEPPER_Y_DIR);
Servo markerServo;

void setup() {
  // LCD setup
  lcd.init();
  lcd.backlight();
  
  // Encoder setup
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  pinMode(ENCODER_SW, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), readEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_DT), readEncoder, CHANGE);
  
  // Buttons setup
  pinMode(BIG_BUTTON, INPUT_PULLUP);
  pinMode(SMALL_BUTTON, INPUT_PULLUP);
  
  // Limit switches setup
  pinMode(LIMIT_SWITCH_UP, INPUT_PULLUP);
  pinMode(LIMIT_SWITCH_DOWN, INPUT_PULLUP);
  
  // Stepper motors setup
  stepperX.setMaxSpeed(1000);
  stepperX.setAcceleration(500);
  stepperY.setMaxSpeed(1000);
  stepperY.setAcceleration(500);
  
  // Servo setup
  markerServo.attach(SERVO_PIN);
  markerServo.write(0);
  
  // Initial LCD display
  lcd.setCursor(0, 0);
  lcd.print("Adjust min distance");
  lcd.setCursor(0, 1);
  lcd.print("turn knob left/right");
  lcd.setCursor(0, 2);
  lcd.print("0.0 meters");
}

void loop() {
  if (!canvasSet) {
    handleCanvasSetting();
  } else if (jogging) {
    handleJogging();
  }
}

void handleCanvasSetting() {
  if (digitalRead(BIG_BUTTON) == LOW) {
    canvasRight = encoderValue * 0.5;
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Confirm details");
    lcd.setCursor(0, 1);
    lcd.print(canvasLeft, 1);
    lcd.print(" meters left -> ");
    lcd.print(canvasRight, 1);
    lcd.print(" meters right");
    lcd.setCursor(0, 2);
    lcd.print("Big button = confirm");
    lcd.setCursor(0, 3);
    lcd.print("Small button = redo");

    while (true) {
      if (digitalRead(BIG_BUTTON) == LOW) {
        canvasSet = true;
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Jogging...");
        jogging = true;
        break;
      } else if (digitalRead(SMALL_BUTTON) == LOW) {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Adjust min distance");
        lcd.setCursor(0, 1);
        lcd.print("turn knob left/right");
        lcd.setCursor(0, 2);
        lcd.print("0.0 meters");
        canvasLeft = 0;
        canvasRight = 0;
        encoderValue = 0;
        break;
      }
    }
  } else if (encoderValue != lastEncoderValue) {
    lcd.setCursor(0, 2);
    lcd.print(encoderValue * 0.5, 1);
    lcd.print(" meters   ");
    lastEncoderValue = encoderValue;
  }
}

void handleJogging() {
  if (joggingProgress == 0) {
    stepperX.moveTo(canvasLeft * stepPerMeter);
    joggingProgress++;
  } else if (joggingProgress == 1 && stepperX.distanceToGo() == 0) {
    stepperX.moveTo(canvasRight * stepPerMeter);
    joggingProgress++;
  } else if (joggingProgress == 2 && stepperX.distanceToGo() == 0) {
    // Move Y axis up until limit switch is triggered
    while (digitalRead(LIMIT_SWITCH_UP) != LOW) {
      stepperY.move(-10);
      stepperY.run();
    }
    stepperY.setCurrentPosition(0); // Set current position as reference
    joggingProgress++;
  } else if (joggingProgress == 3) {
    // Move Y axis down until limit switch is triggered
    while (digitalRead(LIMIT_SWITCH_DOWN) != LOW) {
      stepperY.move(10);
      stepperY.run();
    }
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Jogging complete");
    jogging = false;
  }
}

void readEncoder() {
  static unsigned long lastInterruptTime = 0;
  unsigned long interruptTime = millis();
  if (interruptTime - lastInterruptTime > 5) {
    int newClk = digitalRead(ENCODER_CLK);
    if (newClk != digitalRead(ENCODER_DT)) {
      encoderValue++;
    } else {
      encoderValue--;
    }
  }
  lastInterruptTime = interruptTime;
}

void moveTo(float x, float y) {
  stepperX.moveTo(x * stepPerMeter);
  stepperY.moveTo(y * stepPerMeter);
  while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
    stepperX.run();
    stepperY.run();
  }
}

void dragMarkerTo(float x, float y) {
  markerServo.write(100); // Lower the marker
  moveTo(x, y);
  markerServo.write(0);   // Raise the marker
}