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

// LCD and Stepper definitions
LiquidCrystal_I2C lcd(0x27, 16, 2);  // I2C LCD
AccelStepper stepper(AccelStepper::DRIVER, 9, 8);  // Stepper driver: STEP = 9, DIR = 8

// Relay and stepper control pins
#define RELAY1_PIN 4
#define RELAY2_PIN 5
#define RELAY3_PIN 6
#define ENABLE_PIN 12
#define SLEEP_PIN 3  // Moved SLEEP to pin 3

// Button pins
#define BUTTON1_PIN A0
#define BUTTON2_PIN A1
#define BUTTON3_PIN A2
#define BUTTON4_PIN 7
#define BUTTON5_PIN 13

// Stepper parameters
long stepperPosition = 0;              // Stepper position in mm
const float stepsPerMM = 200.0 / 10.0;  // 200 steps per revolution, 10 mm per revolution
unsigned long lastDebounceTime = 0;
const unsigned long debounceDelay = 200;  // 200 ms debounce delay

// EEPROM address for storing stepper position
#define EEPROM_ADDR 0

void setup() {
  Serial.begin(9600);

  // Initialize LCD
  lcd.init();
  lcd.backlight();

  // Initialize stepper motor
  pinMode(ENABLE_PIN, OUTPUT);
  pinMode(SLEEP_PIN, OUTPUT);  // SLEEP pin as output
  digitalWrite(SLEEP_PIN, HIGH);  // Ensure the driver is awake (SLEEP pin HIGH)
  digitalWrite(ENABLE_PIN, LOW);  // Enable stepper driver
  stepper.setMaxSpeed(1000);  // Maximum speed
  stepper.setAcceleration(500);  // Acceleration

  // Initialize relays
  pinMode(RELAY1_PIN, OUTPUT);
  pinMode(RELAY2_PIN, OUTPUT);
  pinMode(RELAY3_PIN, OUTPUT);

  // Turn off all relays initially
  digitalWrite(RELAY1_PIN, LOW);
  digitalWrite(RELAY2_PIN, LOW);
  digitalWrite(RELAY3_PIN, LOW);

  // Initialize buttons with internal pull-up resistors
  pinMode(BUTTON1_PIN, INPUT_PULLUP);
  pinMode(BUTTON2_PIN, INPUT_PULLUP);
  pinMode(BUTTON3_PIN, INPUT_PULLUP);
  pinMode(BUTTON4_PIN, INPUT_PULLUP);
  pinMode(BUTTON5_PIN, INPUT_PULLUP);
  
  // Load the last stepper position from EEPROM
  EEPROM.get(EEPROM_ADDR, stepperPosition);
  if (stepperPosition == -1) {
    // If the position in EEPROM is invalid, start from 0
    stepperPosition = 0;
  }

  // Set stepper motor to the last saved position
  stepper.setCurrentPosition(stepperPosition * stepsPerMM);

  // Display the initial state on the LCD
  updateLCD();
}

void loop() {
  checkButtons();
  stepper.run();  // Continuously run the stepper motor
}

// Function to check button presses
void checkButtons() {
  unsigned long currentTime = millis();
  
  // Debouncing to prevent rapid multiple presses
  if ((currentTime - lastDebounceTime) > debounceDelay) {
    if (digitalRead(BUTTON1_PIN) == LOW) {
      // First button: Turn relay 1 on for 2 seconds
      digitalWrite(RELAY1_PIN, HIGH);
      updateLCD();  // Update the LCD to show the relay state
      delay(2000);
      digitalWrite(RELAY1_PIN, LOW);
      updateLCD();  // Update the LCD after relay turns off
      lastDebounceTime = currentTime;
    }

    if (digitalRead(BUTTON2_PIN) == LOW) {
      // Second button: Turn relay 2 on, then pause and turn on relay 3
      digitalWrite(RELAY2_PIN, HIGH);
      updateLCD();  // Update the LCD to show the relay state
      delay(2000);
      digitalWrite(RELAY2_PIN, LOW);

      delay(3000);  // Pause for 3 seconds

      digitalWrite(RELAY3_PIN, HIGH);
      updateLCD();  // Update the LCD to show the relay state
      delay(2000);
      digitalWrite(RELAY3_PIN, LOW);
      updateLCD();  // Update the LCD after relay turns off
      lastDebounceTime = currentTime;
    }

    if (digitalRead(BUTTON3_PIN) == LOW) {
      // Third button: Turn relay 3 on for 2 seconds
      digitalWrite(RELAY3_PIN, HIGH);
      updateLCD();  // Update the LCD to show the relay state
      delay(2000);
      digitalWrite(RELAY3_PIN, LOW);
      updateLCD();  // Update the LCD after relay turns off
      lastDebounceTime = currentTime;
    }

    // Stepper motor control with buttons 4 and 5
    handleStepperButtons(currentTime);
  }
}

// Handling button press for stepper motor movement (1 mm per press)
void handleStepperButtons(unsigned long currentTime) {
  if (digitalRead(BUTTON4_PIN) == LOW) {
    // Move stepper -1 mm (counterclockwise)
    stepper.move(-stepsPerMM);  // Move 1 mm in the negative direction
    updateStepperPosition(-1);
    lastDebounceTime = currentTime;
  }

  if (digitalRead(BUTTON5_PIN) == LOW) {
    // Move stepper +1 mm (clockwise)
    stepper.move(stepsPerMM);  // Move 1 mm in the positive direction
    updateStepperPosition(1);
    lastDebounceTime = currentTime;
  }
}

// Function to update the stepper position and display it
void updateStepperPosition(int steps) {
  stepperPosition += steps;  // Update position by the number of steps

  // Save the updated stepper position to EEPROM
  EEPROM.put(EEPROM_ADDR, stepperPosition);  // Store the current position

  // Update the LCD with the new position
  updateLCD();
}

// Function to update the LCD display (relay states and stepper position)
void updateLCD() {
  // First line: Relay status
  lcd.setCursor(0, 0);
  if (digitalRead(RELAY1_PIN) == HIGH) {
    lcd.print("Veikia Rele 1   ");  // Clear extra characters
  } else if (digitalRead(RELAY2_PIN) == HIGH) {
    lcd.print("Veikia Rele 2   ");
  } else if (digitalRead(RELAY3_PIN) == HIGH) {
    lcd.print("Veikia Rele 3   ");
  } else {
    lcd.print("Veikia Rele: Nera");
  }

  // Second line: Stepper position
  lcd.setCursor(0, 1);
  lcd.print("Pozicija: ");

  // Clear old position value before updating
  lcd.print("       ");  // 7 spaces to clear the old number
  lcd.setCursor(10, 1);  // Position cursor after "Pozicija:"

  // Display the stepper position in millimeters
  lcd.print(stepperPosition);
  lcd.print("mm");
}