#include "WPILib.h"
#include "Commands/Command.h"
#include "CommandBase.h"
#include "Commands/RunCompressor.h"
#include "Commands/DriveTrain/ShiftHi.h"
#include "Commands/Autonomous/DriveStraight.h"
#include "Commands/Autonomous/AutoTurn.h"
#include "Commands/Autonomous/AutoDrivePivot.h"

class CommandBasedRobot : public IterativeRobot 
{
private:
	Command * autonomousCommand;
	LiveWindow * lw;
	RunCompressor * RC;
	SendableChooser * autoChooser;
	
	virtual void RobotInit() 
	{
		
		CommandBase::init();
		autonomousCommand = new AutoTurn(90,0.8);
		lw = LiveWindow::GetInstance();
		RC = new RunCompressor();
		RC->Start();
		autoChooser = new SendableChooser();
		autoChooser->AddDefault("Drive Straight",new DriveStraight(100, .45));
		autoChooser->AddObject("Turn 90",new AutoTurn(90,0.8));
		autoChooser->AddObject("Drive and Turn", new AutoDrivePivot());

		SmartDashboard::PutData("Autonomous Modes", autoChooser);
	}
	
	virtual void AutonomousInit() 
	{
		autonomousCommand = (Command * ) autoChooser->GetSelected();
		autonomousCommand->Start();
	}
	
	virtual void AutonomousPeriodic() 
	{
		Scheduler::GetInstance()->Run();
		RC->Start();
	}
	
	virtual void TeleopInit() 
	{
		// This makes sure that the autonomous stops running when
		// teleop starts running. If you want the autonomous to 
		// continue until interrupted by another command, remove
		// this line or comment it out.
		autonomousCommand->Cancel();
		RC->Start();
		CommandBase::driveTrain->ShiftHi();
	}
	
	virtual void TeleopPeriodic() 
	{
		Scheduler::GetInstance()->Run();
	}
	
	virtual void TestPeriodic()
	{
		lw->Run();
	}
};

START_ROBOT_CLASS(CommandBasedRobot);

