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 SHOOTER_WARMUP_DELAY = 4;
        final double PISTON_SET_DELAY = 1;
        final double BALL_LOAD_DELAY = 2.5;
        //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] = shooterVoltageEstimateStep(shooterSpeed, SHOOTER_WARMUP_DELAY);
        //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 warmupTime){
        final double ITER_1_FRACTION = 0.75;
        AutonomousStep[] warmupSteps  = new AutonomousStep[4];
        warmupSteps[0] = new ShooterVoltagePWMStep(targetVelocity, robot);
        warmupSteps[1] = new IdleWaitStep(warmupTime*ITER_1_FRACTION, robot);
        warmupSteps[2] = new ShooterVoltagePWMStep(targetVelocity, robot);
        warmupSteps[3] = new IdleWaitStep(warmupTime*(1-ITER_1_FRACTION),robot);
        return new SequentialAutonomousStepGroup(warmupSteps);
    }
}

@@ -376,55 +376,8 @@
 s)%7B%0A
-        final double SHOOTER_WARMUP_DELAY = 4;%0A
     
@@ -415,16 +415,16 @@
 AY = 1;%0A
+
         
@@ -1169,136 +1169,12 @@
 ed, 
-SHOOTER_WARMUP_DELAY);%0A        //Wait for the shooter to warm up%0A        steps%5B1%5D = new IdleWaitStep(SHOOTER_WARMUP_DELAY, robot
+3, 3
 );%0A 
@@ -1545,26 +1545,53 @@
  double 
-warmupTime
+estimate1Delay, double estimate2Delay
 )%7B%0A     
@@ -1814,34 +1814,22 @@
 tep(
-warmupTime*ITER_1_FRACTION
+estimate1Delay
 , ro
@@ -1955,38 +1955,22 @@
 tep(
-warmupTime*(1-ITER_1_FRACTION)
+estimate2Delay
 ,rob

Fix doubled wait time in Autonomous.