package com.shsrobotics.omicronsquark.subsystems;

import com.shsrobotics.omicronsquark.Maps;
import com.shsrobotics.omicronsquark.commands.DriveWithJoysticks;
import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class DriveTrain extends PIDSubsystem implements Maps {
    
    private Victor frontLeftVictor = new Victor(Robot.Drive.frontLeftWheel);
    private Victor rearLeftVictor = new Victor(Robot.Drive.rearLeftWheel);
    private Victor frontRightVictor = new Victor(Robot.Drive.frontRightWheel);
    private Victor rearRightVictor = new Victor(Robot.Drive.rearRightWheel);

    private RobotDrive robotDrive = new RobotDrive(frontLeftVictor, rearLeftVictor, frontRightVictor, rearRightVictor);
    private Gyro gyroscope = new Gyro(Robot.Drive.gyroscope);
	
	private double userAlignment = 0.0;
    
    public DriveTrain() {    
        super(Robot.Drive.P, Robot.Drive.I, Robot.Drive.D);                
        setInputRange(-359,359);
		this.getPIDController().setOutputRange(-1.0, 1.0);
        gyroscope.setSensitivity(Robot.Drive.gyroVoltsPerDegreeSecond); 	
        setAbsoluteTolerance(Constants.significanceLevel_Angle); 
		getPIDController().reset();
    }

    public void stop() {        
        disable();
		robotDrive.stopMotor();
    }
    
    public void drive(double x, double y, double z, boolean useGyro) {
        disable(); // disables PID
        x = MathUtils.pow(x, 3);
        y = MathUtils.pow(y, 3);
        z = MathUtils.pow(z, 3);
		if (Math.abs(z) < Constants.joystickThreshold) {
			z = 0.0;
		}
		double angle = getGyroAngle();
        double gyroAngle = useGyro ? angle : 0.0;
        robotDrive.mecanumDrive_Cartesian(x, y, z, gyroAngle);
    }

    public void rotateTo(double angle) { // in degrees
        setSetpoint(angle);
        enable();
    }
    
    public void reset() {
        gyroscope.reset();
    }    
	
	public void setUserAlignment(double y) {
		userAlignment = y;
	}


    protected void usePIDOutput(double output) {
        robotDrive.mecanumDrive_Cartesian(0.0, userAlignment, output, 0.0);
    }

    protected double returnPIDInput() {
        double angle = getGyroAngle();
        SmartDashboard.putNumber("GYRO ANGLE", angle);
        return angle;
    }
    
    public double getGyroAngle() {
        return (gyroscope.getAngle() % 360);
    }
	
	public void disable() {
		this.getPIDController().reset();
	}
    
    public void initDefaultCommand() {
        setDefaultCommand(new DriveWithJoysticks());
    }	
}
@@ -296,21 +296,21 @@
 wpilibj.
-Victo
+Jagua
 r;%0Aimpor
@@ -492,29 +492,29 @@
     private 
-Victo
+Jagua
 r frontLeftV
@@ -508,37 +508,37 @@
 ar frontLeft
-Victo
+Jagua
 r = new 
 Victor(Robot
@@ -517,37 +517,37 @@
 eftJaguar = new 
-Victo
+Jagua
 r(Robot.Drive.fr
@@ -569,29 +569,29 @@
     private 
-Victo
+Jagua
 r rearLeftVi
@@ -588,29 +588,29 @@
 rearLeft
-Victo
+Jagua
 r = new 
 Victor(R
@@ -593,37 +593,37 @@
 eftJaguar = new 
-Victo
+Jagua
 r(Robot.Drive.re
@@ -648,21 +648,21 @@
 private 
-Victo
+Jagua
 r frontR
@@ -661,37 +661,37 @@
 r frontRight
-Victo
+Jagua
 r = new 
 Victor(Robot
@@ -670,37 +670,37 @@
 ghtJaguar = new 
-Victo
+Jagua
 r(Robot.Drive.fr
@@ -727,21 +727,21 @@
 private 
-Victo
+Jagua
 r rearRi
@@ -747,21 +747,21 @@
 ight
-Victo
+Jagua
 r = new 
 Vict
@@ -756,21 +756,21 @@
 r = new 
-Victo
+Jagua
 r(Robot.
@@ -853,21 +853,21 @@
 rontLeft
-Victo
+Jagua
 r, rearL
@@ -869,21 +869,21 @@
 rearLeft
-Victo
+Jagua
 r, front
@@ -887,21 +887,21 @@
 ontRight
-Victo
+Jagua
 r, rearR
@@ -908,13 +908,13 @@
 ight
-Victo
+Jagua
 r);%0A

changed to jaguars