#include <Stepper.h>
#include <SPI.h>
#include <SD.h>
#include <Wire.h>
#include "RTClib.h"

// Define number of steps per revolution:
const int stepsPerRevolution = 200;

// Give the motor control pins names:
#define pwmA 3
#define pwmB 11
#define brakeA 9
#define brakeB 8
#define dirA 12
#define dirB 13

// Initialize the stepper library on the motor shield:
Stepper myStepper = Stepper(stepsPerRevolution, dirA, dirB);

const int maxQueueSize = 100; // Maximum number of step requests in the queue
int stepQueue[maxQueueSize]; // Circular buffer to store step requests
int queueHead = 0; // Index of the head of the queue
int queueTail = 0; // Index of the tail of the queue
bool stopMotor = false; // Flag to indicate if the motor should be stopped 

RTC_DS3231 rtc;

#define WAIT_TO_START 0 // Wait for serial input in setup() 0=off, 1=on. Set to off when running without Serial M.
#define LOG_INTERVAL 1000// millis between logs
#define ECHO_TO_SERIAL 1

#define NUMSAMPLES 3 // Number of samples to be taken when calculating average. 

#define Cell_V_Sense_Pin A0 //Ch1 Voltage VIn from Cell is analogue pin A0
#define Top_Temp_Pin A1
#define Bottom_Temp_Pin A2

#define chipSelect 10  //Pin that will be used to select the SD card logger - never changes

byte i = 0; //Used to contain the sample count (byte used to save space - can only increment to 255)
long previousMillis = 0;  //store millis reading for delay between readings

char daysOfTheWeek[7][12] = {"Sunday", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday"};
char filename[] = "DROPTESTLOG00.CSV"; //default name for the log file to be stored onto the SD card. Will increment by 1 if file already exists.
char timestamp[30];
File logFile;

int Cell_Voltage_Sample[NUMSAMPLES];//Create [numsamples]x int's for storing the voltage readings
int Top_Temp_Sample[NUMSAMPLES];//Create [numsamples]x int's for storing the voltage readings
int Bottom_Temp_Sample[NUMSAMPLES];//Create [numsamples]x int's for storing the voltage readings

// IR light gate interrupt handler
void IRGateInterrupt() {
    stopMotor = true; // Set the flag to stop the motor 
}

// Serial input interrupt handler
void serialEvent() {
    while (Serial.available() > 0 && (queueTail + 1) % maxQueueSize != queueHead) {
        String inputString = Serial.readStringUntil(' '); // Read the input string from the serial monitor until a space is encountered

        if (inputString.length() > 0) {
            int steps = inputString.toInt(); // Convert the input string to an integer value

            if (steps != 0) {
                if (inputString.endsWith("mm")) {
                    steps *= abs(steps) / steps * ((steps > 0) ? 2.857 : -3.334); // Multiply positive steps by conversion factor if "mm" is entered
                }

                int repeats = Serial.parseInt(); // Read the number of repeats
                if (repeats != 0) {
                    for (int i = 0; i < repeats; i++) {
                        stepQueue[queueTail] = abs(steps); // Add the step request to the queue
                        queueTail = (queueTail + 1) % maxQueueSize;
                        stepQueue[queueTail] = -abs(steps)*3.334/2.857; // Add the step request in the opposite direction to the queue
                        queueTail = (queueTail + 1) % maxQueueSize;
                    }
                    Serial.print("Step request added to the queue. Queue size: ");
                    Serial.println((queueTail - queueHead + maxQueueSize) % maxQueueSize);
                }
                else {
                    Serial.println("Invalid input for repeats. Please enter a non-zero value.");
                }
            }
            else {
                Serial.println("Invalid input for steps. Please enter a non-zero value.");
            }
        }
    }
}

void setup() {
    Serial.begin(9600);
    pinMode(Cell_V_Sense_Pin, INPUT); 
    pinMode(Top_Temp_Pin, INPUT);
    pinMode(Bottom_Temp_Pin, INPUT);

    if (!rtc.begin()) {
        Serial.println("Couldn't find RTC");
        while (1);
    }

    if (rtc.lostPower()) {
        Serial.println("RTC lost power, lets set the time!");
        // following line sets the RTC to the date & time this sketch was compiled
        rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
    }

    Serial.print("Initializing SD card...");
    pinMode(chipSelect, OUTPUT);

    // see if the card is present and can be initialized:
    if (!SD.begin(chipSelect)) {
        Serial.println("Card failed, or not present");
        // don't do anything more:
        return;
    }

    for (unsigned int i = 0; i < 100; i++) {
        filename[3] = i / 10 + '0';
        filename[4] = i % 10 + '0';
        if (! SD.exists(filename)) {
            // only open a new file if it doesn't exist
            logFile = SD.open(filename, FILE_WRITE);
            break;  // leave the loop!
        }
    }

    DateTime now = rtc.now();

    logFile.print("Filename =,");
    logFile.println(filename);
    logFile.print ("Sketch Name =,");
    logFile.println (__FILE__);
    logFile.print ("Compiled Date =, ");
    logFile.print (__DATE__);
    logFile.print (" ");
    logFile.println (__TIME__);
    logFile.println();;

    logFile.print ("Log Start =,");
    logFile.print(now.year(), DEC);
    logFile.print('/');
    logFile.print(now.month(), DEC);
    logFile.print('/');
    logFile.print(now.day(), DEC);
    logFile.print(" (");
    logFile.print(daysOfTheWeek[now.dayOfTheWeek()]);
    logFile.print(") ");
    logFile.print(now.hour(), DEC);
    logFile.print(':');
    logFile.print(now.minute(), DEC);
    logFile.print(':');
    logFile.println(now.second(), DEC);
    logFile.println();

    // Set the PWM and brake pins so that the direction pins can be used to control the motor:
    pinMode(pwmA, OUTPUT);
    pinMode(pwmB, OUTPUT);
    pinMode(brakeA, OUTPUT);
    pinMode(brakeB, OUTPUT);
    pinMode(dirA, OUTPUT);
    pinMode(dirB, OUTPUT);

    digitalWrite(pwmA, HIGH);
    digitalWrite(pwmB, HIGH);
    digitalWrite(brakeA, LOW);
    digitalWrite(brakeB, LOW);

    // Set the motor speed (RPMs):
    myStepper.setSpeed(140);

  // Attach interrupts to handle events
  attachInterrupt(digitalPinToInterrupt(2), IRGateInterrupt, FALLING); // Attach interrupt to IR light gate on digital channel 2
}

void loop() {
  while (queueHead != queueTail) {
      int steps = stepQueue[queueHead]; // Get the step request from the queue

      if (abs(steps) <= 10000) {
          digitalWrite(brakeA, LOW); // Release brake on motor A
          digitalWrite(brakeB, LOW); // Release brake on motor B

          // Step in one direction for the specified number of steps:
          for (int j = 0; j < abs(steps); j++) {
              if (stopMotor) {
                  stopMotor = false; // Reset the flag
                  break; // Exit the loop if the motor should be stopped instantly
              }
              myStepper.step(steps / abs(steps));
          }

          digitalWrite(brakeA, HIGH); // Apply brake on motor A
          digitalWrite(brakeB, HIGH); // Apply brake on motor B

          Serial.print("Stepped ");
          Serial.print((steps > 0) ? "forward " : "backward ");
          Serial.print(abs(steps));
          Serial.println(" steps");
          delay(1000);
      }
      else {
          Serial.println("Error: Maximum number of steps exceeded (10,000 steps).");
      }

      queueHead = (queueHead + 1) % maxQueueSize; // Move to next element in circular buffer
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= LOG_INTERVAL) {
      previousMillis = currentMillis;

      unsigned long time = millis();// log milliseconds since starting
      DateTime now = rtc.now();

      for (i = 0; i < NUMSAMPLES; i++) {
          Cell_Voltage_Sample[i] = analogRead(Cell_V_Sense_Pin);
          delayMicroseconds(125); 
          Top_Temp_Sample[i] = analogRead(Top_Temp_Pin);
          delayMicroseconds(125);
          Bottom_Temp_Sample[i] = analogRead(Bottom_Temp_Pin);
          delayMicroseconds(125);
      }

      String dataString = "";
      dataString += String(time);
      dataString += ",";

      for (i = 0; i < NUMSAMPLES; i++) {
          dataString += String(Cell_Voltage_Sample[i]);
        dataString += ",";
      }

      for (i = 0; i < NUMSAMPLES; i++) {
        dataString += String(Top_Temp_Sample[i]);
        dataString += ",";
      }

      for (i = 0; i < NUMSAMPLES; i++) {
        dataString += String(Bottom_Temp_Sample[i]);
        dataString += ",";
      }

      dataString += ",";
    

   

    //if the file is open, write data.
    if (logFile) {
      logFile.println(dataString);
      logFile.close();
      
    #if ECHO_TO_SERIAL
    Serial.println(dataString);
    #endif

    }

  }
}



can you make this as efficient as possible