#include <Stepper.h>

#define STEPS_PER_REV 200  // NEMA17 stepper motor
#define MAX_RPS 24
#define MAX_STEPS_PER_SEC (MAX_RPS * STEPS_PER_REV)  // 4800 steps/sec
#define ACCEL_TIME 2000  // 2 seconds acceleration/deceleration

// Motor control pins
#define STEP_PIN_1 27  // L293D IN1
#define STEP_PIN_2 26  // L293D IN2
#define STEP_PIN_3 23  // L293D IN3
#define STEP_PIN_4 22  // L293D IN4
#define ENABLE_PIN_1 25  // L293D Enable A
#define ENABLE_PIN_2 18 // L293D Enable B
#define BUTTON_PIN 17   // Pushbutton input

Stepper stepper(STEPS_PER_REV, STEP_PIN_1, STEP_PIN_3, STEP_PIN_2, STEP_PIN_4);

volatile bool motorRunning = false;
volatile bool buttonPressed = false;
unsigned long lastButtonPress = 0;  // For debounce
const int debounceDelay = 250;  // 50ms debounce time

int stepDelay = 1000; // Initial delay (microseconds)
int targetStepDelay = 1000; // Target delay for acceleration
int accelStepCount = 0;
int totalSteps = 0; // Total steps moved
int targetDegree = 0; // Target degree range for stopping
int stopRange = 10; // Degrees range for stopping (e.g., ±10 degrees)

// Interrupt Service Routine for button press with debounce
void IRAM_ATTR buttonISR() {
    unsigned long currentMillis = millis();
    if (currentMillis - lastButtonPress > debounceDelay) {
        buttonPressed = true;
        lastButtonPress = currentMillis;
    }
}

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

    pinMode(BUTTON_PIN, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), buttonISR, FALLING);

    pinMode(ENABLE_PIN_1, OUTPUT);
    pinMode(ENABLE_PIN_2, OUTPUT);
    
    digitalWrite(ENABLE_PIN_1, LOW); // Keep motor off initially
    digitalWrite(ENABLE_PIN_2, LOW);
    
    stepper.setSpeed(1);  // Start at minimum speed (not moving)
}

void loop() {
    if (buttonPressed) {
        buttonPressed = false;  // Reset flag

        if (!motorRunning) {
            Serial.println("Starting Motor...");
            motorRunning = true;
            accelStepCount = 0;
            //totalSteps = 0;  // Reset total steps at the start
            //targetDegree = 90;  // Set the target degree range to 90° for example
            targetStepDelay = 200;  // Target delay for 4800 steps/sec
            digitalWrite(ENABLE_PIN_1, HIGH);
            digitalWrite(ENABLE_PIN_2, HIGH);
        } else {
            Serial.println("Stopping Motor...");
            motorRunning = false;
            accelStepCount = 0;
            targetStepDelay = 1000;  // Slow down gradually
            delay(2000);  // Allow deceleration
            digitalWrite(ENABLE_PIN_1, LOW);
            digitalWrite(ENABLE_PIN_2, LOW);
        }
    }

    if (motorRunning) {
        // Smooth acceleration
        // if (accelStepCount < (MAX_STEPS_PER_SEC * (ACCEL_TIME / 1000.0))) {
        //     int progress = accelStepCount * 100 / (MAX_STEPS_PER_SEC * (ACCEL_TIME / 1000.0));
        //     stepDelay = 1000 - (progress * 800 / 100);  // Linear acceleration
        //     accelStepCount++;
        // } else {
        //     stepDelay = targetStepDelay;  // Maintain full speed
        // }

        stepper.setSpeed(2);  // Adjust speed dynamically
        stepper.step(200);  // Move one step at a time
        totalSteps++; // Track the total number of steps taken

        // Check if the motor has reached the target angle within the range
        int currentDegree = (totalSteps % STEPS_PER_REV) * (360 / STEPS_PER_REV);
        printf("%d\n", currentDegree);
        motorRunning = false;
        // if (currentDegree >= targetDegree - stopRange && currentDegree <= targetDegree + stopRange) {
        //     Serial.println("Motor stopped within target range.");
        //     motorRunning = false;  // Stop the motor once it's within the desired range
        //     digitalWrite(ENABLE_PIN_1, LOW);
        //     digitalWrite(ENABLE_PIN_2, LOW);
        // }
    }
}