#ifndef DRIVETRAIN_H
#define DRIVETRAIN_H

#include "WPILib.h"
/**
 * @brief Drives the robot
 *
 * 4 wheel mecanum drive
 */
class Drivetrain: public Subsystem {
 private:
  CANTalon *rightRear, //<! Back right motor
           *leftRear; //<! Back left motor
  Gyro *gyro; //<! Gyro
 public:
  /**
   * @brief Constructs Drivetrain
   */
  Drivetrain();
  /**
   * @brief Current motor to test
   */
  enum e_motors {
    BACKRIGHT, //<! Back right motor
    BACKLEFT //<! Back left motor
  };
  /**
   * @brief No action
   */
  void InitDefaultCommand();
  /**
   * @brief Drives the robot with arcade
   *
   * @param x Joystick x value (-1.0 to 1.0)
   * @param y Joystick y value (-1.0 to 1.0)
   * @param z Joystick z value (-1.0 to 1.0)
   * @param sensitivity Sensitivity (0.0 to 1.0)
   * @param driveStraight Overrides z value to correct for motor lag (default: false)
   */
  void DriveArcade(double x, double y, double z, double sensitivity, bool driveStraight = false);
  /**
   * @brief Tests one motor
   *
   * @param motor Motor to test
   * @param power Power to set motor
   */
  void TestMotor(e_motors motor, double power);
  /**
   * @brief Sets the gyro value to 0.0
   */
  void ResetGyro();
};
#endif
// vim: ts=2:sw=2:et