package edu.stuy.commands.auton;

import static edu.stuy.PhysicalConstants.TOTE_WIDTH;
import static edu.stuy.RobotMap.AUTON_DRIVETRAIN_TOTE_WIDTH_TIMEOUT;
import static edu.stuy.RobotMap.AUTON_DRIVE_FORWARD_DRIVER_SIDE_INCHES;
import static edu.stuy.RobotMap.AUTON_DRIVE_FORWARD_DRIVER_SIDE_TIMEOUT;
import edu.stuy.commands.DrivetrainRotateCommand;
import edu.stuy.commands.LiftUpInchesCommand;
import edu.wpi.first.wpilibj.command.CommandGroup;

/**
 *
 */
public class AutonLiftContainerHerdTote extends CommandGroup {

    public  AutonLiftContainerHerdTote() {
        addSequential(new LiftUpInchesCommand(15.0));
        addParallel(new AutonAcquirerAcquireCommand());
        addSequential(new AutonDriveForwardInchesCommand(TOTE_WIDTH , AUTON_DRIVETRAIN_TOTE_WIDTH_TIMEOUT));
        addSequential(new AutonAcquirerOffCommand());
        // Tote is on the right from the driver's point of view, so we turn 90 degrees
        addSequential(new DrivetrainRotateCommand(-90) , 2.0);
        // Go to auton zone
        addSequential(new AutonDriveForwardInchesCommand(AUTON_DRIVE_FORWARD_DRIVER_SIDE_INCHES, AUTON_DRIVE_FORWARD_DRIVER_SIDE_TIMEOUT));
    }
}

@@ -27,16 +27,70 @@
 auton;%0A%0A
+import static edu.stuy.PhysicalConstants.TOTE_HEIGHT;%0A
 import s
@@ -134,16 +134,16 @@
 _WIDTH;%0A
-
 import s
@@ -502,16 +502,255 @@
 %0A%0A/**%0A *
+ The robot starts collinear with the auton set, with the hook in the can.%0A * 1) Lift up the can.%0A * 2) Drive forward and acquire the tote.%0A * 3) Turn 90%C2%B0 to the left.%0A * 4) Drive forward to the auton zone.%0A * 5) Drop the can onto the tote.
 %0A */%0Apub
@@ -853,16 +853,60 @@
 ote() %7B%0A
+        // TODO: factor out TOTE_HEIGHT + 3%0A
         
@@ -947,12 +947,23 @@
 and(
-15.0
+TOTE_HEIGHT + 3
 ));%0A
@@ -1359,16 +1359,16 @@
 on zone%0A
-
         
@@ -1495,20 +1495,166 @@
 _TIMEOUT));%0A
+        // FIXME: lower the can%0A        // TODO: factor out TOTE_HEIGHT + 3%0A        // addSequential(new LiftDownInchesCommand(TOTE_HEIGHT + 3));%0A
     %7D%0A%7D%0A

Fix up AutonLiftContainerHerdTote and add important TODOs