package org.usfirst.frc.team4342.api.drive;

import org.usfirst.frc.team4342.robot.components.Repository;

import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDOutput;

public class TankDrive implements PIDOutput
{
	private Joystick j;
	private DriveTrain driveTrain;
	private CANTalon fr, fl, mr, ml, rr, rl;
	private AHRS navX;
	private DoubleSolenoid shifter;
	
	public TankDrive(Joystick j, DriveTrain talons, AHRS navX, DoubleSolenoid shifter)
	{
		this.j = j;
		
		this.driveTrain = talons;
		fr = driveTrain.getFrontRight();
		fl = driveTrain.getFrontLeft();
		mr = driveTrain.getMiddleRight();
		ml = driveTrain.getMiddleLeft();
		rr = driveTrain.getRearRight();
		rl = driveTrain.getRearLeft();
		
		this.navX = navX;
		
		this.shifter = shifter;
	}
	
	@Override
	public void pidWrite(double output) 
	{
		fr.set(-output);
		fl.set(output);
		mr.set(-output);
		ml.set(output);
		rr.set(-output);
		rl.set(output);
	}
	
	public void drive(int shiftButton)
	{
		checkUserShift(shiftButton);
		
		double x = j.getX();
		double y = j.getY();
		double z = j.getZ();
		
		double a;
		
		if (z == 0)
			a = x;
		else if (x == 0)
			a = z;
		else
			a = (x+z) / 2.0;
		
		double left = (y-a);
		double right = (y+a);
		
		if (left > 1.0)
            left = 1.0;
        else if (left < -1.0)
            left = -1.0;
		
        if (right > 1.0)
            right = 1.0;
        else if (right < -1.0)
            right = -1.0;
        
		try
		{
			fr.set(right);
			fl.set(left);
			mr.set(right);
			ml.set(left);
			rr.set(right);
			rl.set(left);
			
		}
		catch (Exception ex)
		{
			Repository.Logs.error("Failed to set drive motors", ex);
		}
	}
	
	public void stopAll()
	{
		driveTrain.stopAll();
	}
	
	private void checkUserShift(int button)
	{
		if(j.getRawButton(button))
			shifter.set(DoubleSolenoid.Value.kReverse);
		else
			shifter.set(DoubleSolenoid.Value.kForward);	
	}
	
	public void goToSetpoint(double setpointAngle)
	{
		
	}
}

@@ -292,22 +292,126 @@
 libj.PID
-Output
+Controller;%0Aimport edu.wpi.first.wpilibj.PIDOutput;%0Aimport edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 ;%0A%0Apubli
@@ -599,16 +599,53 @@
 hifter;%0A
+%09private PIDController angleControl;%0A
 %09%0A%09publi
@@ -1029,16 +1029,500 @@
 hifter;%0A
+%09%09%0A%09%09int pGain = Integer.parseInt(SmartDashboard.getData(%22Drive P Value%22).toString());%0A%09%09int iGain = Integer.parseInt(SmartDashboard.getData(%22Drive I Value%22).toString());%0A%09%09int dGain = Integer.parseInt(SmartDashboard.getData(%22Drive D Value%22).toString());%0A%09%09angleControl = new PIDController(pGain, iGain, dGain, navX, Repository.TankDrive);%0A%09%09%0A%09%09angleControl.setContinuous();%0A%09%09angleControl.setInputRange(0.0, 360.0);%0A%09%09angleControl.setOutputRange(-1.0, 1.0);%0A%09%09angleControl.enable();%0A
 %09%7D%0A%09%0A%09@O
@@ -1690,24 +1690,37 @@
 %7D%0A%09%0A%09public 
+synchronized 
 void drive(i
@@ -2431,24 +2431,37 @@
 %7D%0A%09%0A%09public 
+synchronized 
 void stopAll
@@ -2504,16 +2504,29 @@
 private 
+synchronized 
 void che
@@ -2700,53 +2700,400 @@
 lic 
-void goToSetpoint(double setpointAngle)%0A%09%7B%0A%09%09
+synchronized void turnOn()%0A%09%7B%0A%09%09angleControl.enable();%0A%09%09Repository.TankDrive.pidWrite(angleControl.get());%0A%09%7D%0A%09%0A%09public synchronized void turnOff()%0A%09%7B%0A%09%09angleControl.disable();%0A%09%7D%0A%09%0A%09public synchronized void goToSetpoint(double setpointAngle)%0A%09%7B%0A%09%09angleControl.setSetpoint(setpointAngle);%0A%09%7D%0A%09%0A%09public synchronized void setPID(double p, double i, double d)%0A%09%7B%0A%09%09angleControl.setPID(p, i, d);
 %0A%09%7D%0A

update tank drive