//1) Extend arm out (enough to get first ball) only
//2) and Lower arm only to below horizontal
//3) rotate only and pick up 1st ball

//4) bring arm in, increase height, and rotate for 2nd ball
//5) drop height and swing past for 3rd ball
//6) rotate a little and tilt up to roll 3rd ball down
//7) bring arm in and rotate past for 4th ball
//8) come up a little, bring arm in, and swing around for 5th ball
//9) extend out, drop the height, and rotate round for 6th ball
//10) increase height for 6-th ball to roll
//miocrostep = 16 //flop is about 160 steps ish 10cm 190g cw
//Arrays controlling target distances (in steps) for each movement

#include <AccelStepper.h>
#include <Servo.h> // Include the Servo library

// SWING AND EXTEND
//int extendDistances[10] =   {0,17560,  0, -3070,  2690,   0, -2570,  -1270,  2995, 0}; 
//int verticalDistances[10] = {0, 0,  -960,  1631, -1650, 1850, -1950, 810, -710, 2850}; 
//int rotateDistances[10] =   {0, 3858, 1732,1632, 3700, 2564, 1432, 1480, 3302-500,0}; //To check, not to move!

// Define arrays controlling target distances OLD
int extendDistances[11] =   {17560,0,  0, -3070+300,  2690-300,   0, -2570,  -1270, 0, 2995, 0}; 
int verticalDistances[11] = {0, -960+500,  0,  1631, -1650, 1850+300, -1950-300, 810,500, -710-500, 2000}; 
int rotateDistances[11] =   {0, 0,   3858, 1732,1632, 3700, 2564, 920, 1000,992, 3302-500}; //To check, not to move!


int iteration = 0;
int rotateCounter = 0;
bool motionStarted = false; // Variable to track if motion has started
const int buttonPin = 2; // Button pin

// Create class instances
AccelStepper Rotation_Stepper(AccelStepper::DRIVER, 9, 10); 
AccelStepper Extend_Stepper(AccelStepper::DRIVER, 4, 7); 
AccelStepper Vertical_Stepper(AccelStepper::DRIVER, 5, 8); 

Servo myServo; // Create a Servo object

// Variables for button state
bool lastButtonState = HIGH; // Previous state of the button

void setup() {
    Serial.begin(9600); // Initialize serial communication
    pinMode(buttonPin, INPUT_PULLUP);

    Rotation_Stepper.moveTo(Rotation_Stepper.currentPosition() + 16*1200); 

    Rotation_Stepper.setMaxSpeed(450); //was 280
    Extend_Stepper.setMaxSpeed(3000); 
    Vertical_Stepper.setMaxSpeed(3500);

    Rotation_Stepper.setAcceleration(150); 
    Extend_Stepper.setAcceleration(4000); 
    Vertical_Stepper.setAcceleration(3000); 

    Extend_Stepper.moveTo(0); 
    Vertical_Stepper.moveTo(0);
    Rotation_Stepper.moveTo(0);

    myServo.attach(12); // Attach the servo on pin 12
    myServo.write(0); // Set initial position of the servo to 0 degrees REMOVE AND DO PHYSICALLY IRL
}

void loop() {
    bool currentButtonState = digitalRead(buttonPin);

    // Check if the button has been pressed or released
    if (lastButtonState == HIGH && currentButtonState == LOW) {
        delay(50); // Debounce delay
        motionStarted = true; // Start motion on button press
    }
    
    lastButtonState = currentButtonState;

    if (!motionStarted) {
        return; // Do nothing until button is pressed
    }

    // Stop robot 
    if (iteration >= 11) {
        Rotation_Stepper.setSpeed(0); 
        Extend_Stepper.setSpeed(0);
        Vertical_Stepper.setSpeed(0); 
        return;
    }

    if (Rotation_Stepper.distanceToGo() == 0 && Extend_Stepper.distanceToGo() == 0 && Vertical_Stepper.distanceToGo() == 0) {
        Extend_Stepper.moveTo(Extend_Stepper.currentPosition() + extendDistances[iteration]);
        Vertical_Stepper.moveTo(Vertical_Stepper.currentPosition() + verticalDistances[iteration]);
        Rotation_Stepper.moveTo(Rotation_Stepper.currentPosition() + rotateDistances[iteration]);

        // Print the current position of the rotation stepper
        Serial.print("Iteration: ");
        Serial.println(iteration);
        iteration++;
        
        // Control the servo based on iteration count
        if (iteration == 2) { // After iteration 1 is complete
            myServo.write(90); // Rotate servo to 90 degrees
        } 
    }

    Extend_Stepper.run();  
    Vertical_Stepper.run(); 

    // Run the rotation stepper only if the iteration is 1 or greater
    if (iteration >= 3) {
        Rotation_Stepper.run();
    }

    if (Rotation_Stepper.currentPosition() == 16*1200 && Vertical_Stepper.distanceToGo() == 0 && iteration == 10) { // At end
            myServo.write(0); // Rotate servo back to 0 degrees
            iteration++;
            
        }
}