#include "vex.h"

using namespace vex;
// Declare a variable that stores if the toggle is on orr off
bool slowDrive = 0;
// Declare a variable for our speed multiplier
float multiplier = 1; // 100%

// Declare function for button callback
void speedToggle() {
  // toggle the drive speed setting
  slowDrive = !slowDrive;
  if(slowDrive) {
      multiplier = 0.3; // 30%
  }
  else {
      multiplier = 1; // 100%
  }

  // Wait until B is released so holding doesn't toggle forever
  waitUntil(!Controller1.ButtonB.pressing());
}

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  float leftSpeed;
  float rightSpeed;

  // set the callback function to the speedToggle function above
  // whenever B button is pressed we run the code in speedToggle in the background
  Controller1.ButtonB.pressed(speedToggle);

  while (true) {
    // combine joystick values to control robot with arcade drive
    // fwd-back and left-right on left joystick
    // our mltiplier controls what % of the joystick value to apply
    leftSpeed = (Controller1.Axis3.position() + Controller1.Axis4.position()) * multiplier;
    rightSpeed = (Controller1.Axis3.position() - Controller1.Axis4.position()) * multiplier;

    // use calculated speed to drive motors
    LeftMotor.spin(forward, leftSpeed, velocityUnits::pct);
    RightMotor.spin(forward, rightSpeed, velocityUnits::pct);

    wait(25, msec);
  }
}