#include <AccelStepper.h>  // Include the AccelStepper library for stepper motor control
#include <Encoder.h>       // Include the Encoder library for rotary encoder
#include <SD.h>            // Include the SD library

// Define stepper motor connections
#define STEP_PIN      5   // Step pin for stepper motor
#define DIR_PIN       6   // Direction pin for stepper motor
#define ENABLE_PIN    7   // Enable pin for stepper motor

// Define rotary encoder pins
#define ENCODER_PIN1  2    // Encoder pin 1
#define ENCODER_PIN2  3    // Encoder pin 2


// Flags for data storage control
bool dataStorageActive = false; // Flag to check if data storage is active

bool homingCompleted = false; // Flag to check if homing is completed
// Define limit switch pin
#define LIMIT_SWITCH_PIN 4 // Pin for limit switch
// Define SD card chip select pin
#define SD_CHIP_SELECT 10

// Initialize stepper motor object
AccelStepper stepper(1, STEP_PIN, DIR_PIN);  // Use driver 1 with the stepper motor pins

// Initialize rotary encoder object
Encoder encoder(ENCODER_PIN1, ENCODER_PIN2);

// Variables for rotary encoder
int previousPosition = 0;
int currentPosition = 0;
bool encoderActive = false;  // Flag to check if the encoder is active

// File to store encoder data
File dataFile;

// Variables to store stepper and encoder data
long storedStepperPosition = 0;
long storedEncoderPosition = 0;

void setup() {
  // Initialize Serial communication
  Serial.begin(9600);

  // Print the header for the columns
  Serial.println("Encoder Position\tStepper Position");

  // Set up stepper motor properties
  stepper.setMaxSpeed(1000);   // Adjust this value as needed
  stepper.setAcceleration(100);  // Adjust this value as needed

  // Set up stepper motor pins
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, LOW);  // Enable the stepper motor driver
  
  // Set initial position of the stepper motor
  stepper.setCurrentPosition(0);

  // Initialize SD card
  if (!SD.begin(SD_CHIP_SELECT)) {
    Serial.println("SD card initialization failed!");
    return;
  }
  Serial.println("SD card initialized.");
  
  // Open file in append mode
  dataFile = SD.open("data.txt", FILE_WRITE);
  if (dataFile) {
    dataFile.println("Encoder Position\tStepper Position");
    dataFile.close();
  } else {
    Serial.println("Error opening file.");
  }
   // Set up button pins
  
  // Set up limit switch pin
  pinMode(LIMIT_SWITCH_PIN, INPUT_PULLUP); // Use internal pull-up resistor

  // Start the homing process
  startHoming();
}

void loop() {
   if (!homingCompleted) {
    return; // Wait until homing is completed
  }

  


  // Read rotary encoder position
  currentPosition = encoder.read();

  // Check if encoder position has changed significantly
  if (abs(currentPosition - previousPosition) >= 4) { // Adjust this threshold as needed
    encoderActive = true;  // Encoder is active
    previousPosition = currentPosition; // Update previous position

    // Map encoder position to stepper motor steps
    long steps = map(currentPosition, 0, 80, 0, 200);  // Adjust 0-80 to your encoder's range and 0-200 to steps range

    // Move stepper motor to desired position
    stepper.moveTo(steps);

    // Print encoder and stepper motor position to Serial monitor in columns
    Serial.print(currentPosition);
    Serial.print("\t\t");
    Serial.println(steps);

    // Write encoder position to SD card in columns
    dataFile = SD.open("data.txt", FILE_WRITE);
    if (dataFile) {
      dataFile.print(currentPosition);
      dataFile.print("\t\t");
      dataFile.println(steps);
      dataFile.flush();  // Ensure data is written to the SD card
      dataFile.close();  // Close the file
    } else {
      Serial.println("Error opening file.");
    }
  } else {
    encoderActive = false;  // Encoder is not active
  }

  // Run the stepper motor
  stepper.run();

  // Ensure a smooth operation
  delay(10);  // Adjust this value as needed for stability

  // Check for Serial input commands
  if (Serial.available() > 0) {
    char command = Serial.read();
    if (command == 'S') {
      // Store current stepper and encoder positions to SD card
      dataFile = SD.open("data.txt", FILE_WRITE);
      if (dataFile) {
        dataFile.print("Stored Encoder Position: ");
        dataFile.print(currentPosition);
        dataFile.print("\t\tStored Stepper Position: ");
        dataFile.println(stepper.currentPosition());
        dataFile.close();  // Close the file
        Serial.println("Data stored successfully.");
      } else {
        Serial.println("Error opening file.");
      }
    } else if (command == 'O') {
      // Open file and read its contents
      File readFile = SD.open("data.txt");
      if (readFile) {
        Serial.println("Reading data from file:");
        while (readFile.available()) {
          Serial.write(readFile.read());
        }
        readFile.close();
      } else {
        Serial.println("Error opening file.");
      }
    } else if (command == 'r') {
      // Read file and execute stored stepper positions
      File readFile = SD.open("data.txt");
      if (readFile) {
        Serial.println("Repeating actions from stored data:");
        while (readFile.available()) {
          String line = readFile.readStringUntil('\n');
          // Skip the header line
          if (line.startsWith("Encoder Position")) continue;

          // Extract encoder and stepper positions from the line
          int tabPos = line.indexOf('\t');
          if (tabPos > 0) {
            long encoderPos = line.substring(0, tabPos).toInt();
            long stepperPos = line.substring(tabPos + 2).toInt(); // +2 to skip the tab character

            // Move stepper motor to the stored position
            stepper.moveTo(stepperPos);
            while (stepper.distanceToGo() != 0) {
              stepper.run();
            }

            // Print the positions to the Serial Monitor
            Serial.print(encoderPos);
            Serial.print("\t\t");
            Serial.println(stepperPos);
          }
        }
        readFile.close();
      } else {
        Serial.println("Error opening file.");
      }
    }
  }
}
void startHoming() {
  Serial.println("Starting homing...");

  // Move the stepper motor towards the home position (clockwise direction)
  stepper.setSpeed(500); // Adjust the speed as needed

  while (digitalRead(LIMIT_SWITCH_PIN) == HIGH) {
    stepper.runSpeed(); // Keep moving until the limit switch is triggered
  }

  stepper.stop(); // Stop the stepper motor
  stepper.setCurrentPosition(0); // Set the current position as home position
  currentPosition = 0; // Reset the encoder position to zero
  encoder.write(0);    // Reset the encoder count to zero

  homingCompleted = true;
  Serial.println("Homing completed.");
}