wh1ter0se / PowerUp-2018

import org.usfirst.frc.team3695.robot.Constants;

import org.usfirst.frc.team3695.robot.commands.ManualCommandDrive;

import org.usfirst.frc.team3695.robot.enumeration.Drift;

import org.usfirst.frc.team3695.robot.util.Xbox;

import com.ctre.CANTalon;

import edu.wpi.first.wpilibj.Joystick;

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

/** VROOM VROOM */

public class SubsystemDrive extends Subsystem {

	/// drift variables

	                     			 	 // pretty much the cumulative angle since button down, minus offset

	// instantiate the CANTalons here

		// EX: private CANTalon left1;

	/** runs at robot boot */

    public void initDefaultCommand() {

    	setDefaultCommand(new ManualCommandDrive()); }

    /** converts RPM to inches per second */

    /** converts an inches per second number to RPM */

    public static final double ips2rpm(double ips) {

    /** converts rotations to distance traveled in inches */

    public static final double rot2in(double rot) {

    	return rot * Constants.WHEEL_DIAMETER * Math.PI; }

    /** converts distance traveled in inches to rotations */

    public static final double in2rot(double in) {

    	return in / Constants.WHEEL_DIAMETER / Math.PI; }

	/** gives birth to the CANTalons */

    public SubsystemDrive(){

    	// assign initial values to CANTalons

    		// EX: left1 = new CANTalon(Constants.LEFT_MOTOR);

    	// call voltage for each instantiated CANTalon

    		// EX: voltage(left1);

    	// train each CANTalon

    		// master EX: left1.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative);

    		//			  left1.setEncPosition(0);

    		//			  left1.reverseSensor(false);

    		// slave EX:  left2.changeControlMode(CANTalon.TalonControlMode.Follower);

    		//			  left2.set(left1.getDeviceID());

    }

    /** configures the voltage of each CANTalon */

    private void voltage(CANTalon talon) {

    	talon.configNominalOutputVoltage(0f, 0f);

    	talon.EnableCurrentLimit(true);

    }

    /// EVERYTHING BELOW IS DRIFT_DRIVE SHELL CODE

    /** method called by ManualCommandDrive

     * 

     * @param joy

     */

    public void driftDrive(Joystick joy) {

    	double joyVal = Xbox.LEFT_X(joy);

    	if (!IN_DRIFT) { // initiate drift if it hasn't been initiated already

    	}

    	if (DRIFT_IS_CW) { joyVal = -1 * joyVal; } // align joyVal to the write negative and positive values

    	Drift status = updateDriftStatus(joyVal);

		 switch(status) {

			 case DONUT:

				 			driveMecanumRot(ROT_RADIUS, ROT_SPEED, OFFSET);

				 			break;

			 case DEAD:

				 			// this will just continue the last known radius, offset, and speed

				 			break;

			 case POWERSLIDE:

				 			break;

			 case TURNOVER:			

				 			break;

		 }

    }

    /** moved from inside driftDrive; sets up the status switch in driftDrive */

    private Drift updateDriftStatus(double joy) {

    	Drift status;

			 if (joy < (Constants.DRIFT_DEADZONE * -1)) { status = Drift.DONUT; }

		else if (joy < (Constants.DRIFT_DEADZONE))      { status = Drift.DEAD; }

		else if (joy < (Constants.DRIFT_TURNOVER))      { status = Drift.POWERSLIDE; }

		else		 									{ status = Drift.TURNOVER; }

		return status;

    }

    /**

     *  drives in an arc, partially strafing

     *  negative radius means left, positive radius means right

     *  	(these still apply going backwards)

     *  speed is 0-10.0

     *  off is the degrees offset the front of the bot is

     *  	(negative offset is pointed out, positive is pointed in)

     * */

    private void driveMecanumRot(double radius, double speed, int offset) {

    	if (radius < 0) { // if turning left, give more power to right side

    		rightRotPower = 1;

    		leftRotPower  = calculateInnerRotPower(radius, Constants.DISTANCE_BETWEEN_WHEELS); }

    	else { // if turning right, give more power to left side

    		rightRotPower = calculateInnerRotPower(radius, Constants.DISTANCE_BETWEEN_WHEELS);

    		leftRotPower  = 1; }

    	/// standard mecanum code (but the front wheels are averaged with the values for rotation)

	    			Math.toRadians(

	    			* speed) 

	    			+ leftRotPower) 

	    			Math.toRadians(

					* speed) 

					+ rightRotPower) 

    }

    /** 

     * strafes/moves at an angle 

     * speed should be -1.00 to 1.00

     * offset must be -90 to 90 degrees

     * */

    private void driveAngle(double speed, int angle) { // right = 0, up = 90, left = 180, down = 270 

    	/// standard mecanum strafing code

    }

    /** calculates inner power in roation, outer being max */

    private double calculateInnerRotPower(double radius, double botWidth) {

    	return radius / (radius + botWidth);

    }

}