package org.robockets.stronghold.robot.highgoalshooter;

import org.robockets.stronghold.robot.RobotMap;
import org.robockets.stronghold.robot.pidsources.EncoderPIDSource;
import org.robockets.stronghold.robot.pidsources.TalonPIDSource;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
 * Subsystem for the high goal shooter mechanism including the turn table, the rollers, the shooting rollers, and the hood.
 */
public class HighGoalShooter extends Subsystem {
	public final double HOOD_ERROR = 2;
	public final double WHEEL_ERROR = 50;
	public final double COUNTS_PER_DEGREE_HOOD = 7.3111;
	
	public final PIDController turnTablePidController;
	public final PIDController hoodPidController;
	public final PIDController shootingWheelPidController;
	
	public final EncoderPIDSource turnTableSource;
	
	public HighGoalShooter() {
		// ! Note the rate should be negative if on the practice robot!
		//turnTableSource = new EncoderPIDSource(RobotMap.turnTableEncoder, 0.16096579, PIDSourceType.kDisplacement);
		turnTableSource = new EncoderPIDSource(RobotMap.turnTableEncoder, 360.0 / 2174.375, PIDSourceType.kDisplacement);
		EncoderPIDSource hoodSource = new EncoderPIDSource(RobotMap.hoodEncoder, 1.0 / COUNTS_PER_DEGREE_HOOD, PIDSourceType.kDisplacement);
		
		turnTablePidController = new PIDController(0.08, 0.0015, 0, turnTableSource, RobotMap.turnTableMotor);
		//hoodPidController = new PIDController(0.02, 0.0001, 0, new HoodPIDSource(), RobotMap.hoodMotor); Used to be correct but for some reason it changed
		hoodPidController = new PIDController(0.075, 0.0001, 0, hoodSource, RobotMap.hoodMotor);
		shootingWheelPidController = new PIDController(0.0001, 0.00001, 0.001, new TalonPIDSource(), RobotMap.shootingWheelMotor);
		
		turnTablePidController.disable();
		hoodPidController.disable();
		shootingWheelPidController.disable();
		
		turnTablePidController.setSetpoint(0);
		//turnTablePidController.setPercentTolerance(0.05);
		turnTablePidController.setContinuous(true);
		
		hoodPidController.setSetpoint(0);
		hoodPidController.setContinuous(true);
		
		shootingWheelPidController.setSetpoint(0);
		shootingWheelPidController.setContinuous(true);
		shootingWheelPidController.setOutputRange(0, 1);
		
		turnTablePidController.enable();
		hoodPidController.enable();
		shootingWheelPidController.enable();
	}
	
    public void initDefaultCommand() {
    }
    
    public void setShooterFlipper(double speed) {
    	RobotMap.shootingFlipper.set(speed);
    }
    
    public double getTurnTableAngle() {
    	return RobotMap.turnTableEncoder.get() * 0.16096579;
    }
    
    public void setTurnTableAngle(double angle) {
    	//if (angle > 270) {
    	//	System.out.println(angle % 270 + 90);
    	//} else if (angle < 270) {
    	//	System.out.println(angle % -270 - 90);
    	//} else {
    	//	System.out.println(angle);
    	//}
    	turnTablePidController.setSetpoint(angle);
    }
    
    public double getTurnTableSetpoint() {
    	return turnTablePidController.getSetpoint();
    }
    
    public void setHoodAngle(double angle) {
    	hoodPidController.setSetpoint(angle);
    }
    
    public double getHoodSetpoint() {
    	return hoodPidController.getSetpoint();
    }
    
    public double getHoodAngle() {
    	return RobotMap.hoodEncoder.get() / COUNTS_PER_DEGREE_HOOD;
    }
    
    public boolean hoodOnTarget() {
    	return Math.abs(Math.abs(hoodPidController.getSetpoint()) - Math.abs(getHoodAngle())) < HOOD_ERROR;
    }

    public boolean turnTableOnTarget(){
    	return Math.abs(turnTablePidController.getSetpoint() - turnTableSource.pidGet()) < 0.5;
    }
    
    /**
     * Roll the shooting wheel to fire the boulder at a desired speed.
     * @param speed 	The rpm to set the shooting mechanism at.
     */
    public void setShootingWheelSpeed(double speed) {
    	shootingWheelPidController.setSetpoint(speed);
    }
    
    public void setShootingWheelVoltage(double voltage) {
    	RobotMap.shootingWheelMotor.set(voltage);
    }
    
    public double getShootingWheelSpeed() {
    	return RobotMap.shootingWheelMotor.getEncVelocity() / 1024.0 * 60.0;
    }
    
    public boolean shootingWheelOnTarget() {
    	return Math.abs(Math.abs(shootingWheelPidController.getSetpoint()) - Math.abs(getShootingWheelSpeed())) < WHEEL_ERROR;
    }
    
    public void enableTurnTablePID() {
    	turnTablePidController.enable();
    }
    
    public void disableTurnTablePID() {
    	turnTablePidController.disable();
    }
    
    public void enableHoodPID() {
		hoodPidController.enable();
		hoodPidController.reset();
		hoodPidController.setSetpoint(RobotMap.hoodEncoder.get());
    }
}


@@ -2733,17 +2733,17 @@
 %7B%0A    %09/
-/
+*
 if (angl
@@ -2750,34 +2750,32 @@
 e %3E 270) %7B%0A    %09
-//
 %09System.out.prin
@@ -2798,26 +2798,24 @@
 + 90);%0A    %09
-//
 %7D else if (a
@@ -2825,34 +2825,32 @@
 e %3C 270) %7B%0A    %09
-//
 %09System.out.prin
@@ -2878,18 +2878,16 @@
 );%0A    %09
-//
 %7D else %7B
@@ -2892,18 +2892,16 @@
  %7B%0A    %09
-//
 %09System.
@@ -2929,11 +2929,11 @@
    %09
-//%7D
+%7D*/
 %0A   

Use slashasterisk instead of many slashslash