package com.team687.frc2017.commands;

import java.util.ArrayList;

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

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

/**
 * Drive a path generated from Bezier curve
 *
 * @author tedlin
 *
 */

public class DriveBezierRio extends Command {

    private BezierCurve m_path;
    private double m_basePower = 1; // always equal to 1
    private double m_straightPower;
    private boolean m_straightPowerIsDynamic;
    private boolean m_softStop;

    private ArrayList<Double> m_heading;
    private ArrayList<Double> m_arcLength;

    private int m_counter;
    private boolean m_pathIsFinished;

    public DriveBezierRio(double x0, double y0, double x1, double y1, double x2, double y2, double x3, double y3,
	    double straightPower, boolean straightPowerIsDynamic, boolean softStop) {
	m_path = new BezierCurve(x0, y0, x1, y1, x2, y2, x3, y3);
	m_straightPower = straightPower;
	m_straightPowerIsDynamic = straightPowerIsDynamic;
	m_softStop = softStop;
    }

    /**
     * 
     * @param path
     * @param straightPower
     *            (postive if going forward (forward is side with climber), negative
     *            if going backwards)
     * @param straightPowerIsDynamic
     *            (true for paths with sharp turns)
     * @param softStop
     *            (if you want to slow down near end)
     */
    public DriveBezierRio(double[] path, double straightPower, boolean straightPowerIsDynamic, boolean softStop) {
	m_path = new BezierCurve(path[0], path[1], path[2], path[3], path[4], path[5], path[6], path[7]);
	m_straightPower = straightPower;
	m_straightPowerIsDynamic = straightPowerIsDynamic;
	m_softStop = softStop;
    }

    @Override
    protected void initialize() {
	SmartDashboard.putString("Current Command", "DriveBezierRio");
	Robot.drive.stopDrive();
	Robot.drive.resetEncoders();
	Robot.drive.shiftUp();

	m_path.calculateBezier();
	m_heading = m_path.getHeading();
	m_arcLength = m_path.getArcLength();

	m_counter = 0;
	m_pathIsFinished = false;
	m_basePower = 1 * Math.signum(m_straightPower);
    }

    @Override
    protected void execute() {
	if (m_counter < m_arcLength.size()) {
	    if (Robot.drive.getDrivetrainTicks() < m_arcLength.get(m_counter)) {
		double robotAngle = (360 - Robot.drive.getCurrentYaw()) % 360;
		double error = -m_heading.get(m_counter) - robotAngle;
		// double expectedDeltaHeading = 0;
		// if (m_counter >= 1) {
		// expectedDeltaHeading = Math.abs(m_heading.get(m_counter) -
		// m_heading.get(m_counter - 1));
		// }

		error = (error > 180) ? error - 360 : error;
		error = (error < -180) ? error + 360 : error;

		double rotPower = Constants.kRotPBezier * error;
		// default is specified straight power
		double straightPower = m_straightPower;
		double direction = Math.signum(m_straightPower);

		// dynamic straight power
		if (m_straightPowerIsDynamic) {
		    straightPower = direction * m_basePower / (Math.abs(error) * Constants.kStraightPowerAdjuster);
		}

		double maxStraightPower = Constants.kMaxStraightPower;
		if (m_softStop) {
		    double straightError = m_arcLength.get(m_arcLength.size() - 1) - Robot.drive.getDrivetrainTicks();
		    double newMaxStraightPower = Constants.kDistPBezier * straightError;
		    maxStraightPower = Math.min(Math.abs(maxStraightPower), Math.abs(newMaxStraightPower));
		}

		// limit straight power to maintain rotPower to straightPower ratio
		// also for soft landings
		if (Math.abs(straightPower) > maxStraightPower) {
		    straightPower = maxStraightPower * direction;
		}

		// make sure robot reaches end point
		if (Math.abs(straightPower) < Constants.kMinStraightPower) {
		    straightPower = Constants.kMinStraightPower * direction;
		}

		double leftPow = rotPower + straightPower;
		double rightPow = rotPower - straightPower;
		Robot.drive.setPower(leftPow, rightPow);
	    } else {
		m_counter++;
	    }
	} else {
	    m_pathIsFinished = true;
	}
    }

    @Override
    protected boolean isFinished() {
	return m_pathIsFinished || Robot.drive.getDrivetrainTicks() >= m_arcLength.get(m_arcLength.size() - 1);
    }

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

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

}

@@ -768,16 +768,48 @@
 inished;
+%0A    private double m_direction;
 %0A%0A    pu
@@ -2205,23 +2205,19 @@
 %0A%09m_
-basePower = 1 *
+direction =
  Mat
@@ -2342,16 +2342,25 @@
 %09    if 
+(Math.abs
 (Robot.d
@@ -2384,16 +2384,17 @@
 nTicks()
+)
  %3C m_arc
@@ -2946,59 +2946,8 @@
 wer;
-%0A%09%09double direction = Math.signum(m_straightPower);
 %0A%0A%09%09
@@ -3028,16 +3028,18 @@
 Power = 
+m_
 directio
@@ -3260,19 +3260,35 @@
 e() - 1)
- - 
+%0A%09%09%09    - Math.abs(
 Robot.dr
@@ -3311,16 +3311,17 @@
 nTicks()
+)
 ;%0A%09%09    
@@ -3671,32 +3671,34 @@
 StraightPower * 
+m_
 direction;%0A%09%09%7D%0A%0A
@@ -3851,16 +3851,18 @@
 Power * 
+m_
 directio
@@ -4168,12 +4168,23 @@
 shed
- %7C%7C 
+%0A%09%09%7C%7C Math.abs(
 Robo
@@ -4211,16 +4211,17 @@
 nTicks()
+)
  %3E= m_ar

fix driving reverse path