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

import org.usfirst.frc.team4533.robot.Robot;
import org.usfirst.frc.team4533.robot.subsystems.DriveSystem;

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

/**
 *
 */
public class AutoRotate extends Command {

	boolean positive = true;
	double degree;
	double startHeading;

	public AutoRotate(double deg) {
		requires(Robot.drive);

		// Use requires() here to declare subsystem dependencies
		// eg. requires(chassis);
		degree = deg;
		if (deg > 0) {
			positive = true;
		} else {
			positive = false;
		}
	}

	// Called just before this Command runs the first time
	protected void initialize() {
		startHeading = Robot.heading;
		SmartDashboard.putNumber("start heading", startHeading);
		System.out.println("start heading: " + startHeading);
	}

	// Called repeatedly when this Command is scheduled to run
	protected void execute() {

		if (positive) {
			Robot.drive.turnRight(.25, -.25);

		} else {
			Robot.drive.turnLeft(-.25, .25);
		}
	}

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

		int sign = 1;
		double heading = Robot.heading;

		// if (heading < startHeading) { sign = -1; }
		// offset = sign * (Math.floorMod((int) Math.abs(heading -
		// startHeading), 360));
		// SmartDashboard.putNumber("offset", offset);
		int offset = Math.floorMod((int) Math.abs(heading - startHeading), 360);
		if (offset > 180) {
			offset = Math.abs(offset - 360);
		}
		if (degree < 0 && offset < degree) {
			System.out.println(offset);
			return true;
		} else if (degree > 0 && offset > degree) {
			System.out.println(offset);
			return true;
		}
		return false;
	}

	// Called once after isFinished returns true
	protected void end() {
	}

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

@@ -346,21 +346,23 @@
 %09double 
-degre
+toRotat
 e;%0A%09doub
@@ -441,184 +441,30 @@
 e);%0A
-%0A
 %09%09
-// Use requires() here to declare subsystem dependencies%0A%09%09// eg. requires(chassis);%0A%09%09degree = deg;%0A%09%09if (deg %3E 0) %7B%0A%09%09%09positive = true;%0A%09%09%7D else %7B%0A%09%09%09positive = false;%0A%09%09%7D
+this.toRotate = deg;
 %0A%09%7D%0A
@@ -587,123 +587,8 @@
 ng;%0A
-%09%09SmartDashboard.putNumber(%22start heading%22, startHeading);%0A%09%09System.out.println(%22start heading: %22 + startHeading);%0A
 %09%7D%0A%0A
@@ -679,23 +679,26 @@
 ) %7B%0A
-%0A
 %09%09if (
-positive
+toRotate %3E 0
 ) %7B%0A
@@ -734,17 +734,16 @@
  -.25);%0A
-%0A
 %09%09%7D else
@@ -903,25 +903,8 @@
 () %7B
-%0A%0A%09%09int sign = 1;
 %0A%09%09d
@@ -938,61 +938,16 @@
 ng;%0A
-%0A
 %09%09
-// if (heading %3C startHeading) %7B sign = -1; %7D%0A%09%09//
+double
  off
@@ -956,16 +956,8 @@
 t = 
-sign * (
 Math
@@ -959,37 +959,37 @@
  Math.floorMod((
-int) 
+long)
 Math.abs(heading
@@ -990,21 +990,16 @@
 eading -
-%0A%09%09//
  startHe
@@ -1010,133 +1010,14 @@
 g), 
-360));%0A%09%09// SmartDashboard.putNumber(%22offset%22, offset);%0A%09%09int offset = Math.floorMod((int) Math.abs(heading - startHeadi
+(lo
 ng)
-, 
 360)
@@ -1090,156 +1090,87 @@
 if (
-degree %3C 0 && offset %3C degree) %7B%0A%09%09%09System.out.println(offset);%0A%09%09%09return true;%0A%09%09%7D else if (degree %3E 0 && offset %3E degree) %7B%0A%09%09%09System.out.println(
+offset %3E Math.abs(toRotate)) %7B%0A%09%09%09System.out.println(%22Stopping rotation at %22 + 
 offs

Use longs