/* Copyright (c) 2014, FRC3161
* All rights reserved.
* 
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* 
* * Redistributions of source code must retain the above copyright notice, this
*   list of conditions and the following disclaimer.
* 
* * Redistributions in binary form must reproduce the above copyright notice, this
*   list of conditions and the following disclaimer in the documentation and/or
*   other materials provided with the distribution.
* 
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
* ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package ca.team3161.iapetus;

import ca.team3161.lib.robot.ThreadedAutoRobot;
import ca.team3161.lib.utils.controls.LogitechDualAction;
import ca.team3161.lib.robot.Drivetrain;
import ca.team3161.lib.utils.Utils;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Talon;
import ca.team3161.lib.utils.io.DriverStationLCD;
import edu.wpi.first.wpilibj.Timer;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Iapetus extends ThreadedAutoRobot {

    private final SpeedController leftDrive = new Drivetrain (new SpeedController[] {new Talon (1), new Victor (2), new Talon (3)}).setInverted(true);
    private final SpeedController rightDrive = new Drivetrain (new SpeedController[] {new Talon (4), new Victor (5), new Talon (6)}).setInverted(false);
    
    private final LogitechDualAction gamepad = new LogitechDualAction (Constants.Gamepad.PORT, Constants.Gamepad.DEADZONE);
    private final DriverStationLCD dsLcd = DriverStationLCD.getInstance();
    
    private final Timer autoElapsedTimer = new Timer();

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        gamepad.setInverted(true);
        dsLcd.println(0, "Robot Init!");
    }
    
    /**
     * This method is invoked by a new Thread.
     * The main robot thread's role during autonomous is simply to maintain
     * core robot functionality, eg feeding the Watchdog and responding to
     * FMS events. A new Thread handles autonomous behaviour so that we can
     * use Thread.sleep() rather than checking Timer values.
     * This method's semantics are one-shot, as opposed to continuous looping
     * as provided by autonomousPeriodic or autonomousContinuous. This allows
     * simpler scripting and also avoids busy-wait conditions.
     * @throws Exception 
     */
    public void autonomousThreaded() throws Exception {
        SpeedController allDrive = new Drivetrain(new SpeedController[] {leftDrive, rightDrive});
        dsLcd.clear();
        dsLcd.println(1, "AUTO: DRIVE 0.5 0.5");
        allDrive.set(0.5);
        waitFor(1000);
        dsLcd.println(1, "AUTO: DRIVE 0.0 0.0");
        allDrive.set(0.0);
        dsLcd.println(5, "AUTO: FINISHED");
    }
    
    /**
     * Put anything here that needs to be performed by the main robot thread
     * during the autonomous period. DO NOT use any Drivetrain, Gamepad,
     * Solenoid, etc. fields in here - these are reserved for use ONLY
     * within autonomousThreaded()!
     */
    public void autonomousPeriodic() {
        autoElapsedTimer.start(); // if it's already started, nothing happens
        dsLcd.println(0, "Auto elapsed: " + Utils.round(autoElapsedTimer.get(), 2) + "s");
    }

    /**
     * Runs through once at the start of teleop
     */
    public void teleopInit() {
    }

    /**
     * This function is called periodically during operator control, just like
     * teleopPeriodic in the IterativeRobot base class.
     * We use teleopThreadsafe instead to ensure safety with our separate
     * autonomous thread. DO NOT create a teleopPeriodic in this class or any
     * subclasses! Use teleopThreadsafe instead, only!
     */
    public void teleopThreadsafe() {
        dsLcd.clear();
        dsLcd.println(0, "Teleop running");
        dsLcd.println(1, "Left Drive: " + leftDrive.get());
        dsLcd.println(2, "Right Drive: " + rightDrive.get());        

        leftDrive.set(gamepad.getLeftY() + gamepad.getRightX());
        rightDrive.set(gamepad.getLeftY() - gamepad.getRightX());
    }

    /**
     * Called once when the robot enters the disabled state
     */
    public void disabledInit() {
        leftDrive.disable();
        rightDrive.disable();
    }

    /**
     * Called periodically while the robot is in the disabled state
     */
    public void disabledPeriodic() {
        leftDrive.disable();
        rightDrive.disable();
    }

    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
    }

}

@@ -2245,16 +2245,61 @@
 onLCD;%0D%0A
+import edu.wpi.first.wpilibj.DriverStation;%0D%0A
 import e
@@ -3291,16 +3291,62 @@
 imer();%0D
+%0A    private DriverStation.Alliance alliance;%0D
 %0A%0D%0A    /
@@ -3501,32 +3501,95 @@
  robotInit() %7B%0D%0A
+        alliance = DriverStation.getInstance().getAlliance();%0D%0A
         gamepad.
@@ -3645,17 +3645,44 @@
 ot Init!
-%22
+ Alliance: %22 + alliance.name
 );%0D%0A    

Store current alliance info