package org.usfirst.frc.team1760.robot.operations;

import org.usfirst.frc.team1760.robot.TitanRobot;
import org.usfirst.frc.team1760.robot.components.JoystickButton;
import org.usfirst.frc.team1760.robot.components.Switch;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SpeedController;

public class ElevatorOperator {
	public static final double AUTO_UP_SPEED = 0.75;
	public static final double PARKING_SPEED = -0.25;

	private TitanRobot robot;
	private SpeedController elevatorMotor;
	private Joystick operatorJoystick;
	private JoystickButton holdElevatorHighButton;
	private JoystickButton parkElevatorButton;
	private JoystickButton cancelElevatorHoldButton;
	private Switch elevatorUpperLimitSwitch;
	private Switch elevatorLowerLimitSwitch;

	private boolean holdHigh;
	private boolean parking;

	public ElevatorOperator(TitanRobot pRobot) {
		robot = pRobot;
		elevatorMotor = robot.getMotorStore().getElevatorMotor();
		operatorJoystick = robot.getJoystickStore().getOperatorJoystick();
		holdElevatorHighButton = robot.getJoystickStore().getHoldElevatorHighButton();
		parkElevatorButton = robot.getJoystickStore().getParkElevatorButton();
		cancelElevatorHoldButton = robot.getJoystickStore().getCancelElevatorHoldButton();
		elevatorUpperLimitSwitch = robot.getSwitchStore().getElevatorUpperLimitSwitch();
		elevatorLowerLimitSwitch = robot.getSwitchStore().getElevatorLowerLimitSwitch();
		holdHigh = false;
		parking = false;
	}

	public void periodic() {
        if (holdElevatorHighButton.isSwitchOn()) {
        	holdHigh = true;
        	parking = false;
        }
        else if (parkElevatorButton.isSwitchOn()) {
        	parking = true;
        	holdHigh = false;
        }
        else if (cancelElevatorHoldButton.isSwitchOn()) {
        	parking = false;
        	holdHigh = false;
        }

        double speed = 0.0;

        if (parking) {
            if (elevatorLowerLimitSwitch.isSwitchOn()) {
            	parking = false;
            }
            else {
            	speed = PARKING_SPEED;
            }
        }

        if (holdHigh) {
        	speed = AUTO_UP_SPEED;
        }
        else {
            double y = operatorJoystick.getY();
            speed = y * y;
        }

        if ((speed > 0.0) && elevatorUpperLimitSwitch.isSwitchOn()) {
        	speed = 0.0;
        }
        else if ((speed < 0.0) && elevatorLowerLimitSwitch.isSwitchOn()) {
        	speed = 0.0;
        }

        elevatorMotor.set(speed);
	}
}

@@ -352,359 +352,109 @@
 %0D%0A%09p
-ublic static final double AUTO_UP_SPEED = 0.75;%0D%0A%09public static final double PARKING_SPEED = -0.25;%0D%0A%0D%0A%09private TitanRobot robot;%0D%0A%09private SpeedController elevatorMotor;%0D%0A%09private Joystick operatorJoystick;%0D%0A%09private JoystickButton holdElevatorHighButton;%0D%0A%09private JoystickButton parkElevatorButton;%0D%0A%09private JoystickButton cancelElevatorHoldButton
+rivate TitanRobot robot;%0D%0A%09private SpeedController elevatorMotor;%0D%0A%09private Joystick operatorJoystick
 ;%0D%0A%09
@@ -544,65 +544,8 @@
 %0D%0A%0D%0A
-%09private boolean holdHigh;%0D%0A%09private boolean parking;%0D%0A%0D%0A
 %09pub
@@ -743,1269 +743,435 @@
 %0D%0A%09%09
-holdElevatorHighButton = robot.getJoystickStore().getHoldElevatorHighButton();%0D%0A%09%09parkElevatorButton = robot.getJoystickStore().getParkElevatorButton();%0D%0A%09%09cancelElevatorHoldButton = robot.getJoystickStore().getCancelElevatorHoldButton();%0D%0A%09%09elevatorUpperLimitSwitch = robot.getSwitchStore().getElevatorUpperLimitSwitch();%0D%0A%09%09elevatorLowerLimitSwitch = robot.getSwitchStore().getElevatorLowerLimitSwitch();%0D%0A%09%09holdHigh = false;%0D%0A%09%09parking = false;%0D%0A%09%7D%0D%0A%0D%0A%09public void periodic() %7B%0D%0A        if (holdElevatorHighButton.isSwitchOn()) %7B%0D%0A        %09holdHigh = true;%0D%0A        %09parking = false;%0D%0A        %7D%0D%0A        else if (parkElevatorButton.isSwitchOn()) %7B%0D%0A        %09parking = true;%0D%0A        %09holdHigh = false;%0D%0A        %7D%0D%0A        else if (cancelElevatorHoldButton.isSwitchOn()) %7B%0D%0A        %09parking = false;%0D%0A        %09holdHigh = false;%0D%0A        %7D%0D%0A%0D%0A        double 
+elevatorUpperLimitSwitch = robot.getSwitchStore().getElevatorUpperLimitSwitch();%0D%0A%09%09elevatorLowerLimitSwitch = robot.getSwitchStore().getElevatorLowerLimitSwitch();%0D%0A%09%7D%0D%0A%0D%0A%09public void periodic() %7B%0D%0A        double speed = 0.0;%0D%0A    double y = operatorJoystick.getY();%0D%0A    speed = y * y;%0D%0A    if ((speed %3E= -0.05) && (
 speed 
+%3C
 = 0.0
-;%0D%0A%0D%0A        if (parking) %7B%0D%0A            if (elevatorLowerLimitSwitch.isSwitchOn()) %7B%0D%0A            %09parking = false;%0D%0A            %7D%0D%0A            else %7B%0D%0A            %09speed = PARKING_SPEED;%0D%0A            %7D%0D%0A        %7D%0D%0A%0D%0A        if (holdHigh) %7B%0D%0A        %09speed = AUTO_UP_SPEED;%0D%0A        %7D%0D%0A        else %7B%0D%0A            double y = operatorJoystick.getY();%0D%0A            speed = y * y;%0D%0A        %7D%0D%0A
+5)) %7B%0D%0A    %09elevatorMotor.set(0.0);%0D%0A    %09// Apply Brake%0D%0A    %7D%0D%0A    else %7B%0D%0A    %09//release brake
 %0D%0A  
@@ -1243,36 +1243,51 @@
  %7B%0D%0A        
-%09speed = 
+    %09elevatorMotor.set(
 0.0
+)
 ;%0D%0A        %7D
@@ -1376,20 +1376,35 @@
     
-%09speed = 
+    %09elevatorMotor.set(
 0.0
+)
 ;%0D%0A 
@@ -1413,18 +1413,33 @@
      %7D%0D%0A
+        else %7B
 %0D%0A
+%09
         
@@ -1465,15 +1465,84 @@
 peed);%0D%0A
-%09%7D
+        %7D%0D%0A        %7D%0D%0A%09%7D%0D%0A%09// Solenoid off? (need to keep brake state?)
 %0D%0A%7D%0D%0A

Remove parking and holding logic