/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.Hand;


/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the TimedRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the build.gradle file in the
 * project.
 */
public class Robot extends TimedRobot {
  private static final String kDefaultAuto = "Default";
  private static final String kCustomAuto = "My Auto";
  private final SendableChooser<String> m_chooser = new SendableChooser<>();

  // Create xbox controller variable.
  private XboxController xbox;
  // Create a constant representing the port the xbox controller is plugged into
  private final int PORT = 0;

  /**
   * This function is run when the robot is first started up and should be
   * used for any initialization code.
   */
  @Override
  public void robotInit() {
    m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
    m_chooser.addOption("My Auto", kCustomAuto);
    SmartDashboard.putData("Auto choices", m_chooser);

    //Instantiate xbox controller object
    // the argument is the USB port that controller is plugged into
    xbox = new XboxController(PORT);
 
  }

  /**
   * This function is called periodically during operator control.
   */
  @Override
  public void teleopPeriodic() {

    // Print "Y button pressed" every time the button is pressed
    if (xbox.getYButtonPressed()) {
      System.out.println("Y button pressed");
    }
    else if (xbox.getXButtonPressed()){
      System.out.println("X button presssed");
    }
    else if (xbox.getAButtonPressed()) {
      System.out.println("A button pressed");
    }
    else if (xbox.getBButtonPressed()) {
      System.out.println("B button pressed");
    }

    // Get and print x,y values of left joystick
    double x = xbox.getX(Hand.kLeft);
    double y = xbox.getY(Hand.kLeft);
    System.out.println("left Joystick (" + x + ", " + y + ")");

    // Get and print x,y values of right joystick
    x = xbox.getX(Hand.kRight);
    y = xbox.getY(Hand.kRight);
    System.out.println("right Joystick (" + x + ", " + y + ")");
  }
}