package com.edinarobotics.zephyr.autonomous;

import com.edinarobotics.utils.autonomous.*;
import com.edinarobotics.zephyr.Zephyr;

/**
 *
 */
public class AutonomousStepFactory {
    private Zephyr robot;
    
    public AutonomousStepFactory(Zephyr robot){
        this.robot = robot;
    }
    
    public AutonomousStep getShooterFireStep(double shooterSpeed, int numShots){
        final double PISTON_SET_DELAY = 1;
        final double BALL_LOAD_DELAY = 2.5;
        final double SHOOTER_WARMUP_DELAY = 3;
        //Steps to bring the piston up and down
        AutonomousStep[] fireSteps = new AutonomousStep[7];
        fireSteps[0] = new ShooterPistonControlStep(true, robot);
        fireSteps[1] = new IdleWaitStep(PISTON_SET_DELAY, robot);
        fireSteps[2] = new ShooterPistonControlStep(false, robot);
        fireSteps[3] = new IdleWaitStep(PISTON_SET_DELAY, robot);
        fireSteps[4] = new SetConveyorStep(true, robot);
        fireSteps[5] = new IdleWaitStep(BALL_LOAD_DELAY, robot);
        fireSteps[6] = new SetConveyorStep(false, robot);
        
        AutonomousStep[] steps = new AutonomousStep[4];
        //Start the shooter
        steps[0] = new SetShooterSpeedStep(shooterSpeed, robot);
        //Wait for the shooter to warm up
        steps[1] = new IdleWaitStep(SHOOTER_WARMUP_DELAY, robot);
        //Fire the piston several times
        steps[2] = new ForLoopStep(new SequentialAutonomousStepGroup(fireSteps), numShots);
        //Stop the shooter
        steps[3] = new SetShooterSpeedStep(0, robot);
        
        return new SequentialAutonomousStepGroup(steps);
    }
    
    public AutonomousStep shooterVoltageEstimateStep(double targetVelocity, double estimate1Delay, double estimate2Delay){
        final double ITER_1_FRACTION = 0.75;
        AutonomousStep[] warmupSteps  = new AutonomousStep[4];
        warmupSteps[0] = new ShooterVoltagePWMStep(targetVelocity, robot);
        warmupSteps[1] = new IdleWaitStep(estimate1Delay, robot);
        warmupSteps[2] = new ShooterVoltagePWMStep(targetVelocity, robot);
        warmupSteps[3] = new IdleWaitStep(estimate2Delay,robot);
        return new SequentialAutonomousStepGroup(warmupSteps);
    }
}

@@ -503,17 +503,17 @@
 DELAY = 
-3
+4
 ;%0A      
@@ -610,17 +610,17 @@
 ousStep%5B
-7
+8
 %5D;%0A     
@@ -694,32 +694,81 @@
   fireSteps%5B1%5D =
+ new PrintStep(%22Firing!%22);%0A        fireSteps%5B2%5D =
  new IdleWaitSte
@@ -805,33 +805,33 @@
       fireSteps%5B
-2
+3
 %5D = new ShooterP
@@ -872,33 +872,33 @@
       fireSteps%5B
-3
+4
 %5D = new IdleWait
@@ -938,33 +938,33 @@
       fireSteps%5B
-4
+5
 %5D = new SetConve
@@ -995,33 +995,33 @@
       fireSteps%5B
-5
+6
 %5D = new IdleWait
@@ -1060,33 +1060,33 @@
       fireSteps%5B
-6
+7
 %5D = new SetConve
@@ -1161,36 +1161,97 @@
  AutonomousStep%5B
-4
+5
 %5D;%0A
+        steps%5B0%5D = new PrintStep(%22Warming the shooter up!%22);%0A
         //Start 
@@ -1268,33 +1268,33 @@
 r%0A        steps%5B
-0
+1
 %5D = new SetShoot
@@ -1375,33 +1375,33 @@
 p%0A        steps%5B
-1
+2
 %5D = new IdleWait
@@ -1481,33 +1481,33 @@
 s%0A        steps%5B
-2
+3
 %5D = new ForLoopS
@@ -1600,33 +1600,33 @@
 r%0A        steps%5B
-3
+4
 %5D = new SetShoot

Add debugging messages to autonomous.