/*
Write code to enables four BTS7960B motor controllers. The motor controllers should to be activated whenever the joystick is activated. 
Direction of the Motor drivers is selected via a switch. 
When in Position A, all motor drivers move in the forward direction.
When in Position B, all motor drivers move in the reverse direction.  
If the joystick goes UP, both FRONT LEFT and FRONT RIGHT Motor Drivers are activated.
If the joystick goes DOWN, both REAR LEFT and REAR RIGHT Motor Drivers are activated.
If the joystick goes LEFT, both FRONT LEFT and REAR LEFT Motor Drivers are activated.
If the joystick goes RIGHT, both FRONT RIGHT and REAR RIGHT Motor Drivers are activated.
When no joystick buttons are pressed, all Motors Stop. Motors only move when the joystick is activated.

The below code is accurate in terms of need of use, but edit as needed. 
*/

// Define pins for BTS7960 motor driver #1 (UP)
const int motorRPWM1 = 32; // Connect to RPWM pin 1 of first BTS7960
const int motorLPWM1 = 33; // Connect to RPWM pin 2 of first BTS7960

// Define pins for BTS7960 motor driver #2 (DOWN)
const int motorRPWM2 = 26; // Connect to RPWM pin 1 of first BTS796
const int motorLPWM2 = 25; // Connect to RPWM pin 2 of first BTS7960

// Define pins for BTS7960 motor driver #3 (LEFT)
const int motorRPWM3 = 5; // Connect to RPWM pin 1 of first BTS796
const int motorLPWM3 = 18; // Connect to RPWM pin 2 of first BTS7960

// Define pins for BTS7960 motor driver #4 (RIGHT)
const int motorRPWM4 = 19; // Connect to RPWM pin 1 of first BTS796
const int motorLPWM4 = 21; // Connect to RPWM pin 2 of first BTS7960

// Define pin for analog joystick X-axis, Y-axis & push button of joystick
const int joystickXPin = 12; // Connect to X-axis pin of joystick
const int joystickYPin = 13; // Connect to Y-axis pin of joystick
const int buttonPin = 14; // Connect to push button pin of joystick

// Define thresholds for joystick direction
const int joystickThresholdUp = 2300; // Threshold for detecting joystick push up
const int joystickThresholdDown = 1700;   // Threshold for detecting joystick push down
const int joystickThresholdRight = 2300; // Threshold for detecting joystick push up
const int joystickThresholdLeft = 1700;   // Threshold for detecting joystick push down

bool forwardDirection = true; // Flag to track motor direction

void setup() {
  // Initialize serial communication
  Serial.begin(9600);

  // Set motor and joystick pins as inputs/outputs
  pinMode(motorRPWM1, OUTPUT);
  pinMode(motorLPWM1, OUTPUT);
  pinMode(motorRPWM2, OUTPUT);
  pinMode(motorLPWM2, OUTPUT);
  pinMode(motorRPWM3, OUTPUT);
  pinMode(motorLPWM3, OUTPUT);
  pinMode(motorRPWM4, OUTPUT);
  pinMode(motorLPWM4, OUTPUT);
  pinMode(joystickXPin, INPUT);
  pinMode(joystickYPin, INPUT);
  pinMode(buttonPin, INPUT_PULLUP); // Enable internal pull-up resistor for button

  // Initially stop the motor
  stopMotor();

  // Print initial message to Serial monitor
  Serial.println("Motor control initialized");
}

void loop() {

  // Read joystick X-axis & Y-axis values
  int joystickX = analogRead(joystickXPin);
  int joystickY = analogRead(joystickYPin);
  // Check if the button is pressed to change motor direction
  if (digitalRead(buttonPin) == LOW) {
    forwardDirection = !forwardDirection; // Toggle motor direction
    delay(150); // Debounce delay
  }

  // Control motor direction based on joystick position and button state
  if (forwardDirection) {
          if (joystickX > joystickThresholdUp) {     // Joystick pushed up, move motor forward for the first BTS7960
            moveForward1();
            Serial.println("Motor 1 moving forward");
          } else if (joystickX < joystickThresholdDown) {      // Joystick pushed down, move motor forward for the second BTS7960
            moveForward2();
            Serial.println("Motor 2 moving forward");
          } else if (joystickY < joystickThresholdDown) {      // Joystick pushed left, move motor forward for the second BTS7960
            moveForward3();
            Serial.println("Motor 3 moving forward");
          } else if (joystickY > joystickThresholdUp) {       // Joystick pushed right, move motor forward for the second BTS7960
            moveForward4();
            Serial.println("Motor 4 moving forward");
          } else {      // Joystick not pushed up or down, stop both motors
            stopMotor();
            Serial.println("Motors stopped");
          }
  } 
  else {
          if (joystickX > joystickThresholdUp) {       // Joystick pushed down, move motor in reverse for the first BTS7960
            moveReverse1();
            Serial.println("Motor 1 moving in reverse");
          } else if (joystickX < joystickThresholdDown) {       // Joystick pushed up, move motor in reverse for the second BTS7960
            moveReverse2();
            Serial.println("Motor 2 moving in reverse");
          } else if (joystickY < joystickThresholdDown) {       // Joystick pushed left, move motor forward for the second BTS7960
            moveReverse3();
            Serial.println("Motor 3 moving reverse");
          } else if (joystickY > joystickThresholdUp) {       // Joystick pushed right, move motor forward for the second BTS7960
            moveReverse4();
            Serial.println("Motor 4 moving reverse");
          } else {      // Joystick not pushed up or down, stop both motors
            stopMotor();
            Serial.println("Motors stopped");
          }
        }

  // Add a short delay to prevent rapid changes
  delay(250);
}

//-------Function to move the motor forward--------//
// Function to move the first motor forward
void moveForward1() {
  digitalWrite(motorRPWM1, LOW);
  digitalWrite(motorLPWM1, HIGH);
}
// Function to move the second motor forward
void moveForward2() {
  digitalWrite(motorRPWM2, LOW);
  digitalWrite(motorLPWM2, HIGH);
}
// Function to move the third motor forward
void moveForward3() {
  digitalWrite(motorRPWM3, LOW);
  digitalWrite(motorLPWM3, HIGH);
}
// Function to move the forth motor forward
void moveForward4() {
  digitalWrite(motorRPWM4, LOW);
  digitalWrite(motorLPWM4, HIGH);
}
//-------Function to move the motor backwards--------//
// Function to move the first motor in reverse
void moveReverse1() {
  digitalWrite(motorRPWM1, HIGH);
  digitalWrite(motorLPWM1, LOW);
}
// Function to move the second motor in reverse
void moveReverse2() {
  digitalWrite(motorRPWM2, HIGH);
  digitalWrite(motorLPWM2, LOW);
}
// Function to move the first motor in reverse
void moveReverse3() {
  digitalWrite(motorRPWM3, HIGH);
  digitalWrite(motorLPWM3, LOW);
}
// Function to move the second motor in reverse
void moveReverse4() {
  digitalWrite(motorRPWM4, HIGH);
  digitalWrite(motorLPWM4, LOW);
}
// Function to stop the motor
void stopMotor() {
  digitalWrite(motorRPWM1, LOW);
  digitalWrite(motorLPWM1, LOW);
  digitalWrite(motorRPWM2, LOW);
  digitalWrite(motorLPWM2, LOW);
  digitalWrite(motorRPWM3, LOW);
  digitalWrite(motorLPWM3, LOW);
  digitalWrite(motorRPWM4, LOW);
  digitalWrite(motorLPWM4, LOW);
}