package com.team687.frc2017.commands;

import com.team687.frc2017.Constants;
import com.team687.frc2017.Robot;

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_kP;

    private double m_straightPower;
    private double m_desiredAngle;
    private boolean m_isRightPowered;
    private double m_timeout;
    private boolean m_isHighGear;

    private double m_startTime;
    private double m_error;

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

	requires(Robot.drive);
    }

    /**
     * @param desiredAngle
     * @param isRightPowered
     * @param straightPower
     * @param isHighGear
     */
    public ArcTurn(double desiredAngle, boolean isRightPowered, double straightPower, boolean isHighGear) {
	m_desiredAngle = desiredAngle;
	m_isRightPowered = isRightPowered;
	m_straightPower = straightPower;
	m_timeout = 10;
	m_isHighGear = isHighGear;

	requires(Robot.drive);
    }

    /**
     * Arc Turn
     * 
     * @param desiredAngle
     * @param isRightPowered
     * @param striaghtPower
     * @param isHighGear
     * @param timeout
     */
    public ArcTurn(double desiredAngle, boolean isRightPowered, double straightPower, boolean isHighGear,
	    double timeout) {
	m_desiredAngle = desiredAngle;
	m_isRightPowered = isRightPowered;
	m_straightPower = straightPower;
	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_kP = Constants.kRotPHighGear;
	} else if (!m_isHighGear) {
	    Robot.drive.shiftDown();
	    m_kP = Constants.kRotPLowGear;
	}
    }

    @Override
    protected void execute() {
	double robotAngle = (360 - Robot.drive.getCurrentYaw()) % 360;
	m_error = m_desiredAngle - robotAngle;
	SmartDashboard.putNumber("Angle Error", m_error);
	double power = m_kP * m_error * 1.95; // multiplied by 2 because only one side of the drivetrain is moving
	if (m_isRightPowered) {
	    Robot.drive.setPower(0 + m_straightPower, power - m_straightPower);
	} else if (!m_isRightPowered) {
	    Robot.drive.setPower(power + m_straightPower, 0 - m_straightPower);
	}
    }

    @Override
    protected boolean isFinished() {
	// TODO Auto-generated method stub
	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();
    }

}

@@ -361,16 +361,84 @@
 le m_kP;
+%0A    private double m_minRotPower;%0A    private double m_maxRotPower;
 %0A%0A    pr
@@ -2062,93 +2062,303 @@
 r;%0A%09
-%7D else if (!m_isHighGear) %7B%0A%09    Robot.drive.shiftDown();%0A%09    m_kP = Constants.kRotP
+    m_minRotPower = Constants.kMinRotPowerHighGear;%0A%09    m_maxRotPower = Constants.kMaxRotPowerHighGear;%0A%09%7D else if (!m_isHighGear) %7B%0A%09    Robot.drive.shiftDown();%0A%09    m_kP = Constants.kRotPLowGear;%0A%09    m_minRotPower = Constants.kMinRotPowerLowGear;%0A%09    m_maxRotPower = Constants.kMaxRotPower
 LowG
@@ -2580,17 +2580,20 @@
 %09double 
-p
+rotP
 ower = m
@@ -2683,16 +2683,224 @@
  moving%0A
+%0A%09double sign = Math.signum(rotPower);%0A%09if (Math.abs(rotPower) %3E m_maxRotPower) %7B%0A%09    rotPower = m_maxRotPower * sign;%0A%09%7D%0A%09if (Math.abs(rotPower) %3C m_minRotPower) %7B%0A%09    rotPower = m_minRotPower * sign;%0A%09%7D%0A%0A
 %09if (m_i
@@ -2963,17 +2963,20 @@
 tPower, 
-p
+rotP
 ower - m
@@ -3051,17 +3051,20 @@
 etPower(
-p
+rotP
 ower + m
@@ -3166,44 +3166,8 @@
 ) %7B%0A
-%09// TODO Auto-generated method stub%0A
 %09ret

switch gears for arcturn