#include <Arduino.h>
#include "StepperMotor.h"

int STEP_PIN = 4;
int DIR_PIN = 16;
int ENABLE_PIN = 17;

// acceleration profile
const float accnProfile[] = { 
   1.570796, 1.570796, -1.570796, -1.570796, -1.570796, -1.570796, 1.570796, 1.570796
};

unsigned long previousMillis_accn = 0;
const long accn_update_interval = 500;
int idx_ = 0;
int num_steps = sizeof(accnProfile) / sizeof(accnProfile[0]);

StepperMotor motor(STEP_PIN, DIR_PIN, ENABLE_PIN);

// Requested data to send
String properties[] = {"angle", "omega", "alpha"};

void setup() {
    Serial.begin(115200);

    motor.initializeMotor();
    // set initial velocity
    motor.omega = 0.0;
    // set initial acceleration
    motor.alpha = 0.0;
    // set pulse
    motor.setPulse();
}

void loop() {
    // Track the total elapsed time since the program started
    unsigned long elapsedTime = millis();

    // Change acceleration every preferred time interval
    if (elapsedTime - previousMillis_accn > accn_update_interval) {
        previousMillis_accn += accn_update_interval; // Update timestamp for the next interval
        motor.alpha = accnProfile[idx_];

        idx_++;
        if (idx_ > (num_steps - 1)) {
            idx_ = 0;
        }

        Serial.print("Current angle: ");
        Serial.println(motor.angle); // Print the current angle at each change
    }

    motor.updateVelocity(10);
    motor.stepThrough();
}