package com.team687.frc2017.commands;

import com.team687.frc2017.Constants;
import com.team687.frc2017.Robot;
import com.team687.frc2017.utilities.NerdyMath;
import com.team687.frc2017.utilities.PGains;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * Arc turning
 * 
 * @author tedlin
 *
 */

public class ArcTurn extends Command {

    private double m_desiredAngle;
    private boolean m_isRightPowered;
    private boolean m_isHighGear;

    private PGains m_rotPGains;

    private double m_startTime, m_timeout;
    private double m_error;

    public ArcTurn(double desiredAngle, boolean isRightPowered, boolean isHighGear) {
	m_desiredAngle = desiredAngle;
	m_isRightPowered = isRightPowered;
	m_timeout = 10;
	m_isHighGear = isHighGear;

	requires(Robot.drive);
    }

    /**
     * Arc Turn
     * 
     * @param desiredAngle
     * @param isRightPowered
     * @param isHighGear
     * @param timeout
     */
    public ArcTurn(double desiredAngle, boolean isRightPowered, boolean isHighGear, double timeout) {
	m_desiredAngle = desiredAngle;
	m_isRightPowered = isRightPowered;
	m_timeout = timeout;
	m_isHighGear = isHighGear;

	requires(Robot.drive);
    }

    @Override
    protected void initialize() {
	SmartDashboard.putString("Current Command", "ArcTurn");

	m_startTime = Timer.getFPGATimestamp();
	if (m_isHighGear) {
	    Robot.drive.shiftUp();
	    m_rotPGains = Constants.kRotHighGearPGains;
	} else if (!m_isHighGear) {
	    Robot.drive.shiftDown();
	    m_rotPGains = Constants.kRotLowGearPGains;
	}
    }

    @Override
    protected void execute() {
	double robotAngle = (360 - Robot.drive.getCurrentYaw()) % 360;
	m_error = -m_desiredAngle - robotAngle;
	m_error = (m_error > 180) ? m_error - 360 : m_error;
	m_error = (m_error < -180) ? m_error + 360 : m_error;
	double rotPower = m_rotPGains.getP() * m_error * 1.95; // multiplied by 2 because the rotational component is
							       // only added to one side of the drivetrain
	rotPower = NerdyMath.threshold(rotPower, m_rotPGains.getMinPower(), m_rotPGains.getMaxPower());

	if (m_isRightPowered) {
	    Robot.drive.setPower(0, rotPower);
	} else if (!m_isRightPowered) {
	    Robot.drive.setPower(rotPower, 0);
	}
    }

    @Override
    protected boolean isFinished() {
	return Math.abs(m_error) < Constants.kDriveRotationTolerance
		|| Timer.getFPGATimestamp() - m_startTime > m_timeout;
    }

    @Override
    protected void end() {
	Robot.drive.stopDrive();
    }

    @Override
    protected void interrupted() {
	end();
    }

}

@@ -652,232 +652,29 @@
    p
-ublic ArcTurn(double desiredAngle, boolean isRightPowered, boolean isHighGear) %7B%0A%09m_desiredAngle = desiredAngle;%0A%09m_isRightPowered = isRightPowered;%0A%09m_timeout = 10;%0A%09m_isHighGear = isHighGear;%0A%0A%09requires(Robot.drive);%0A    %7D
+rivate double m_sign;
 %0A%0A  
@@ -806,16 +806,68 @@
 timeout%0A
+     * @param sign%0A     *            (+1.0 or -1.0)%0A
      */%0A
@@ -964,16 +964,29 @@
  timeout
+, double sign
 ) %7B%0A%09m_d
@@ -1098,16 +1098,45 @@
 ighGear;
+%0A%09m_sign = Math.signum(sign);
 %0A%0A%09requi
@@ -1949,16 +1949,58 @@
 ivetrain
+%0A%0A%09double rawSign = Math.signum(rotPower);
 %0A%09rotPow
@@ -2024,24 +2024,33 @@
 reshold(
+Math.abs(
 rotPower
 , m_rotP
@@ -2041,16 +2041,17 @@
 rotPower
+)
 , m_rotP
@@ -2097,16 +2097,69 @@
 Power())
+%0A%09%09* rawSign;%0A%09rotPower = Math.abs(rotPower) * m_sign
 ;%0A%0A%09if (

fix arc turn