package org.texastorque.texastorque2015.auto;

import org.texastorque.texastorque2015.input.Input;
import org.texastorque.texastorque2015.constants.Constants;

public abstract class AutoMode extends Input {

    public class DriveDistance extends AutoCommand {

        private double distance;
        private double doneRange;

        public DriveDistance(String name, double distance, double tolerance, double doneCycles, double timeOut) {
            super(name, doneCycles, timeOut);

            this.distance = distance;
            this.doneRange = tolerance;

            drivebaseControlled = true;
            driveDistance = distance;
            driveAngle = 0.0;

            feedback.resetDriveEncoders();
        }

        @Override
        public boolean isDone() {
            if (Math.abs((feedback.getLeftDrivePosition() + feedback.getRightDrivePosition()) / 2 - distance) <= doneRange) {
                doneCycles++;
            }
            return doneCycles >= minDoneCycles;
        }

        @Override
        public void stop() {
            drivebaseControlled = false;
            leftSpeed = 0.0;
            rightSpeed = 0.0;
            feedback.resetDriveEncoders();
        }
    }

    public class TurnAngle extends AutoCommand {

        private double angle;
        private double doneRange;

        public TurnAngle(String name, double angle, double tolerance, double doneCycles, double timeOut) {
            super(name, doneCycles, timeOut);

            this.angle = angle;
            this.doneRange = tolerance;

            drivebaseControlled = true;
            driveAngle = angle;
            driveDistance = 0.0;

            feedback.resetGyro();
        }

        @Override
        public boolean isDone() {
            if (Math.abs(feedback.getAngle() - angle) <= doneRange) {
                doneCycles++;
            }
            return doneCycles > minDoneCycles;
        }

        @Override
        public void stop() {
            drivebaseControlled = false;
            leftSpeed = 0.0;
            rightSpeed = 0.0;
            feedback.resetGyro();
        }
    }

    public class ToteStack extends AutoCommand {

        private boolean wentDown;

        public ToteStack(String name, double doneCycles, double timeOut) {
            super(name, doneCycles, timeOut);
            wentDown = false;
        }

        @Override
        public void run() {
            if (!wentDown) {
                elevatorPosition = Constants.FloorElevatorLevel1.getDouble();
                if (feedback.isElevatorDone()) {
                    wentDown = true;
                }
            } else {
                elevatorPosition = Constants.FloorElevatorLevel2.getDouble();
                numTotes++;
            }
        }

        @Override
        public boolean isDone() {
            if (elevatorPosition == Constants.FloorElevatorLevel2.getDouble() && feedback.isElevatorDone() && wentDown) {
                doneCycles++;
            }
            return doneCycles > minDoneCycles;
        }
    }

    protected void runCommand(AutoCommand command) {
        command.reset();
        while (!command.isDone() && command.isTimedOut()) {
            command.run();
            wait(0.01);
        }
        command.stop();
    }

    protected void wait(double time) {
        try {
            Thread.sleep((long) (time * 1000));
        } catch (InterruptedException ex) {
        }
    }
}

@@ -40,16 +40,52 @@
 .auto;%0A%0A
+import edu.wpi.first.wpilibj.Timer;%0A
 import o
@@ -3368,24 +3368,144 @@
 ble time) %7B%0A
+        double startTime = Timer.getFPGATimestamp();%0A%0A        while (Timer.getFPGATimestamp() %3C startTime + time) %7B%0A    
         try 
@@ -3518,16 +3518,20 @@
         
+    
 Thread.s
@@ -3539,31 +3539,17 @@
 eep(
-(long) (time * 1000));%0A
+10);%0A    
     
@@ -3584,24 +3584,38 @@
 ption ex) %7B%0A
+            %7D%0A
         %7D%0A  

Use fpga time to wait in auto