package org.robockets.stronghold.robot.highgoalshooter;

import org.robockets.stronghold.robot.ResetPID;
import org.robockets.stronghold.robot.Robot;
import org.robockets.stronghold.robot.RobotMap;
import org.robockets.stronghold.robot.flipper.FireShooter;
import org.robockets.stronghold.robot.shootingwheel.RPMAlign;
import org.robockets.stronghold.robot.turntable.HorizontalAlign;
import org.robockets.stronghold.robot.turntable.MoveTurnTable;
import org.robockets.stronghold.robot.commands.SetPID;
import org.robockets.stronghold.robot.drivetrain.AssistedDrive;
import org.robockets.stronghold.robot.drivetrain.AssistedRotateType;
import org.robockets.stronghold.robot.drivetrain.AssistedTranslateType;
import org.robockets.stronghold.robot.drivetrain.DirectDrive;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 */
public class UpdateHighGoalShooterDashboard extends Command {

	NetworkTable visionTable;
	
    public UpdateHighGoalShooterDashboard() {
    }

    protected void initialize() {
    	visionTable = NetworkTable.getTable("vision");
    	
    	/*SmartDashboard.putData("Move Servo", new FireShooter());
    	SmartDashboard.putNumber("pid error", 0);*/
    	SmartDashboard.putData("Reset Turn Table", new MoveTurnTable(0));
    	SmartDashboard.putData("Turn table right", new MoveTurnTable(20, 0));
    	SmartDashboard.putData("Turn table left", new MoveTurnTable(-20, 0));
    	SmartDashboard.putData("Set PID", new SetPID("turntable", Robot.turntable.pidController));
    	SmartDashboard.putData("Move turn table to 30 degrees", new MoveTurnTable(30));
    	SmartDashboard.putData("Horizontal align", new HorizontalAlign(true));
    	SmartDashboard.putData("RPM align", new RPMAlign(true));
    	SmartDashboard.putData("Vertical align", new VerticalAlign(true));
    	SmartDashboard.putData("Shoot", new FireShooter());
    	SmartDashboard.putData("Free fire", new FreeFire());
    	SmartDashboard.putNumber("Extra", 200);
    	SmartDashboard.putNumber("Bonus Angle", 1);
    	
    	SmartDashboard.putNumber("Setpoint Forward", 0);
    	SmartDashboard.putNumber("Setpoint Backward", 0);

       	SmartDashboard.putData("Set Left Drivetrain PID", new SetPID("Left Drivetrain", Robot.driveTrain.leftWheelsPID));
    	SmartDashboard.putData("Set Right Drivetrain PID", new SetPID("Right Drivetrain", Robot.driveTrain.rightWheelsPID));
    	SmartDashboard.putData("Move Forwards", new DirectDrive(24));
    	SmartDashboard.putData("Move Backwards", new DirectDrive(-24));
    	SmartDashboard.putData("Left Side Reset", new ResetPID(RobotMap.driveEncoderLeft, Robot.driveTrain.leftWheelsPID));
    	SmartDashboard.putData("Right Side Reset", new ResetPID(RobotMap.driveEncoderRight, Robot.driveTrain.rightWheelsPID));
    	
    	SmartDashboard.putData("Assisted Move Forwards", new AssistedDrive(AssistedTranslateType.ENCODER, AssistedRotateType.ENCODER, 144, 0, 36));
    	SmartDashboard.putData("Assisted Move Backwards", new AssistedDrive(AssistedTranslateType.ENCODER, AssistedRotateType.ENCODER, -144, 0, -36));

    }

    protected void execute() {    	
    	SmartDashboard.putNumber("Drive Encoder Left", Robot.driveTrain.getLeftDistanceInInches());
    	SmartDashboard.putNumber("Drive Encoder Right", Robot.driveTrain.getRightDistanceInInches());
    	
    	/*SmartDashboard.putBoolean("Front Breakbeam", RobotMap.frontBB.get());
    	SmartDashboard.putNumber("Turn table angle", Robot.turntable.getAngle());
    	SmartDashboard.putNumber("Turn table encoder", RobotMap.turnTableEncoder.get());
    	SmartDashboard.putNumber("Left Setpoint", Robot.driveTrain.getLeftDistanceSetpointInInches());
    	SmartDashboard.putNumber("Right Side Setpoint", Robot.driveTrain.getRightDistanceSetpointInInches());

    	SmartDashboard.putNumber("Hood angle", Robot.hood.getAngle());
    	SmartDashboard.putNumber("Spin RPM", Robot.shootingWheel.getSpeed());
    	SmartDashboard.putNumber("Front Intake angle", RobotMap.intakeEncoderFront.get() / Robot.intakeVerticalFront.COUNTS_PER_DEGREE);
    	SmartDashboard.putNumber("Yaw", RobotMap.navX.getYaw());
    	
    	SmartDashboard.putNumber("Lock", visionTable.getNumber("can_see_target", 0));*/
    	SmartDashboard.putNumber("pid error", Robot.turntable.getSetpoint() - Robot.turntable.getAngle());
		//SmartDashboard.putNumber("intake angle", Robot.intakeVerticalFront.getIntakeAngle());
    	SmartDashboard.putNumber("distance", visionTable.getNumber("distance_guess", 0));
    	SmartDashboard.putBoolean("Can see target", visionTable.getNumber("can_see_target", 0) == 1);
    }

    protected boolean isFinished() {
        return false;
    }

    protected void end() {
    }

    protected void interrupted() {
    }
}

@@ -54,56 +54,8 @@
 r;%0A%0A
-import org.robockets.stronghold.robot.ResetPID;%0A
 impo
@@ -95,16 +95,16 @@
 .Robot;%0A
+
 import o
@@ -450,275 +450,8 @@
 PID;
-%0Aimport org.robockets.stronghold.robot.drivetrain.AssistedDrive;%0Aimport org.robockets.stronghold.robot.drivetrain.AssistedRotateType;%0Aimport org.robockets.stronghold.robot.drivetrain.AssistedTranslateType;%0Aimport org.robockets.stronghold.robot.drivetrain.DirectDrive;
 %0A%0Aim
@@ -1798,1049 +1798,8 @@
 1);%0A
-    %09%0A    %09SmartDashboard.putNumber(%22Setpoint Forward%22, 0);%0A    %09SmartDashboard.putNumber(%22Setpoint Backward%22, 0);%0A%0A       %09SmartDashboard.putData(%22Set Left Drivetrain PID%22, new SetPID(%22Left Drivetrain%22, Robot.driveTrain.leftWheelsPID));%0A    %09SmartDashboard.putData(%22Set Right Drivetrain PID%22, new SetPID(%22Right Drivetrain%22, Robot.driveTrain.rightWheelsPID));%0A    %09SmartDashboard.putData(%22Move Forwards%22, new DirectDrive(24));%0A    %09SmartDashboard.putData(%22Move Backwards%22, new DirectDrive(-24));%0A    %09SmartDashboard.putData(%22Left Side Reset%22, new ResetPID(RobotMap.driveEncoderLeft, Robot.driveTrain.leftWheelsPID));%0A    %09SmartDashboard.putData(%22Right Side Reset%22, new ResetPID(RobotMap.driveEncoderRight, Robot.driveTrain.rightWheelsPID));%0A    %09%0A    %09SmartDashboard.putData(%22Assisted Move Forwards%22, new AssistedDrive(AssistedTranslateType.ENCODER, AssistedRotateType.ENCODER, 144, 0, 36));%0A    %09SmartDashboard.putData(%22Assisted Move Backwards%22, new AssistedDrive(AssistedTranslateType.ENCODER, AssistedRotateType.ENCODER, -144, 0, -36));%0A%0A
     
@@ -2488,17 +2488,16 @@
 hes());%0A
-%0A
     %09Sma
@@ -2765,16 +2765,16 @@
 EGREE);%0A
+
     %09Sma
@@ -2823,30 +2823,24 @@
 .getYaw());%0A
-    %09%0A
     %09SmartDa

Remove test buttons from SD