#include "StepperController.h"
#include "StepperXController.h"
#include "Time.h"

#define LeftPin 3
#define RightPin 2

Time time;

void setup(){
    // Start serial communication at 115200 baud
    Serial.begin(115200);
    
    // Set the pin mode for the defined pins
    pinMode(LeftPin, INPUT_PULLUP);
    pinMode(RightPin, INPUT_PULLUP);

    initSteppers();
    initXStepper();
}

float position = 0;

void loop(){
    // Updates deltatime
    time.update();

    stepperXLoop();

    bool left = digitalRead(LeftPin) == LOW;
    bool right = digitalRead(RightPin) == LOW;

    processJoystickInput(left, right);

    // //Serial.print(millis());

    // Serial.print("Left state: ");
    // Serial.print(left);
    // Serial.print("  |  ");
    // Serial.print("Right state: ");
    // Serial.println(right);

    if (Serial.available() > 0){
        beginGrabSequence();
        Serial.flush();
    }


    delay(10);  // delay in milliseconds
}

void beginGrabSequence(){
    moveStepper(StepperAxis::Head, 100000, 75);
    moveStepper(StepperAxis::Y, 10000, 0);
    moveStepper(StepperAxis::Head, 1000, 0, 100);
    moveStepper(StepperAxis::Y, 10000, 1000);
}