#include <Wire.h>
#include <AccelStepper.h>
#include <RTClib.h>

//Serial system variables
uint16_t SerialStart;
bool SerialConnected = false;

//Generic cycle counter variable
int8_t i = 0;

// Define constants
const uint8_t NUM_MOTORS = 6;  // Number of stepper motors (HH:MM:SS)
const uint8_t STEPS_PER_REVOLUTION = 200;  // Adjust based on your stepper motor
const uint8_t POSITIONS_PER_DIGIT = 10;  // 0-9 for each digit
const uint16_t MAX_SPEED = 1000;  // Maximum speed in steps per second
const uint16_t ACCELERATION = 500;  // Acceleration in steps per second per second

// Pin definitions for stepper motors
const uint8_t STEP_PINS[NUM_MOTORS] = {2, 3, 4, 5, 6, 7};
const uint8_t DIR_PINS[NUM_MOTORS] = {8, 9, 10, 11, 12, 13};

// Create stepper motor objects
AccelStepper steppers[NUM_MOTORS] = {
    AccelStepper(AccelStepper::DRIVER, STEP_PINS[0], DIR_PINS[0]),
    AccelStepper(AccelStepper::DRIVER, STEP_PINS[1], DIR_PINS[1]),
    AccelStepper(AccelStepper::DRIVER, STEP_PINS[2], DIR_PINS[2]),
    AccelStepper(AccelStepper::DRIVER, STEP_PINS[3], DIR_PINS[3]),
    AccelStepper(AccelStepper::DRIVER, STEP_PINS[4], DIR_PINS[4]),
    AccelStepper(AccelStepper::DRIVER, STEP_PINS[5], DIR_PINS[5])
};

// Create RTC object
RTC_DS3231 rtc;

// Global variables
uint8_t currentPositions[NUM_MOTORS] = {0, 0, 0, 0, 0, 0};
uint8_t desiredPositions[NUM_MOTORS] = {0, 0, 0, 0, 0, 0};
uint16_t lastUpdateTime = 0;
const uint16_t UPDATE_INTERVAL = 100;  // Update motor variables every X ms

// Function prototypes
void updateClockDisplay();
int calculateSteps(int currentPos, int desiredPos);
void moveMotors();

void setup() {

  // Serial initialisation or bypass system
  Serial.begin(9600);
  delay(1000);
  Serial.println("Press any key to begin serial monitor");
  SerialStart = millis();
while (millis() - SerialStart < 5000) {
  if (Serial.available()) {
    SerialConnected = true;
    Serial.println("Serial monitor started");
    pinMode(LED_BUILTIN, OUTPUT);
    digitalWrite(LED_BUILTIN, HIGH);
    break;
  }
}
if (!SerialConnected) {
  Serial.println("Serial monitor disconnected, normal operation will begin now");
  Serial.end();
}

Wire.begin();
    

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

    // Uncomment the following line to set the RTC to the date and time this sketch was compiled
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));

    // Initialize stepper motors
    for (i = 0; i < NUM_MOTORS; i++) {
        steppers[i].setMaxSpeed(MAX_SPEED);
        steppers[i].setAcceleration(ACCELERATION);
    }

    // Initial position setup
    updateClockDisplay();
}

void loop() {
    unsigned long currentTime = millis();

    // Update clock display every second
    if (currentTime - lastUpdateTime >= UPDATE_INTERVAL) {
        updateClockDisplay();
        lastUpdateTime = currentTime;
    }

    // Run stepper motors
    moveMotors();
}

void updateClockDisplay() {
    DateTime now = rtc.now();
    
    // Calculate desired positions based on current time
    desiredPositions[0] = now.second() % 10;
    desiredPositions[1] = now.second() / 10;
    desiredPositions[2] = now.minute() % 10;
    desiredPositions[3] = now.minute() / 10;
    desiredPositions[4] = now.hour() % 10;
    desiredPositions[5] = now.hour() / 10;

    if(SerialConnected){
      Serial.print(desiredPositions[5]);
      Serial.print(desiredPositions[4]);
      Serial.print(":");
      Serial.print(desiredPositions[3]);
      Serial.print(desiredPositions[2]);
      Serial.print(":");
      Serial.print(desiredPositions[1]);
      Serial.println(desiredPositions[0]);
    }

    // Set motor movements
    for (i = 0; i < NUM_MOTORS; i++) {
        int steps = calculateSteps(currentPositions[i], desiredPositions[i]);
        if (steps != 0) {
            steppers[i].move(steps);
            currentPositions[i] = desiredPositions[i];
        }
    }
}

int calculateSteps(int currentPos, int desiredPos) {
    int diff = desiredPos - currentPos;
    if (diff < -POSITIONS_PER_DIGIT / 2) {
        diff += POSITIONS_PER_DIGIT;
    } else if (diff > POSITIONS_PER_DIGIT / 2) {
        diff -= POSITIONS_PER_DIGIT;
    }
    return diff * (STEPS_PER_REVOLUTION / POSITIONS_PER_DIGIT);
}

void moveMotors() {
    bool allMotorsStopped = false;
    while (!allMotorsStopped) {
        allMotorsStopped = true;
        for (i = 0; i < NUM_MOTORS; i++) {
            if (steppers[i].distanceToGo() != 0) {
                steppers[i].run();
                allMotorsStopped = false;
            }
        }
    }
}