// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.

package org.usfirst.frc3824.BetaBot.commands;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc3824.BetaBot.Robot;

/**
 *
 */
public class JiggleBoulder extends Command
{

	// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS

	// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS

	private Timer m_Timer;
	
	// Setup the Jiggle Boulder "state" machine
	//
	// Time:  Outtake   Stop     Intake   
	//        _________|_____|______________|
	//                0.4  0.6             1.0
	//            0.4    0.1       0.5
	//
	private double outtakeTimeOn       = 0.4;
	private double stopTransistionTime = 0.5;
	private double intakeTimeOn        = 1.0;
	
	// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
	public JiggleBoulder()
	{

		// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
		// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING

		// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
		// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        requires(Robot.shooter);

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
	}

	// Called just before this Command runs the first time
	protected void initialize()
	{
		// Create an instance of the Timer
		m_Timer = new Timer();
		
		// Reset and start the timer
		m_Timer.reset();
		m_Timer.start();
		
		// Start the rear shooter wheels out with the front shooter wheels in to hold the boulder 
		Robot.shooter.ShooterRearWheelControl(0.8);
		Robot.shooter.ShooterFrontWheelControl(-0.8);
	}

	// Called repeatedly when this Command is scheduled to run
	protected void execute()
	{
		double presentTimer = m_Timer.get();
		
		// Check the state
		if (presentTimer > stopTransistionTime)
			Robot.shooter.ShooterRearWheelControl(-1.0);
		else if (presentTimer > outtakeTimeOn)
			Robot.shooter.ShooterRearWheelControl(0.0);		
	}

	// Make this return true when this Command no longer needs to run execute()
	protected boolean isFinished()
	{
		// Determine if the routine is complete
		if (m_Timer.get() > intakeTimeOn)
			return true;
		
		return false;
	}

	// Called once after isFinished returns true
	protected void end()
	{
		// Stop the shooter wheels
		Robot.shooter.ShooterAllWheelControl(0);

		// Stop and reset the timer
		m_Timer.stop();
		m_Timer.reset();
	}

	// Called when another command which requires one or more of the same
	// subsystems is scheduled to run
	protected void interrupted()
	{
		this.end();
	}
}
