package org.lunatecs316.frc2013;

import edu.wpi.first.wpilibj.Joystick;
import org.lunatecs316.frc2013.lib.Util;
import org.lunatecs316.frc2013.subsystems.*;

/**
 * The OI (Operator Interface) manages control of the robot during teleop
 * @author domenicpaul
 */
public class OI {
    
    /* Joysticks */
    private static Joystick driverController;
    private static Joystick operatorJoystick;
    
    /**
     * Initialize the Operator Interface
     */
    public static void init(Joystick js1, Joystick js2) {
        driverController = js1;
        operatorJoystick = js2;
    }
    
    public static void run() {
        // Run each subsystem's section
        runDrivetrain();
        runPickup();
        runShooter();
        runClimber();
    }
    
    /**
     * Run the drivetrain section
     */
    private static boolean firstPressDB1 = true;
    private static boolean firstPressDB2 = true;
    private static boolean firstPressDB3 = true;
    private static void runDrivetrain() {
        if (driverController.getRawButton(1)) {
            if (firstPressDB1) {
                Drivetrain.setTargetDistance(48);
                firstPressDB1 = false; 
            } else {
                Drivetrain.driveStraight();
            }
        } else if (driverController.getRawButton(2)) {
            if (firstPressDB2) {
                Drivetrain.setTargetDistance(24);
                firstPressDB2 = false;
            } else {
                Drivetrain.driveStraight();
            }
        } else if (driverController.getRawButton(3) || driverController.getRawButton(4)) {
            if (firstPressDB3) {
                if (driverController.getRawButton(3)) Drivetrain.setTargetAngle(90);
                if (driverController.getRawButton(4)) Drivetrain.setTargetAngle(-90);
                firstPressDB3 = false;
            } else {
                Drivetrain.turn();
            }
        } else {
            firstPressDB1 = true;
            firstPressDB2 = true;
            firstPressDB3 = true;
            Drivetrain.arcadeDrive(driverController);
        }
        
        if (driverController.getRawButton(4)) {
            Drivetrain.resetGyro();
        }
    }
    
    /**
     * Run the pickup section
     */
    private static void runPickup() {
        
        // Angle control
        if (driverController.getRawButton(5)) {
            Pickup.raise();
        } else if (driverController.getRawButton(6)) {
            Pickup.lower();
        } else {
            Pickup.stop();
        }
        
        // Belt control
        if (operatorJoystick.getRawButton(6)) {
            Pickup.setBeltState(Pickup.BeltState.Reverse);
        } else if (operatorJoystick.getRawButton(7)) {
            Pickup.setBeltState(Pickup.BeltState.Forwards);
        } else {
            Pickup.setBeltState(Pickup.BeltState.Off);
        }
    }
    
    /**
     * Run the shooter section
     */
    private static void runShooter() {
        
        // Angle control
        if (operatorJoystick.getRawButton(11)) {
            Shooter.moveToPosition(Shooter.kTopPosition);
        } else if (operatorJoystick.getRawButton(10)) {
            Shooter.moveToPosition(Shooter.kMidPosition);
        } else if (operatorJoystick.getRawButton(8)) {
            Shooter.moveToPosition(Shooter.kLoadPosition);
        } else {
            Shooter.move(Util.deadband(operatorJoystick.getY(), 0.2));
        }
        
        // Motor control
        if (operatorJoystick.getRawButton(2)) {
            Shooter.enable();    
        } else if (operatorJoystick.getRawButton(5)) {
            Shooter.setSpeed(-1.0);
        } else if (operatorJoystick.getRawButton(1)) {
            Shooter.autoFire();
        } else {
            Shooter.disable();
        }
        
        // Firing control
        if (operatorJoystick.getRawButton(2) || operatorJoystick.getRawButton(5))
            Shooter.fire(operatorJoystick.getRawButton(1));
        else if (!operatorJoystick.getRawButton(1))
            Shooter.fire(false);
        
        // Indicator lights
        Shooter.indications();
        
        Debugger.log("shooterAngle", Shooter.getAngle());
    }
    
    /**
     * Run the climber section
     */
    private static void runClimber() {
        Climber.climb(operatorJoystick.getRawButton(4));
    }
}

@@ -276,21 +276,17 @@
 ss OI %7B%0A
-    
 %0A
+
     /* J
@@ -389,20 +389,164 @@
 ystick;%0A
+%0A
     
+private static boolean firstPressDB1 = true;%0A    private static boolean firstPressDB2 = true;%0A    private static boolean firstPressDB3 = true;%0A
 %0A    /**
@@ -719,29 +719,25 @@
  js2;%0A    %7D%0A
-    
 %0A
+
     public s
@@ -769,377 +769,41 @@
   //
- Run each subsystem's section%0A        runDrivetrain();%0A        runPickup();%0A        runShooter();%0A        runClimber();%0A    %7D%0A    %0A    /**%0A     * Run the drivetrain section%0A     */%0A    private static boolean firstPressDB1 = true;%0A    private static boolean firstPressDB2 = true;%0A    private static boolean firstPressDB3 = true;%0A    private static void runDrivetrain() %7B
+%0A        // Drivetrain%0A        //
 %0A   
@@ -968,17 +968,16 @@
 = false;
- 
 %0A       
@@ -1887,33 +1887,25 @@
 ;%0A        %7D%0A
-        
 %0A
+
         if (
@@ -1990,102 +1990,37 @@
   %7D%0A
-    %7D
 %0A    
-%0A
     /
-**
+/
 %0A     
-* Run the pickup section%0A     */%0A    private static void run
+   // 
 Pickup
-() %7B
 %0A   
@@ -2016,32 +2016,35 @@
  Pickup%0A        
+//%0A
 %0A        // Angl
@@ -2262,33 +2262,25 @@
 ;%0A        %7D%0A
-        
 %0A
+
         // B
@@ -2599,104 +2599,38 @@
   %7D%0A
-    %7D
 %0A    
-%0A
     /
-**
+/
 %0A     
-* Run the shooter section%0A     */%0A    private static void run
+   // 
 Shooter
-() %7B
 %0A   
@@ -2626,32 +2626,35 @@
 Shooter%0A        
+//%0A
 %0A        // Angl
@@ -3092,33 +3092,25 @@
 ;%0A        %7D%0A
-        
 %0A
+
         // M
@@ -3199,20 +3199,16 @@
 nable();
-    
 %0A       
@@ -3436,25 +3436,17 @@
       %7D%0A
-        
 %0A
+
         
@@ -3690,25 +3690,17 @@
 false);%0A
-        
 %0A
+
         
@@ -3754,17 +3754,9 @@
 ();%0A
-        
 %0A
+
     
@@ -3813,104 +3813,50 @@
 ));%0A
-    %7D
 %0A    
-%0A
     /
-**
+/
 %0A     
-* Run the climber section%0A     */%0A    private static void runClimber() %7B
+   // Climbing%0A        //
 %0A   

Combine OI methods into one method