package org.usfirst.frc.team997.robot.commands;

import org.usfirst.frc.team997.robot.Robot;
import org.usfirst.frc.team997.robot.RobotMap;

import edu.wpi.first.wpilibj.command.Command;

/**
 *
 */
public class Drive extends Command {
    public static boolean useAccelerationControl = false;
    

	public Drive() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.driveTrain);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	if (useAccelerationControl) {
    		Robot.driveTrain.startPID();
    	}
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	double left, right;
    	if(Robot.oi.isTank) {
    		left = Robot.deadband(Robot.oi.getLeftY());
    		right = Robot.deadband(Robot.oi.getRightX());
    	} else {
    		double aleft = Robot.deadband(Robot.oi.getLeftY());
    		double aright = Robot.deadband(Robot.oi.getRightX()) * 
    				(RobotMap.Values.driveTrainMaxTurn -
    						(RobotMap.Values.driveTrainMaxTurn -
    								RobotMap.Values.driveTrainMinTurn) *
    						Math.abs(aleft));
    		left = aleft + aright;
    		right = aleft - aright;
    	}
    	if (useAccelerationControl) {
    		Robot.driveTrain.drivePID(left * 200, right * 200);
    	} else {
    		if(Robot.oi.useDeccelerationControl && Robot.oi.forward) {
        		Robot.driveTrain.driveDeccel(Robot.clamp(left), Robot.clamp(right));
        		
        	} else if(Robot.oi.useDeccelerationControl && !Robot.oi.forward) {
        		Robot.driveTrain.driveVoltage(-Robot.clamp(left), -Robot.clamp(right));
        		
        	} else if(Robot.oi.forward && !Robot.oi.useDeccelerationControl) {
    			Robot.driveTrain.driveVoltage(Robot.clamp(left), Robot.clamp(right));
    			
    		} else if(!Robot.oi.forward && !Robot.oi.useDeccelerationControl) {
    			Robot.driveTrain.driveVoltage(-Robot.clamp(left), -Robot.clamp(right));
    		}
    	}
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    	if (useAccelerationControl) {
    		Robot.driveTrain.stopPID();
    	}
    	Robot.driveTrain.driveVoltage(0, 0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	end();
    }
}

@@ -1409,346 +1409,88 @@
 %09%09if
-(Robot.oi.useDeccelerationControl && Robot.oi.forward) %7B%0D%0A        %09%09Robot.driveTrain.driveDeccel(Robot.clamp(left), Robot.clamp(right));%0D%0A   
+ (!Robot.oi.forward) %7B%0D%0A    %09%09%09left *= -1;%0D%0A
     
- 
 %09%09
-%0D%0A        %09%7D else if(Robot.oi.useDeccelerationControl && !Robot.oi.forward) %7B%0D%0A        %09%09Robot.driveTrain.driveVoltage(-Robot.clamp(left), -Robot.clamp(right));%0D%0A   
+%09right *= -1;%0D%0A
     
- 
 %09%09
+%7D
 %0D%0A
-    
     %09
-%7D else 
+%09
 if(R
@@ -1497,37 +1497,16 @@
 obot.oi.
-forward && !Robot.oi.
 useDecce
@@ -1521,33 +1521,36 @@
 Control) %7B%0D%0A    
+  
-%09
+  
 %09%09Robot.driveTra
@@ -1557,23 +1557,22 @@
 in.drive
-Voltage
+Deccel
 (Robot.c
@@ -1613,84 +1613,19 @@
     
-%09%09%09%0D%0A
     %09
-%09
 %7D else
- if(!Robot.oi.forward && !Robot.oi.useDeccelerationControl)
  %7B%0D%0A
@@ -1661,17 +1661,16 @@
 Voltage(
--
 Robot.cl
@@ -1680,17 +1680,16 @@
 (left), 
--
 Robot.cl
@@ -1702,25 +1702,28 @@
 ght));%0D%0A    
-%09
+    
 %09%7D%0D%0A    %09%7D%0D%0A

Simplify logic of drive reversal decceleration