/*----------------------------------------------------------------------------*/
/* 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 com.edinarobotics.zed;


import com.edinarobotics.utils.commands.MaintainStateCommand;
import com.edinarobotics.utils.commands.RepeatCommand;
import com.edinarobotics.utils.gamepad.Gamepad;
import com.edinarobotics.utils.pid.PIDTuningManager;
import com.edinarobotics.zed.commands.AugerRotateCommand;
import com.edinarobotics.zed.commands.GamepadDriveRotationCommand;
import com.edinarobotics.zed.commands.GamepadDriveStrafeCommand;
import com.edinarobotics.zed.commands.SetConveyorCommand;
import com.edinarobotics.zed.commands.SetShooterCommand;
import com.edinarobotics.zed.commands.VisionTrackingCommand;
import com.edinarobotics.zed.subsystems.Auger;
import com.edinarobotics.zed.subsystems.Collector;
import com.edinarobotics.zed.subsystems.Conveyor;
import com.edinarobotics.zed.subsystems.DrivetrainRotation;
import com.edinarobotics.zed.subsystems.DrivetrainStrafe;
import com.edinarobotics.zed.subsystems.Lifter;
import com.edinarobotics.zed.subsystems.Shooter;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.PrintCommand;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.command.WaitCommand;
import edu.wpi.first.wpilibj.command.WaitForChildren;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;

public class Zed extends IterativeRobot {
    private Command autonomousCommand;
    
    /**
     * This function is called when the robot switches between modes (eg. Autonomous,
     * Teleop) to reset running subsystems.
     */
    public void betweenModes() {
        stop();
        if(autonomousCommand != null) {
            autonomousCommand.cancel();
        }
    }
    
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        Components.getInstance(); //Create all robot subsystems.
        System.out.println("Robot initialized.");
    }
    
    public void disabledInit() {
        betweenModes();
    }
    
    public void disabledPeriodic() {
        stop();
    }
    
    /**
     * This function is called once at the start of autonomous mode.
     */
    public void autonomousInit(){
        DriverStation driverStation = DriverStation.getInstance();
        double delayTime = driverStation.getAnalogIn(1);
        
        betweenModes();
        DrivetrainStrafe drivetrainStrafe = Components.getInstance().drivetrainStrafe;
        drivetrainStrafe.setDefaultCommand(new MaintainStateCommand(drivetrainStrafe));
        DrivetrainRotation drivetrainRotation = Components.getInstance().drivetrainRotation;
        drivetrainRotation.setDefaultCommand(new MaintainStateCommand(drivetrainRotation));
        
        CommandGroup augerSequence = new CommandGroup();
        augerSequence.addSequential(new PrintCommand("Dispensing auger"));
        augerSequence.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        augerSequence.addSequential(new WaitCommand(2));
        
        CommandGroup autoCommand = new CommandGroup();
        autoCommand.addSequential(new PrintCommand("Starting autonomous"));
        autoCommand.addSequential(new WaitCommand(delayTime));
        autoCommand.addSequential(new VisionTrackingCommand(VisionTrackingCommand.ANY_GOAL), 1.5);
        autoCommand.addSequential(new SetShooterCommand(Shooter.SHOOTER_ON));
        autoCommand.addSequential(new SetConveyorCommand(Conveyor.CONVEYOR_SHOOT_IN));
        autoCommand.addSequential(new WaitCommand(1));
        autoCommand.addParallel(new RepeatCommand(augerSequence, 4));
        autoCommand.addSequential(new WaitForChildren());
        autoCommand.addSequential(new WaitCommand(2));
        autoCommand.addSequential(new SetConveyorCommand(Conveyor.CONVEYOR_STOP));
        autoCommand.addSequential(new SetShooterCommand(Shooter.SHOOTER_OFF));
        
        autonomousCommand = autoCommand;
        autonomousCommand.start();
    }

    /**
     * This function is called periodically during autonomous.
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }
    
    /**
     * This function is called once at the start of teleop mode.
     */
    public void teleopInit(){
        betweenModes();
        Gamepad driveGamepad = Controls.getInstance().gamepad1;
        Gamepad shootGamepad = Controls.getInstance().gamepad2;
        Components.getInstance().drivetrainStrafe.setDefaultCommand(new GamepadDriveStrafeCommand(driveGamepad));
        Components.getInstance().drivetrainRotation.setDefaultCommand(new GamepadDriveRotationCommand(driveGamepad, shootGamepad));
    }

    /**
     * This function is called periodically during operator control.
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
    }
    
    /**
     * This function is called once at the start of test mode.
     */
    public void testInit() {
        betweenModes();
        LiveWindow.setEnabled(false);
        teleopInit();
    }
    
    /**
     * This function is called periodically during test mode.
     */
    public void testPeriodic() {
        teleopPeriodic();
        PIDTuningManager.getInstance().runTuning();
    }
    
    /**
     * This function is called to stop <i>each</i> subsystem.
     */
    public void stop() {
        Components.getInstance().auger.setAugerDirection(Auger.AugerDirection.AUGER_STOP);
        Components.getInstance().collector.setCollectorDirection(Collector.CollectorDirection.COLLECTOR_STOP);
        Components.getInstance().collector.setCollectorLiftDirection(Collector.CollectorLiftDirection.COLLECTOR_LIFT_STOP);
        Components.getInstance().conveyor.setConveyorSpeed(0);
        Components.getInstance().drivetrainRotation.mecanumPolarRotate(0);
        Components.getInstance().drivetrainStrafe.mecanumPolarStrafe(0, 0);
        Components.getInstance().lifter.setLifterDirection(Lifter.LifterDirection.LIFTER_STOP);
        Components.getInstance().shooter.setShooterVelocity(0);
    }
    
}

@@ -2163,24 +2163,384 @@
 enModes() %7B%0A
+        DrivetrainStrafe drivetrainStrafe = Components.getInstance().drivetrainStrafe;%0A        drivetrainStrafe.setDefaultCommand(new MaintainStateCommand(drivetrainStrafe));%0A        DrivetrainRotation drivetrainRotation = Components.getInstance().drivetrainRotation;%0A        drivetrainRotation.setDefaultCommand(new MaintainStateCommand(drivetrainRotation));%0A
         stop

Add code to set default commands of drivetrain to betweenModes().