// Forum: https://forum.arduino.cc/t/stepper-motor-breaks-the-entire-code-if-i-move-the-xmotor-just-a-bit/1127427/
// Sketch by: badmans
// This Wokwi project: https://wokwi.com/projects/364859540746806273

#include <Stepper.h>
#define joystick A0

// define number of steps per revolution
#define STEPS 32

// define stepper motor control pins
#define XIN1 A1
#define XIN2 A2
#define XIN3 A3
#define XIN4 A4

#define YIN1 8
#define YIN2 9
#define YIN3 10
#define YIN4 11

// initialize stepper library
Stepper stepperX(STEPS, XIN4, XIN2, XIN3, XIN1);
Stepper stepperY(STEPS, YIN4, YIN2, YIN3, YIN1);

const int whiteled = 2;
const int redled = 3;
const int blueled = 4;
const int greenled = 5;


//redledmotorbrakes

const int buttonPin = 6;
const int sensitivity = 600;
int joystickState = 0;        // current state of the joystick button
int lastJoystickState = 1;    // previous state of the joystick button
int counter = 0;

void setup() {
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(redled, OUTPUT);
  pinMode(blueled, OUTPUT);
  pinMode(whiteled, OUTPUT);
  Serial.begin(9600);
  digitalWrite(greenled, HIGH);
}

// add button that shuts off all components
// figure out why the ymotor turns even tho its on whiteled
// MOTOR TURNS BECAUSE WHITELED = 0 and same with BLUELED. FIX: Make a Counter that goes from 0-2 then bounces between 1-2 once past 0
// find way to make motors stay on but not move
void loop() {

  joystickState = digitalRead(buttonPin);
  int val = analogRead(joystick);

  // check for a valid joystick button press (with software debounce)
  if (joystickState != lastJoystickState && joystickState == LOW) {
    counter++;

    if (counter > 2) {
      counter = 1;
    }
  }
  
  lastJoystickState = joystickState;

  delay(50);
  if (counter == 0) {
    allmotor_off();
    WHITElight();
  }
  else if (counter == 1) {
    ymotor();
    ymotor_off();
    BLUElight();

    Serial.println("Controlling X-Axis");
  }
  else if (counter == 2) {
    xmotor();
    xmotor_off();
    REDlight();
    Serial.println("Controlling Y-Axis");
  }
}
int allmotor_off() {
  xmotor_off();
  ymotor_off();
}

int xmotor_off() {
  digitalWrite(XIN1, LOW);
  digitalWrite(XIN2, LOW);
  digitalWrite(XIN3, LOW);
  digitalWrite(XIN4, LOW);
}

int ymotor_off() {
  digitalWrite(YIN1, LOW);
  digitalWrite(YIN2, LOW);
  digitalWrite(YIN3, LOW);
  digitalWrite(YIN4, LOW);
}

int xmotor() {
  int valX = analogRead(joystick);

  // if the joystic is in the middle ===> stop the motor
  if (  (valX > 500)  && (valX < sensitivity))
  {
    xmotor_off();
  }
  else
  {
    // move the motor in the first direction
    while (valX  >= 515)
    {
      // map the speed between 5 and 500 rpm
      int speed_  = map(valX, sensitivity, 1023, 5, 700);
      // set motor speed
      stepperX.setSpeed(speed_);

      // move the motor (1 step)
      stepperX.step(-1);

      valX  = analogRead(joystick);
    }

    // move the motor in the other direction
    while (valX <= 500)
    {
      // map the speed between 5 and 500 rpm
      int speed_  = map(valX, 500, 0, 5, 700);

      // set motor speed
      stepperX.setSpeed(speed_);

      // move the motor (1 step)
      stepperX.step(1);

      valX  = analogRead(joystick);
    }
  }
}

int ymotor() { //BLUE IS YAXIS
  int valY = analogRead(joystick);

  // if the joystic is in the middle ===> stop the motor
  if (  (valY > 500)  && (valY < sensitivity) )
  {
    ymotor_off();
  }

  else
  {
    // move the motor in the first direction
    while (valY  >= sensitivity)
    {
      // map the speed between 5 and 500 rpm
      int speed_  = map(valY, sensitivity, 1023, 5, 700);
      // set motor speed
      stepperY.setSpeed(speed_);

      // move the motor (1 step)
      stepperY.step(1);

      valY  = analogRead(joystick);
    }

    // move the motor in the other direction
    while (valY <= 500)
    {
      // map the speed between 5 and 500 rpm
      int speed_  = map(valY, 500, 0, 5, 700);
      // set motor speed
      stepperY.setSpeed(speed_);

      // move the motor (1 step) - sign to change the direction of it.
      stepperY.step(-1);

      valY  = analogRead(joystick);
    }
  }
}

int REDlight() {
  digitalWrite(redled, HIGH);
  digitalWrite(blueled, LOW);
  digitalWrite(whiteled, LOW);
}

int BLUElight() {
  digitalWrite(redled, LOW);
  digitalWrite(blueled, HIGH);
  digitalWrite(whiteled, LOW);
}

int WHITElight() {
  digitalWrite(redled, LOW);
  digitalWrite(blueled, LOW);
  digitalWrite(whiteled, HIGH);
}