package org.robockets.stronghold.robot;

import org.robockets.buttonmanager.ButtonManager;
import org.robockets.stronghold.robot.OI;
import org.robockets.stronghold.robot.highgoalshooter.HighGoalShooter;
import org.robockets.stronghold.robot.intake.Intake;
import org.robockets.stronghold.robot.intake.IntakeSide;
import org.robockets.stronghold.robot.drivetrain.Drivetrain;
import org.robockets.stronghold.robot.commands.Autonomous;
import org.robockets.stronghold.robot.commands.Teleop;

import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;

/**
 * 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 OI oi;
	public static final Drivetrain driveTrain = new Drivetrain();
	public static final Intake intakeFront = new Intake(IntakeSide.FRONT);
	public static final Intake intakeBack = new Intake(IntakeSide.BACK);
	public static final HighGoalShooter shooter = new HighGoalShooter();

	
	Command teleop;
	Command autonomousCommand = new Autonomous();

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
      oi = new OI();
      teleop = new Teleop();
      CameraServer server = CameraServer.getInstance();
      server.startAutomaticCapture("cam0");
    }
	
	/**
     * 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();
	    shooter.setHoodAngle(shooter.getHoodAngle());
	    shooter.setShootingWheelSpeed(shooter.getShootingWheelSpeed());
	    shooter.setTurnTableAngle(shooter.getTurnTableAngle());
	    intakeBack.setIntakeAngle(intakeBack.getIntakeAngle());
	    intakeFront.setIntakeAngle(intakeFront.getIntakeAngle());
	}

	/**
	 * 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() {        
    	// schedule the autonomous command (example)
        if (autonomousCommand != null) autonomousCommand.start();
    }

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

    public void teleopInit() {
    	ButtonManager.start();
		// 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();
        //(new UpdateDashboard()).start();
        teleop.start();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
        LiveWindow.run();
    }
}

@@ -716,16 +716,73 @@
 eWindow;
+%0Aimport edu.wpi.first.wpilibj.networktables.NetworkTable;
 %0A%0A/**%0A *
@@ -1826,16 +1826,55 @@
 %22cam0%22);
+%0A%0A      NetworkTable.globalDeleteAll();
 %0A    %7D%0A%09

Delete all network tables when robot starts