#include <AccelStepper.h>

AccelStepper objectiveStepper(AccelStepper::DRIVER, 3, 2);
#define objectiveEN 4
AccelStepper coarseStepper(AccelStepper::DRIVER, 5, 6);
#define coarseEN 7
AccelStepper fineStepper(AccelStepper::DRIVER, 9, 8);
#define fineEN 10

String dataReceived;

//for Objective Lens step values
long objStepConstant = 2055;
long objStepsToRun;

void setup() {
  // put your setup code here, to run once:
  objectiveStepper.setMaxSpeed(300.0);
  objectiveStepper.setAcceleration(500.0);
  pinMode(objectiveEN, OUTPUT);
  coarseStepper.setMaxSpeed(300.0);
  coarseStepper.setAcceleration(500.0);
  pinMode(coarseEN, OUTPUT);
  
  digitalWrite(objectiveEN, HIGH);
  digitalWrite(coarseEN, HIGH);

  Serial.begin(9600);
  Serial.println("Serial has started...");
}

void loop() {
  if (Serial.available()) {
    dataReceived = Serial.readStringUntil('\n');
    dataReceived.toLowerCase();
    Serial.println(dataReceived);
  }

  //checks the data received
  if (dataReceived == "cup") moveCoarse("up");
  else if (dataReceived == "cdn") moveCoarse("down");
  else if (dataReceived == "cstp") moveCoarse("stop");
  
  else if (dataReceived == "fup") moveFine("up");
  else if (dataReceived == "fdn") moveFine("down");
  else if (dataReceived == "fstp") moveFine("stop");

  else if (dataReceived == "objc") {
    objStepsToRun += objStepConstant;
    moveObjectiveLens(objStepsToRun);
  }
}

void moveCoarse(String upORdown) {
  // Convert input to lowercase for consistent comparison
  upORdown.toLowerCase();

  if (upORdown == "up") {
    Serial.println("Moving up");
    digitalWrite(coarseEN, LOW); // Enable stepper
    coarseStepper.setSpeed(-300); // Set a high negative speed (adjust as needed)
    coarseStepper.runSpeed(); // Continuous movement
  } 
  else if (upORdown == "down") {
    Serial.println("Moving down");
    digitalWrite(coarseEN, LOW); // Enable stepper
    coarseStepper.setSpeed(300); // Set a high positive speed (adjust as needed)
    coarseStepper.runSpeed(); // Continuous movement
  } 
  else {
    Serial.println("Motor Stopped!");
    digitalWrite(coarseEN, HIGH); // Disable stepper
    coarseStepper.stop(); // Stop movement
    dataReceived = "";
  }
}


void moveFine(String upORdown) {
  // Convert input to lowercase for consistent comparison
  upORdown.toLowerCase();

  if (upORdown == "up") {
    Serial.println("Moving up");
    digitalWrite(fineEN, LOW); // Enable stepper
    fineStepper.setSpeed(-300); // Set a high negative speed (adjust as needed)
    fineStepper.runSpeed(); // Continuous movement
  } 
  else if (upORdown == "down") {
    Serial.println("Moving down");
    digitalWrite(fineEN, LOW); // Enable stepper
    fineStepper.setSpeed(300); // Set a high positive speed (adjust as needed)
    fineStepper.runSpeed(); // Continuous movement
  } 
  else {
    Serial.println("Motor Stopped!");
    digitalWrite(fineEN, HIGH); // Disable stepper
    fineStepper.stop(); // Stop movement
    dataReceived = "";
  }
}

void moveObjectiveLens(long step) {
  Serial.print("Steps to Run: ");
  Serial.println(step);

  // Enable the stepper motor driver
  digitalWrite(objectiveEN, LOW);

  // Set the target position
  objectiveStepper.moveTo(step);

  // Continuously update the motor's position until it reaches the target
  while (objectiveStepper.currentPosition() != step) {
    objectiveStepper.run(); // Non-blocking stepper motor update
  }

  // Motor has reached the desired position
  Serial.println("Motor stopped!");
  digitalWrite(objectiveEN, HIGH);

  // Reset dataReceived to avoid re-triggering
  dataReceived = "";
}