package org.lunatecs316.frc2013.auto;

import org.lunatecs316.frc2013.subsystems.*;
import edu.wpi.first.wpilibj.DriverStationLCD;

/**
 * Three disk autonomous mode
 * @author domenicpaul
 */
public class ThreeDiskAuto implements AutonomousMode {
    
    /**
     * Represents the various states of the robot during the autonomous mode
     */
    private static class State {
        
        /**
         * Start of the Autonomous Mode
         */
        public static final State kStart = new State("Start");
        
        /**
         * Firing a shot
         */
        public static final State kFiring = new State("Firing");
        
        /**
         * Preparing to fire the next shot
         */
        public static final State kPreparingNextShot = new State("PreparingNextShot");
        
        /**
         * Finished the Autonomous Mode
         */
        public static final State kFinished = new State("Finished");
                
        private String name;
        
        // Private to prevent creation outside the class
        private State(String name) {
            this.name = name;
        }
        
        /**
         * Convert the State to a string
         * @return the string value
         */
        public String toString() {
            return name;
        }
    }
    
    private State state = State.kStart;
    private int shotsFired = 0;
    private long startTime;
    private boolean finished = false;
    
    private DriverStationLCD LCD = DriverStationLCD.getInstance();

    /**
     * Returns the time spent in the current state
     * @return the time spent in the current state
     */
    private double ellapsedStateTime() {
        return System.currentTimeMillis() - startTime;
    }
    
    /**
     * Sets the state for the next loop and resets startTime
     * @param state the new state
     */
    private void setState(State state) {
        this.state = state;
        startTime = System.currentTimeMillis();
    }
    
    /**
     * Run any setup. Called from autonomousInit()
     */
    public void init() {
        startTime = System.currentTimeMillis();
        state = State.kStart;
        shotsFired = 0;
        LCD.println(DriverStationLCD.Line.kUser1, 1,
                "[ThreeDiskAuto][init] Autonomous Initialized");
        System.out.println("[ThreeDiskAuto][init] Autonomous Initialized");
    }
    
    /**
     * Run one iteration of the mode. Called from autonomousPeriodic()
     */
    public void run() {
        String output = "[ThreeDiskAuto][run] ";    // used for debugging
        
        if (!finished) {
            // Add the current state to debug output
            output += "State: " + state.toString() + "; ";
            
            // Switch through the states
            if (state == State.kStart) {
                // Enable the shooter and wait for it to come up to speed
                Shooter.enable();

                if (Shooter.atSpeed() || ellapsedStateTime() >= 2.0) {
                    setState(State.kFiring);
                }
            } else if (state == State.kFiring) {
                // Fire a shot
                Shooter.fire(true);

                // Wait 200ms
                if (ellapsedStateTime() >= 0.2) {
                    shotsFired++;
                    if (shotsFired >= 5) {
                        setState(State.kFinished);
                    } else {
                        setState(State.kPreparingNextShot);
                    }
                }
            } else if (state == State.kPreparingNextShot) {
                // Reset and wait for the shooter to come back up to speed
                Shooter.fire(false);

                if (ellapsedStateTime() >= 0.5 &&
                        (Shooter.atSpeed() || ellapsedStateTime() >= 2.0)) {
                    setState(State.kFiring);
                }
            } else if (state == State.kFinished) {
                // Turn off the shooter
                Shooter.disable();
                finished = true;
            }
        } else {
            // We're done
            output += "Finished";
        }
        
        // Print debugging info
        LCD.println(DriverStationLCD.Line.kUser1, 1, output);
        System.out.println(output);
    }
}

@@ -80,55 +80,8 @@
 s.*;
-%0Aimport edu.wpi.first.wpilibj.DriverStationLCD;
 %0A%0A/*
@@ -195,24 +195,90 @@
 Mode %7B%0A    %0A
+    // %3Ceditor-fold desc=%22State 'Enum'%22 defaultstate=%22collapsed%22%3E%0A
     /**%0A    
@@ -1326,24 +1326,46 @@
     %7D%0A    %7D%0A
+    // %3C/editor-fold%3E%0A
     %0A    pri
@@ -1504,76 +1504,8 @@
    %0A
-    private DriverStationLCD LCD = DriverStationLCD.getInstance();%0A%0A
     
@@ -2162,126 +2162,8 @@
  0;%0A
-        LCD.println(DriverStationLCD.Line.kUser1, 1,%0A                %22%5BThreeDiskAuto%5D%5Binit%5D Autonomous Initialized%22);%0A
     
@@ -3992,32 +3992,32 @@
      %7D%0A        %0A
+
         // Print
@@ -4036,70 +4036,8 @@
 nfo%0A
-        LCD.println(DriverStationLCD.Line.kUser1, 1, output);%0A
     

Remove reference to LCD from ThreeDiskAuto