// 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.frc4388.robot;

//import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.DriverStation;
//import edu.wpi.first.wpilibj.PowerDistributionPanel;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

//import org.usfirst.frc4388.controller.Robot.OperationMode;
import org.usfirst.frc4388.robot.commands.*;
import org.usfirst.frc4388.robot.commands.auton.*;
import org.usfirst.frc4388.robot.OI;
import org.usfirst.frc4388.robot.subsystems.*;
import org.usfirst.frc4388.utility.ControlLooper;
import org.usfirst.frc4388.robot.subsystems.Drive;
import org.usfirst.frc4388.robot.subsystems.Carriage;
import org.usfirst.frc4388.robot.subsystems.LED;;
/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the TimedRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the build.properties file in 
 * the project.
 */
public class Robot extends IterativeRobot 
{

	public static OI oi;
	
	public static final Drive drive = new Drive();

	//public static final Elevator elevator = new Elevator();
    public static final Carriage carriage = new Carriage();
    public static final ElevatorAuton elevatorAuton = new ElevatorAuton();
    public static final Elevator elevator = new Elevator();
    
    
    public static final LED led = new LED();

    public static final Climber climber = new Climber();
    public static final Pnumatics pnumatics = new Pnumatics();
    //public static FlashyBlinky flashyBlinky;
    //public static Sensors sensors;

	public static final long periodMS = 10;
	public static final ControlLooper controlLoop = new ControlLooper("Main control loop", periodMS);
	//public static final PowerDistributionPanel pdp = new PowerDistributionPanel();

	//public static final String ElevatorAuton = null;


	public static enum OperationMode { TEST, COMPETITION };
	public static OperationMode operationMode = OperationMode.TEST;

	private SendableChooser<OperationMode> operationModeChooser;
	private SendableChooser<Command> autonTaskChooser;

    private Command autonomousCommand;
    
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() 
    {
    	//Printing game data to riolog
    	String gameData = DriverStation.getInstance().getGameSpecificMessage();
    	System.out.println(gameData);
    	CameraServer.getInstance().startAutomaticCapture();
    	CameraServer.getInstance().putVideo("res", 640, 480);
    	
      try {
    	//drive = new Drive();
    	//controlLoop = new ControlLooper("Main control loop", periodMS);
		oi = OI.getInstance();
		//camera.initialize();
		
    	controlLoop.addLoopable(drive);
    	controlLoop.addLoopable(elevator);
  
//		Waypoint[] points = new Waypoint[] {
//                new Waypoint(0, 0, 0),
//                new Waypoint(-95, -9, Pathfinder.d2r(-27))
//        };
//
//        PathGenerator boilerPath = new PathGenerator(points, 0.01, 120, 200.0, 700.0);			

        operationModeChooser = new SendableChooser<OperationMode>();
	    operationModeChooser.addDefault("Test", OperationMode.TEST);
	    operationModeChooser.addObject("Competition", OperationMode.COMPETITION);
		SmartDashboard.putData("Operation Mode", operationModeChooser);
		
		autonTaskChooser = new SendableChooser<Command>();

		//autonTaskChooser.addDefault("Blue Gear Loading Side Plus Travel Forward", new BlueGearLoadingSideForwardAuton());
		autonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
		//autonTaskChooser.addObject("Right Switch Forward", new RightSwitchForward());
		autonTaskChooser.addDefault("RightStartLeftScore", new RightStartLeftScore());
		
		SmartDashboard.putData("Auton Task", autonTaskChooser);

		
		//ledLights.setAllLightsOn(false);
      } catch (Exception e) {
    		System.err.println("An error occurred in robotInit()");
      }
    }
	
	/**
     * This function is called once each time the robot enters Disabled mode.
     * You can use it to reset any subsystem information you want to clear when
	 * the robot is disabled.
     */
    public void disabledInit(){

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

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
	 * or additional comparisons to the switch structure below with additional strings & commands.
	 */
    public void autonomousInit() {    	
    	updateChoosers();
    	
        // Schedule the autonomous command (example)
    	controlLoop.start();
    	drive.endGyroCalibration();
    	drive.resetEncoders();
    	//elevator.resetElevatorEncoder();
    	drive.resetGyro();
    	drive.setIsRed(getAlliance().equals(Alliance.Red));
        if (autonomousCommand != null) autonomousCommand.start();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
		updateStatus();
    }

    public 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.
        if (autonomousCommand != null) autonomousCommand.cancel();
        //MotionProfileCache.getInstance().release();
    	updateChoosers();
        controlLoop.start();
    	drive.resetEncoders();
    	drive.endGyroCalibration();
    	//shooter.setStage1Speed(0);
    	//shooter.setStage2Speed(0);
    	//shooterFeed.setSpeed(0);
    	//zarkerFeed.setSpeed(0);
    	//shooter.setHopperPosition(HopperState.CLOSE);
        updateStatus();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() 
    {
        Scheduler.getInstance().run();
		updateStatus();
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
        LiveWindow.run();
		updateStatus();
   }
    
    private void updateChoosers() {
		operationMode = (OperationMode)operationModeChooser.getSelected();
		autonomousCommand = (Command)autonTaskChooser.getSelected();
    }
    
    public Alliance getAlliance() {
    	return m_ds.getAlliance();
    }
    
    public void updateStatus() {
    	drive.updateStatus(operationMode);
    	//carriage.updateStatus(operationMode);
    	//shooter.updateStatus(operationMode);
    	//shooterFeed.updateStatus(operationMode);
    	//zarkerFeed.updateStatus(operationMode);
    	//camera.updateStatus(operationMode);
   }

}