/*
 * Copyright (c) 2017 Titan Robotics Club (http://www.titanrobotics.com)
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

package team492;

import java.util.Date;

import frclib.FrcChoiceMenu;
import frclib.FrcValueMenu;
import hallib.HalDashboard;
import trclib.TrcRobot;

public class FrcAuto implements TrcRobot.RobotMode
{
    private static final boolean USE_TRACELOG = false;

    public static enum AutoStrategy
    {
        X_TIMED_DRIVE,
        Y_TIMED_DRIVE,
        X_DISTANCE_DRIVE,
        Y_DISTANCE_DRIVE,
        TURN_DEGREES,
        DO_NOTHING
    }   //enum AutoStrategy

    private Robot robot;

    //
    // Menus.
    //
    private FrcChoiceMenu<FrcAuto.AutoStrategy> autoStrategyMenu;
    private FrcValueMenu delayMenu;

    private AutoStrategy autoStrategy;
    private double delay;

    private TrcRobot.RobotCommand autoCommand;

    public FrcAuto(Robot robot)
    {
        this.robot = robot;
        //
        // Create Autonomous Mode specific menus.
        //
        autoStrategyMenu = new FrcChoiceMenu<>("Autonomous Strategies");
        delayMenu = new FrcValueMenu("Delay", 0.0);
        //
        // Populate Autonomous Mode menus.
        //
        autoStrategyMenu.addChoice("X Timed Drive", FrcAuto.AutoStrategy.X_TIMED_DRIVE);
        autoStrategyMenu.addChoice("Y Timed Drive", FrcAuto.AutoStrategy.Y_TIMED_DRIVE);
        autoStrategyMenu.addChoice("X Distance Drive", FrcAuto.AutoStrategy.X_DISTANCE_DRIVE);
        autoStrategyMenu.addChoice("Y Distance Drive", FrcAuto.AutoStrategy.Y_DISTANCE_DRIVE);
        autoStrategyMenu.addChoice("Turn Degrees", FrcAuto.AutoStrategy.TURN_DEGREES);
        autoStrategyMenu.addChoice("Do Nothing", FrcAuto.AutoStrategy.DO_NOTHING);
    }   //FrcAuto

    //
    // Implements TrcRobot.RunMode.
    //

    @Override
    public void startMode()
    {
        HalDashboard.getInstance().clearDisplay();
        if (USE_TRACELOG) robot.startTraceLog();
        Date now = new Date();
        robot.tracer.traceInfo(Robot.programName, "%s: ***** Starting autonomous *****", now.toString());

        //
        // Retrieve menu choice values.
        //
        autoStrategy = autoStrategyMenu.getCurrentChoiceObject();
        delay = delayMenu.getCurrentValue();

        switch (autoStrategy)
        {
            case X_TIMED_DRIVE:
                autoCommand = new CmdTimedDrive(robot, delay, robot.driveTime, robot.drivePower, 0.0, 0.0);
                break;

            case Y_TIMED_DRIVE:
                autoCommand = new CmdTimedDrive(robot, delay, robot.driveTime, 0.0, robot.drivePower, 0.0);
                break;

            case X_DISTANCE_DRIVE:
                autoCommand = new CmdPidDrive(robot, delay, robot.driveDistance, 0.0, 0.0);
                break;

            case Y_DISTANCE_DRIVE:
                autoCommand = new CmdPidDrive(robot, delay, 0.0, robot.driveDistance, 0.0);
                break;

            case TURN_DEGREES:
                autoCommand = new CmdPidDrive(robot, delay, 0.0, 0.0, robot.turnDegrees);
                break;

            default:
            case DO_NOTHING:
                autoCommand = null;
                break;
        }

        robot.driveBase.resetPosition();
        robot.targetHeading = 0.0;
    }   //startMode

    @Override
    public void stopMode()
    {
        robot.driveBase.stop();
        if (USE_TRACELOG) robot.tracer.closeTraceLog();
    }   //stopMode

    @Override
    public void runPeriodic(double elapsedTime)
    {
    }   //runPeriodic

    @Override
    public void runContinuous(double elapsedTime)
    {
        if (autoCommand != null)
        {
            autoCommand.cmdPeriodic(elapsedTime);
        }
    }   //runContinuous

}   //class FrcAuto

@@ -1448,24 +1448,90 @@
 ategy%0A    %7B%0A
+    %09LEFT_GEAR_LIFT,%0A    %09RIGHT_GEAR_LIFT,%0A    %09MIDDLE_GEAR_LIFT,%0A
         X_TI
@@ -1533,32 +1533,32 @@
  X_TIMED_DRIVE,%0A
-
         Y_TIMED_
@@ -2277,32 +2277,311 @@
 nus.%0A        //%0A
+        autoStrategyMenu.addChoice(%22Left Gear Lift%22, FrcAuto.AutoStrategy.LEFT_GEAR_LIFT);%0A        autoStrategyMenu.addChoice(%22Right Gear Lift%22, FrcAuto.AutoStrategy.RIGHT_GEAR_LIFT);%0A        autoStrategyMenu.addChoice(%22Middle Gear Lift%22, FrcAuto.AutoStrategy.MIDDLE_GEAR_LIFT);%0A
         autoStra
@@ -3662,16 +3662,16 @@
 rategy)%0A
-
         
@@ -3664,32 +3664,398 @@
 tegy)%0A        %7B%0A
+        %09case LEFT_GEAR_LIFT:%0A        %09%09autoCommand = new CmdSideGearLift(robot, delay, false);%0A        %09%09break;%0A        %09%09%0A        %09case RIGHT_GEAR_LIFT:%0A        %09%09autoCommand = new CmdSideGearLift(robot, delay, true);%0A        %09%09break;%0A        %09%09%0A        %09case MIDDLE_GEAR_LIFT:%0A        %09%09autoCommand = new CmdMidGearLift(robot, delay);%0A        %09%09break;%0A        %09%09%0A
             case

Add left, right & middle gear lift to AutoStrategy