
package org.usfirst.frc.team997.robot;

import org.usfirst.frc.team997.robot.commands.AutoDriveBackwards;
import org.usfirst.frc.team997.robot.commands.AutoDriveForward;
import org.usfirst.frc.team997.robot.commands.DriveToSetpoint;
import org.usfirst.frc.team997.robot.commands.NullCommand;
import org.usfirst.frc.team997.robot.subsystems.DriveTrain;
import org.usfirst.frc.team997.robot.subsystems.Gatherer;
import org.usfirst.frc.team997.robot.subsystems.GathererArm;
//import org.usfirst.frc.team997.robot.subsystems.GathererArmNoSP;
import org.usfirst.frc.team997.robot.subsystems.Shooter;
import org.usfirst.frc.team997.robot.subsystems.ShooterPivot;

import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {
	public static Relay clight;
	private CameraServer camera;
	public static final Shooter shooter = new Shooter(
			RobotMap.shooterBallSensor, RobotMap.shooterMotorPort,
			RobotMap.servoMotorFirstPort, RobotMap.servoMotorSecondPort);
	public static final ShooterPivot shooterPivot = new ShooterPivot(
			RobotMap.aimingMotorPort, RobotMap.shooterPivotAnglePort);
	public static final DriveTrain driveTrain = 
			new DriveTrain(RobotMap.leftMotorPort, RobotMap.rightMotorPort,
			               RobotMap.leftEncoderFirstPort, RobotMap.leftEncoderSecondPort, 
			               RobotMap.rightEncoderFirstPort, RobotMap.rightEncoderSecondPort);
	public static final Gatherer gatherer = new Gatherer(RobotMap.rollerMotorPort);
	public static final GathererArm gathererArm = 
			new GathererArm(RobotMap.gatherArmMotorPort, RobotMap.gathererArmAnglePort);
	//public static final GathererArmNoSP gathererArm = new GathererArmNoSP(RobotMap.gatherArmMotorPort);
	
	public static ADXRS450_Gyro gyro;

	public static OI oi;

	private Command autonomousCommand;
//	private SendableChooser chooser;

	//public static Compressor compressor;
	public static PowerDistributionPanel pdp;

	public void robotInit() {
		oi = new OI();

//		chooser = new SendableChooser();
//		chooser.addDefault("Forward", new AutoDriveForward());
//		chooser.addObject("Backward", new AutoDriveBackwards());
//		chooser.addObject("Nothing", new NullCommand());

//		SmartDashboard.putData("Auto mode", chooser);
		
		gyro = new ADXRS450_Gyro();
		gyro.calibrate();
		
		pdp = new PowerDistributionPanel();

        camera = CameraServer.getInstance();
        camera.setQuality(42);
        camera.startAutomaticCapture("cam1");        
        //clight = new Relay(RobotMap.circleLightPort);

		// Need to reset the servo's position to be ready to capture a ball. Retracts kicker servos.
		Robot.shooter.retractKicker();
		
		autonomousCommand = new AutoDriveForward();
	}

	public void disabledInit() {

	}

	public void disabledPeriodic() {
		Scheduler.getInstance().run();
	}

	public void autonomousInit() {
		gyro.reset();
		//autonomousCommand = (Command) chooser.getSelected();

		/* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
		switch(autoSelected) {
		case "My Auto":
			autonomousCommand = new MyAutoCommand();
			break;
		case "Default Auto":
		default:
			autonomousCommand = new ExampleCommand();
			break;
		} */

		// schedule the autonomous command (example)
//		if (autonomousCommand != null) autonomousCommand.start();
		getSelected().start();
	}
	
	private Command getSelected() { return autonomousCommand; }
	//private Command getSelected() { return (Command) chooser.getSelected(); }

	public void autonomousPeriodic() {
		Scheduler.getInstance().run();
	}

	public void teleopInit() {
		Robot.gathererArm.lockArmPosition();
//		if (autonomousCommand != null) autonomousCommand.cancel();
		//getSelected().start();
	}

	public void teleopPeriodic() {
		Scheduler.getInstance().run();
		smartDashboard();
	}

	public void testPeriodic() {
		LiveWindow.run();
	}

	public void smartDashboard(){
		Robot.driveTrain.smartDashboard();
		Robot.shooter.smartDashboard();
		Robot.gathererArm.smartDashboard();
		Robot.shooterPivot.smartDashboard();
		
		//imu info put on the smartdashboard
		SmartDashboard.putNumber("Gyro Angle", Robot.gyro.getAngle());
		SmartDashboard.putNumber("Gyro Rate", Robot.gyro.getRate());
	}

}

@@ -2564,18 +2564,16 @@
 ommand;%0A
-//
 %09private
@@ -2728,18 +2728,16 @@
  OI();%0A%0A
-//
 %09%09choose
@@ -2763,18 +2763,16 @@
 oser();%0A
-//
 %09%09choose
@@ -2816,26 +2816,24 @@
 Forward());%0A
-//
 %09%09chooser.ad
@@ -2878,19 +2878,40 @@
 ards());
-%0A
+ 
 //
+use this for low bar%0A
 %09%09choose
@@ -2954,18 +2954,16 @@
 nd());%0A%0A
-//
 %09%09SmartD
@@ -4091,16 +4091,18 @@
 );%0A%09%7D%0A%09%0A
+//
 %09private
@@ -4155,18 +4155,16 @@
 and; %7D%0A%09
-//
 private 

Use autonomous chooser