package org.usfirst.frc.team687.robot.commands;

import org.usfirst.frc.team687.robot.Constants;
import org.usfirst.frc.team687.robot.Robot;
import org.usfirst.frc.team687.robot.utilities.NerdyMath;

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

import jaci.pathfinder.*;

/**
 * Drive a trajectory from Pathfinder (https://github.com/JacisNonsense/Pathfinder)
 *
 * @author tedfoodlin
 *
 */

public class DriveTrajectoryPathfinder extends Command {
	
	private Waypoint[] m_path;
	private double m_dT = 0.01;

	private Trajectory m_trajectory;
	private Trajectory m_leftTrajectory;
	private Trajectory m_rightTrajectory;
	
	private EncoderFollower m_leftFollower;
	private EncoderFollower m_rightFollower;
	
	private double m_leftPow;
	private double m_rightPow;
	
	public DriveTrajectoryPathfinder(Waypoint[] path) {
		m_path = path;
		
		// subsystem dependencies
		requires(Robot.drive);
	}
	
	@Override
	protected void initialize() {
        Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH, 
        		m_dT, Constants.kMaxVelocity, Constants.kMaxAcceleration, Constants.kMaxJerk);
        m_trajectory = Pathfinder.generate(m_path, config);
        TankModifier modifier = new TankModifier(m_trajectory).modify(Constants.kWheelbaseWidth);
        
        m_leftTrajectory = modifier.getLeftTrajectory();
        m_rightTrajectory = modifier.getRightTrajectory();
        
        m_leftFollower = new EncoderFollower(m_leftTrajectory);
        m_rightFollower = new EncoderFollower(m_rightTrajectory);
        
        m_leftFollower.configurePIDVA(Constants.kDistP, Constants.kDistI, Constants.kDistD, Constants.kV, Constants.kA);
        m_rightFollower.configurePIDVA(Constants.kDistP, Constants.kDistI, Constants.kDistD, Constants.kV, Constants.kA);
        
        m_leftFollower.configureEncoder(0, Constants.kTicksPerRev, Constants.kWheelDiameter);
        m_rightFollower.configureEncoder(0, Constants.kTicksPerRev, Constants.kWheelDiameter);
	}
	
	@Override
	protected void execute() {
		m_leftPow = m_leftFollower.calculate(Robot.drive.getLeftTicks());
		m_rightPow = m_rightFollower.calculate(Robot.drive.getRightTicks());
		
		double[] pow = {m_leftPow, m_rightPow};
		NerdyMath.normalize(pow, false);
		
		Robot.drive.setPower(pow[0], pow[1]);
	}

	@Override
	protected boolean isFinished() {
		return m_leftFollower.isFinished() && m_rightFollower.isFinished();
	}
	
	@Override 
	protected void end() {
		reset();
	}
	
	@Override
	protected void interrupted() {
		end();
	}
	
	private void reset() {
		m_leftFollower.reset();
		m_rightFollower.reset();
		Robot.drive.stopDrive();
	}

}

@@ -191,16 +191,73 @@
 rdyMath;
+%0Aimport org.usfirst.frc.team687.robot.utilities.NerdyPID;
 %0A%0Aimport
@@ -412,16 +412,75 @@
 finder)%0A
+ * Includes heading correction on top of encoder algorithm%0A
  *%0A * @a
@@ -817,18 +817,16 @@
 llower;%0A
-%09%0A
 %09private
@@ -830,52 +830,36 @@
 ate 
-double m_leftPow;%0A%09private double m_rightPow
+NerdyPID m_headingCorrection
 ;%0A%09%0A
@@ -2118,16 +2118,162 @@
 meter);%0A
+        %0A        m_headingCorrection = new NerdyPID(Constants.kRotP, Constants.kRotI, Constants.kRotD);%0A        %0A        Robot.drive.stopDrive();%0A
 %09%7D%0A%09%0A%09@O
@@ -2306,26 +2306,31 @@
 ecute() %7B%0A%09%09
-m_
+double 
 leftPow = m_
@@ -2379,26 +2379,31 @@
 Ticks());%0A%09%09
-m_
+double 
 rightPow = m
@@ -2460,16 +2460,208 @@
 ());%0A%09%09%0A
+%09%09m_headingCorrection.setDesired(m_leftFollower.getHeading());%0A%09%09double angularPow = m_headingCorrection.calculate(Robot.drive.getYaw());%0A%09%09leftPow += angularPow;%0A%09%09rightPow -= angularPow;%0A%09%09%0A
 %09%09double
@@ -2670,18 +2670,16 @@
  pow = %7B
-m_
 leftPow,
@@ -2679,18 +2679,16 @@
 eftPow, 
-m_
 rightPow
@@ -2725,19 +2725,16 @@
 false);%0A
-%09%09%0A
 %09%09Robot.
@@ -2932,24 +2932,51 @@
 %0A%09%09reset();%0A
+%09%09Robot.drive.stopDrive();%0A
 %09%7D%0A%09%0A%09@Overr
@@ -3106,35 +3106,8 @@
 ();%0A
-%09%09Robot.drive.stopDrive();%0A
 %09%7D%0A%0A

add heading correction