package com.team687.frc2017;

import com.team687.frc2017.commands.ResetGyro;
import com.team687.frc2017.commands.ShiftDown;
import com.team687.frc2017.commands.ShiftUp;
import com.team687.frc2017.commands.SnapToTarget;
import com.team687.frc2017.commands.TankDrive;
import com.team687.frc2017.commands.TurnToAngle;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 * 
 * @author tedfoodlin
 * 
 */

public class OI {
	
	public Joystick driveJoyLeft = new Joystick(0);
	public Joystick driveJoyRight = new Joystick(1);
	
	// buttons on driveJoyRight
	public JoystickButton zeroGyro_9;
	public JoystickButton clearAll_7;
	public JoystickButton quickTurn_1;
	
	public JoystickButton snapToTarget_2;
	public JoystickButton turnToAngle_5;
	public JoystickButton shiftDown_4;
	
	public JoystickButton shiftUp_3;
	
	public OI() {
		zeroGyro_9 = new JoystickButton(driveJoyRight, 9);
		zeroGyro_9.whenPressed(new ResetGyro());
		clearAll_7 = new JoystickButton(driveJoyRight, 7);
		clearAll_7.cancelWhenPressed(Robot.drive.getCurrentCommand());
		quickTurn_1 = new JoystickButton(driveJoyRight, 1);
		
		snapToTarget_2 = new JoystickButton(driveJoyRight, 2);
		snapToTarget_2.whenPressed(new SnapToTarget(15));
		turnToAngle_5 = new JoystickButton(driveJoyRight, 5);
		turnToAngle_5.whenPressed(new TurnToAngle(45, 15));
		shiftDown_4 = new JoystickButton(driveJoyRight, 4);
		shiftDown_4.whenPressed(new ShiftDown());
		
		shiftUp_3 = new JoystickButton(driveJoyRight, 3);
		shiftUp_3.whenPressed(new ShiftUp());
		
		
		// Smart Dashboard buttons
		SmartDashboard.putData("Tank Drive", new TankDrive());
		SmartDashboard.putData("Snap to Target", new SnapToTarget(15));
		SmartDashboard.putData("Turn to 90", new TurnToAngle(90, 15));
		SmartDashboard.putData("Turn to 45", new TurnToAngle(45, 15));
		SmartDashboard.putData("Shift Up", new ShiftUp());
		SmartDashboard.putData("Shift Down", new ShiftDown());
		SmartDashboard.putData("Zero Gyro", new ResetGyro());
	}
	
	/**
	 * @return input power from left drive joystick (-1.0 to +1.0)
	 */
	public double getDriveJoyL() {
		return driveJoyLeft.getY();
	}
	
	/**
	 * @return input power from right drive joystick (-1.0 to +1.0)
	 */
	public double getDriveJoyR() {
		return driveJoyRight.getY();
	}
	
	/**
	 * @return input throttle from right drive joystick (0 to +1.0)
	 */
	public double getThrottleR() {
		return (driveJoyRight.getThrottle() + 1) / 2;
	}
	
	/**
	 * @return if quick turn state in cheesydrive mode
	 */
	public boolean getQuickTurn() {
		return quickTurn_1.get();
	}
}

@@ -59,25 +59,17 @@
 ommands.
-ResetGyro
+*
 ;%0Aimport
@@ -102,208 +102,15 @@
 nds.
-ShiftDown;%0Aimport com.team687.frc2017.commands.ShiftUp;%0Aimport com.team687.frc2017.commands.SnapToTarget;%0Aimport com.team687.frc2017.commands.TankDrive;%0Aimport com.team687.frc2017.commands.TurnToAngle
+tests.*
 ;%0A%0Ai
@@ -577,16 +577,163 @@
 k(1);%0A%09%0A
+%09// buttons on driveJoyLeft%0A%09public JoystickButton testSensors_9;%0A%09public JoystickButton testMinRotPower_7;%0A%09public JoystickButton testRotPID_1;%0A%09%0A
 %09// butt
@@ -820,17 +820,17 @@
 learAll_
-7
+8
 ;%0A%09publi
@@ -856,17 +856,17 @@
 ickTurn_
-1
+7
 ;%0A%09%0A%09pub
@@ -1134,17 +1134,17 @@
 learAll_
-7
+8
  = new J
@@ -1172,17 +1172,17 @@
 yRight, 
-7
+8
 );%0A%09%09cle
@@ -1187,17 +1187,17 @@
 learAll_
-7
+8
 .cancelW
@@ -1253,17 +1253,17 @@
 ickTurn_
-1
+7
  = new J
@@ -1291,17 +1291,17 @@
 yRight, 
-1
+7
 );%0A%09%09%0A%09%09
@@ -1713,16 +1713,337 @@
 ());%0A%09%09%0A
+%09%09testSensors_9 = new JoystickButton(driveJoyLeft, 9);%0A%09%09testSensors_9.whenPressed(new TestSensors());%0A%09%09testMinRotPower_7 = new JoystickButton(driveJoyLeft, 7);%0A%09%09testMinRotPower_7.whenPressed(new TestMinRotPower());%0A%09%09testRotPID_1 = new JoystickButton(driveJoyLeft, 1);%0A%09%09testRotPID_1.whenPressed(new TestRotPID());%0A%09%09%0A
 %09%09%0A%09%09// 
@@ -2485,16 +2485,211 @@
 yro());%0A
+%09%09%0A%09%09SmartDashboard.putData(%22Test Sensors%22, new TestSensors());%0A%09%09SmartDashboard.putData(%22Test Min Rot Power%22, new TestMinRotPower());%0A%09%09SmartDashboard.putData(%22Test Rot PID%22, new TestRotPID());%0A
 %09%7D%0A%09%0A%09/*
@@ -3243,17 +3243,17 @@
 ickTurn_
-1
+7
 .get();%0A

add tests to OI