// Simulation: https://wokwi.com/projects/364440263181446145
// for https://forum.arduino.cc/t/continuous-rotation-for-stepper-motors/992363

#include <ContinuousStepper.h> // https://github.com/bblanchon/ArduinoContinuousStepper
#include <elapsedMillis.h>

// Run time
elapsedMillis timeElapsed;

const float minutes = 10;                    // in min
const unsigned int runTime = minutes * 1000; // in ms  // FOR DEBUGGING
// unsigned int runTime = minutes * 60 * 1000;      // in ms

// Stepper Motors
ContinuousStepper<StepperDriver> stepper_1;
ContinuousStepper<StepperDriver> stepper_2;

// Driver Connection
const uint8_t stepPin_1 = 9;
const uint8_t dirPin_1 = 8;
const uint8_t stepPin_2 = 7;
const uint8_t dirPin_2 = 6;

// Switches
const uint8_t onoffSwitch = 13;
const uint8_t doorSwitch = 12;

// Motor Configuration
const float stepsPerRevolution = 3200;            // 1 = pulses per Revolution, see driver configuration SW1-4
const float stepAngle = 360 / stepsPerRevolution; // ° : Just for checking

// Motor Configuration - Alternatively
// const float stepAngle = 0.45;                       // °
// const float stepsPerRevolution = 360 / stepAngle;   // 1 = pulses per Revolution

// ! CAREFUL HERE, DO NOT CHANGE WHEN MOUNTED ON CENTRIFUGE !
const float maxAcceleration = stepsPerRevolution / 40; // steps / s^2
const bool accelerationOn = true;                      // Turn off for debuggin if required
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

// Rotation settings
const float RPM_1 = 5;                             // 1 / min
const float RPM_2 = 10;                             // 1 / min
const float SPS_1 = RPM_1 * stepsPerRevolution / 60; // step / sec
const float SPS_2 = RPM_2 * stepsPerRevolution / 60; // step / sec

// RPM =  (60 seconds per minute) x (steps per second) / (steps per revolution)]
// SPS =  (revolutions per minute) x (steps per revolution) / (60 seconds per minute)

// Flag to make sure motor runs only once during runtime, only if door switch is on and on/off switch is on
// After runtime expired on/off Switch has to be reseted to run again
bool runState = false;

// Debugging
// doorSwitch = HIGH;
// onoffSwitch = HIGH;

void setup()
{
    stepper_1.begin(stepPin_1, dirPin_1);
    stepper_2.begin(stepPin_2, dirPin_2);

    if (accelerationOn == true)
    {
        stepper_1.setAcceleration(maxAcceleration);
        stepper_2.setAcceleration(maxAcceleration);
    }

    Serial.begin(115200);

    pinMode(onoffSwitch, INPUT_PULLUP);
    pinMode(doorSwitch, INPUT_PULLUP);

    // Wait for on/off switch to be reseted and set a flag
    // if on/off switch has been on
    if (digitalRead(onoffSwitch) == LOW)
    {
        runState = false;
    }
}

void loop()
{
    // Check if runState flag is set, make sure code is only executed once
    // Furthermore check if door switch is on and on/off switch is on
    if (runState && digitalRead(doorSwitch) == LOW && digitalRead(onoffSwitch) == LOW)
    {
        // Reset elapsed time and set flag to make sure code runs only once
        timeElapsed = 0;
        runState = false;

        // Run motor while runtime is not expired, door switch is on and on/off switch is on
        while (timeElapsed < runTime && digitalRead(doorSwitch) == LOW && digitalRead(onoffSwitch) == LOW)
        {
            stepper_1.spin(SPS_1); // Sets the target speed.
            stepper_1.loop();    // Updates the status of the step and dir pins.
            stepper_2.spin(SPS_2); // Sets the target speed.
            stepper_2.loop();    // Updates the status of the step and dir pins.
            reportSpeed(timeElapsed);
        }
        reportTime(timeElapsed);
    }
    else
    {
        // Stopp motor gently after runTime has expired, on/off switch has been turned
        // off or door switch has been turned off
        stepper_1.loop(); // Updates the status of the step and dir pins.
        stepper_1.stop(); // Sets the target speed to 0.
        stepper_2.loop(); // Updates the status of the step and dir pins.
        stepper_2.stop(); // Sets the target speed to 0.
    }

    // Reset runState if on/off switch has been turned first off and on again
    if (digitalRead(onoffSwitch) == HIGH)
    {
        runState = true;
    }

    // Reset on/off switch if door switch has been activated
    if (digitalRead(doorSwitch) == HIGH)
    {
        digitalWrite(onoffSwitch, HIGH);
        runState = false;
    }
}

void reportSpeed(float timeElapsed)
{
    // Report speed every second
    const int interval = 1000;
    static unsigned long last = -interval;
    unsigned long now = millis();

    if (now - last < interval)
        return;
    last = now;
    Serial.print("runtime:");
    Serial.print(timeElapsed / 1000); // Convert elapsed time to seconds

    Serial.print(" sec; Speed M1:");
    Serial.print(60 * stepper_1.speed() / stepsPerRevolution);
    Serial.print(" rpm; ");
    // Serial.print(" rpm (= ");
    // Serial.print(stepper_1.speed());
    // Serial.print(" sps; )");

    Serial.print(" sec; Speed M2:");
    Serial.print(60 * stepper_2.speed() / stepsPerRevolution);
    Serial.println(" rpm");
    // Serial.print(" rpm (= ");
    // Serial.print(stepper_2.speed());
    // Serial.println(" sps)");
}

void reportTime(float timeElapsed)
{
    // Report time elapsed after runtime expired
    Serial.print("runtime:");
    // Serial.print(timeElapsed / 1000 / 60);
    // Serial.println(" min; - End");
    Serial.print(timeElapsed / 1000); // DEBUGGING
    Serial.println(" sec; - End");    // DEBUGGING
}