package com.team254.frc2013.auto;

import com.team254.frc2013.commands.DriveAtSpeedCommand;
import com.team254.frc2013.commands.DriveProfiledCommand;
import com.team254.frc2013.commands.IntakeRaiseCommand;
import com.team254.frc2013.commands.ResetDriveEncodersCommand;
import com.team254.frc2013.commands.ResetGyroCommand;
import com.team254.frc2013.commands.RunIntakeCommand;
import com.team254.frc2013.commands.ShiftCommand;
import com.team254.frc2013.commands.ShootSequenceCommand;
import com.team254.frc2013.commands.ShooterOnCommand;
import com.team254.frc2013.commands.ShooterPresetCommand;
import com.team254.frc2013.commands.WaitCommand;
import com.team254.frc2013.subsystems.Shooter;
import com.team254.lib.control.impl.CustomProfile;
import edu.wpi.first.wpilibj.command.CommandGroup;

/**
 * Scores three starting discs from the back of the pyramid, picks up four more, then scores them
 * from the front of the pyramid.

 * @author tom@team254.com (Tom Bottiglieri)
 * @author pat@team254.com (Patrick Fairbank)
 */
public class SevenDiscAutoMode extends CommandGroup {
  CustomProfile profile = new CustomProfile();

  public SevenDiscAutoMode() {
    // Shoot first discs
    addSequential(new ShooterOnCommand(true));
    addSequential(new ShooterPresetCommand(Shooter.PRESET_BACK_PYRAMID));
    addSequential(new IntakeRaiseCommand(IntakeRaiseCommand.INTAKE_DOWN));
    addSequential(new ShiftCommand(false));
  //  addSequential(new ShootSequenceCommand());
  //  addSequential(new ShootSequenceCommand());
    //addSequential(new ShootSequenceCommand(false));
addSequential(new WaitCommand(1));
    // Pick up 2 middle discs
    addSequential(new ResetDriveEncodersCommand());
    addSequential(new ResetGyroCommand());
    addSequential(new RunIntakeCommand(1));
    addSequential(new ShooterPresetCommand(Shooter.PRESET_FRONT_PYRAMID));

    // Drive to front of pyramid and shoot 2
    addSequential(new DriveProfiledCommand(6, 3.5, 0, 5));
    addSequential(new RunIntakeCommand(0.0));
   // addSequential(new ShootSequenceCommand());
    //addSequential(new ShootSequenceCommand(false));
    addSequential(new WaitCommand(1));

    // Pick up 2 far discs
    addSequential(new RunIntakeCommand(1.0));
    addSequential(new DriveAtSpeedCommand(10.5, 2.0, 0, 4.6));

    // Drive to front of pyramid and shoot 2
    addSequential(new DriveProfiledCommand(6, 5, 0, 2.5));
    addSequential(new RunIntakeCommand(0.0));
   // addSequential(new ShootSequenceCommand());
   // addSequential(new ShootSequenceCommand(false));
addSequential(new WaitCommand(1));
    addSequential(new ShooterOnCommand(false));
    addSequential(new RunIntakeCommand(0.0));
  }
}

@@ -1417,26 +1417,24 @@
 (false));%0A  
-//
   addSequent
@@ -1468,18 +1468,16 @@
 d());%0A  
-//
   addSeq
@@ -1509,34 +1509,32 @@
 Command());%0A    
-//
 addSequential(ne
@@ -1569,42 +1569,8 @@
 ));%0A
-addSequential(new WaitCommand(1));
 %0A   
@@ -1956,34 +1956,32 @@
 mmand(0.0));%0A   
-//
  addSequential(n
@@ -2012,18 +2012,16 @@
 ));%0A    
-//
 addSeque
@@ -2063,47 +2063,8 @@
 e));
-%0A    addSequential(new WaitCommand(1));
 %0A%0A  
@@ -2347,26 +2347,24 @@
 d(0.0));%0A   
-//
  addSequenti
@@ -2398,18 +2398,16 @@
 ());%0A   
-//
  addSequ
@@ -2438,25 +2438,24 @@
 Command(
-false
 ));%0A
+    
 addSeque
@@ -2468,20 +2468,29 @@
 new 
-Wait
+ShootSequence
 Command(
 1));
@@ -2477,37 +2477,42 @@
 SequenceCommand(
-1
+false
 ));%0A
+%0A
     addSequentia

Return 7 disc to its former goodness