package org.robockets.stronghold.robot.intake;

import org.robockets.stronghold.robot.RobotMap;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 */
public class Intake extends Subsystem {

	Victor intakeVerticalMotor;
	Victor intakeMotor;

	public final PIDController encoderPID;

	public Intake(IntakeSide intakeSide) {
		if (intakeSide == IntakeSide.FRONT) {
			intakeVerticalMotor = RobotMap.intakeVerticalMotorFront;
			intakeMotor = RobotMap.intakeMotorFront;
			encoderPID = new PIDController(0.1, 0.1, 0, RobotMap.intakeEncoderFront, RobotMap.intakeVerticalMotorFront);
		} else {
			intakeVerticalMotor = RobotMap.intakeVerticalMotorBack;
			intakeMotor = RobotMap.intakeMotorBack;
			encoderPID = new PIDController(0.1, 0.1, 0, RobotMap.intakeEncoderBack, RobotMap.intakeVerticalMotorBack);
		}

		encoderPID.disable();
		encoderPID.setSetpoint(0);
		encoderPID.setPercentTolerance(0.05);
		encoderPID.setContinuous(true);
	}

	public void initDefaultCommand() {

	}

	public void spin(double speed) {
		intakeMotor.set(speed);
	}

	public void spinIn() {
		intakeMotor.set(0.5);
	}

	public void spinOut() {
		intakeMotor.set(-0.5);
	}

	public void moveUp() {
		intakeVerticalMotor.set(0.5);
	}

	public void moveDown() {
		intakeVerticalMotor.set(-0.5);
	}

	public void move(int speed) {
		intakeVerticalMotor.set(speed);
	}

	public void move(double setpoint) {
		encoderPID.setSetpoint(setpoint);
	}
	
	public void moveAssisted() {
		encoderPID.setPID(SmartDashboard.getNumber("P"), SmartDashboard.getNumber("I"), SmartDashboard.getNumber("D")); // Still broken
		intakeVerticalMotor.set(encoderPID.get());
	}

	public void setIntakeAngle(double angle) {
		encoderPID.setSetpoint(angle);
	}

	public void enablePID() {
		encoderPID.enable();
		encoderPID.reset();
		encoderPID.setSetpoint(encoderPID.getSetpoint());
	}

	public void stopIntake() {
		intakeMotor.set(0);
	}

	public void stopVertical() {
		intakeVerticalMotor.set(0);
	}
}

@@ -1564,217 +1564,8 @@
 %7D%0A%09%0A
-%09public void moveAssisted() %7B%0A%09%09encoderPID.setPID(SmartDashboard.getNumber(%22P%22), SmartDashboard.getNumber(%22I%22), SmartDashboard.getNumber(%22D%22)); // Still broken%0A%09%09intakeVerticalMotor.set(encoderPID.get());%0A%09%7D%0A%0A
 %09pub

Remove unused methods