// Include the AccelStepper library:
#include "MultiStepper.h"
#include "AccelStepper.h"


// Define stepper motor connections and motor interface type. 
// Motor interface type must be set to 1 when using a driver
#define dirPin 2
#define stepPin 3
#define motorInterfaceType 1
  

#define dirPinB 8
#define stepPinB 9
#define motorInterfaceTypeB 1

const int endStopAR = 5;// the number of the endstopp pin
const int endStopAL = 6;
const int endStopBR = 12;
const int endStopBL = 11; // Pin 13 by default high

int distance = 5;
//int moveSpeedA = 600;
//int moveSpeedB = 600;




// Create a new instance of the AccelStepper class:
AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);
AccelStepper stepperB = AccelStepper(motorInterfaceTypeB, stepPinB, dirPinB);


void setup() {
  // Set the maximum speed in steps per second:
  stepper.setMaxSpeed(1000);
  stepperB.setMaxSpeed(1000);
Serial.begin(9600);

  pinMode(endStopAR, INPUT);// initialize the endstop pin as an input
  pinMode(endStopAL, INPUT);
  pinMode(endStopBR, INPUT);
  pinMode(endStopBL, INPUT);
}

void loop() { 

int joystickA = analogRead (A0);
int joystickB = analogRead (A1);
   // Set the current position to 0:
  stepper.setCurrentPosition(0);
    stepperB.setCurrentPosition(0);

int moveSpeedAL = -600 ;
int moveSpeedAR = 600;
int moveSpeedBL = -600;
int moveSpeedBR = 600;

int endStopReadAR =digitalRead(endStopAR);
int endStopReadAL =digitalRead(endStopAL);
int endStopReadBR =digitalRead(endStopBR);
int endStopReadBL =digitalRead(endStopBL);


//if (digitalRead(endStopReadAR) == LOW){ // Programm kommt nicht aus dem if Loop raus
 // moveSpeedAR = 1;
//}

  // A=0 B=0
  if (joystickA >400 && joystickA <600 && joystickB >400 && joystickB <600 )
{
   stepper.setSpeed(0);
     stepper.runSpeed();
      stepperB.setSpeed(0);
     stepperB.runSpeed();
 }
//A=low B=0
   if (joystickA <400 &&  joystickB >400 && joystickB <600 && endStopReadAR == HIGH)
{
   // Run the motor forward at 200 steps/second until the motor reaches 400 steps (2 revolutions):
  while(stepper.currentPosition() != -distance)
  {
    stepper.setSpeed(moveSpeedAL);
    stepper.runSpeed();
    stepperB.setSpeed(0);
    stepperB.runSpeed();
  }
}

//A=high B=0
 if (joystickA >600 &&  joystickB >400 && joystickB <600 && endStopReadAL == HIGH)
{
   // Run the motor forward at 200 steps/second until the motor reaches 400 steps (2 revolutions):
  while(stepper.currentPosition() != distance)
  {
    stepper.setSpeed(moveSpeedAR);
    stepper.runSpeed();
    stepperB.setSpeed(0);
    stepperB.runSpeed();
  }
}

//A=0 B=high
 if (joystickA >400 && joystickA <600 && joystickB >600 && endStopReadBL == HIGH)
{
   // Run the motor forward at 200 steps/second until the motor reaches 400 steps (2 revolutions):
  while(stepperB.currentPosition() != -distance)
  {
    stepper.setSpeed(0);
    stepper.runSpeed();
    stepperB.setSpeed(moveSpeedBL);
    stepperB.runSpeed();
   
  }
}
// Serial.println(moveSpeedBL);
// A=0 B=low
 if (joystickA >400 && joystickA <600 && joystickB <400 && endStopReadBR == HIGH)
{
   // Run the motor forward at 200 steps/second until the motor reaches 400 steps (2 revolutions):
  while(stepperB.currentPosition() != distance)
  {
    stepper.setSpeed(0);
    stepper.runSpeed();
    stepperB.setSpeed(moveSpeedBR);
    stepperB.runSpeed();
  }
}
 
 // A= low B=high
   if (joystickA <400 && joystickB <400 && endStopReadAR == HIGH && endStopReadBL == HIGH)
{
   // Run the motor forward at 200 steps/second until the motor reaches 400 steps (2 revolutions):
  while(stepper.currentPosition() != -distance)
  {
    stepper.setSpeed(moveSpeedAL);
    stepper.runSpeed();
    stepperB.setSpeed(moveSpeedBR);
    stepperB.runSpeed();
    //Serial.println(moveSpeedB);
  } 
 }
// A=high B=high

  if (joystickA  >600 && joystickB >600 && endStopReadAL == HIGH && endStopReadBL == HIGH)
{  
  // Run the motor forward at 200 steps/second until the motor reaches 400 steps (2 revolutions):
  while(stepper.currentPosition() != distance)
  {
    stepper.setSpeed(moveSpeedAR);
    stepper.runSpeed();
     stepperB.setSpeed(moveSpeedBL);
    stepperB.runSpeed();
  } 
 }

//A=high B=low
   if (joystickA  >600 && joystickB <400 && endStopReadAL == HIGH && endStopReadBR == HIGH )
{  
  // Run the motor forward at 200 steps/second until the motor reaches 400 steps (2 revolutions):
  while(stepper.currentPosition() != distance)
  {
    stepper.setSpeed(moveSpeedAR);
    stepper.runSpeed();
     stepperB.setSpeed(moveSpeedBR);
    stepperB.runSpeed();
  } 
 }

 


 Serial.println(endStopReadAR);
 Serial.println(joystickA);
 
}