package first;

import edu.wpi.first.wpilibj.*;

public class ShooterMotor extends Motor {
    Joystick conformationJoy;
    Gyro gyro;

    final int manualOverrideButtons = 9;
    final int restButton = 8;

    boolean isManual = false;
    boolean isResting = false;

    final double sensitivityRange = .01;
    final double maxAngle = 80;
    final double minAngle = 0;

    public ShooterMotor(Jaguar duper, Joystick driveJoy, Joystick overrideJoy, Gyro g) {
        super(duper);
        theJoy = driveJoy;
        conformationJoy = overrideJoy;
        gyro = g;
    }

    @Override public void run() {
        while (true) {
            if (!isManual && joy.getRawButton(manualOverrideButtons) && conformationJoy.getRawButton(manualOverrideButtons))
                isManual = true;
            else if (joy.getRawButton(manualOverrideButtons) || conformationJoy.getRawButton(manualOverrideButtons))
                isManual = false;

            if (joy.getRawButton(restButton))
                isResting = !isResting;

            if (isManual) {
                if (Math.toRadians(gyro.getAngle()) - 15 > confirmationJoy.getY()*2*Math.PI+sensitivityRange && gyro.getAngle() >= minAngle)
                    theJag.set(-.2);
                else if (Math.toRadians(gyro.getAngle())-15 < confirmationJoy.getY()*2*Math.PI-sensitivityRange && gyro.getAngle() <= maxAngle)
                    theJag.set(.2);
                else
                    theJag.set(0);
            }

            else if (isResting) {
                if (Math.toRadians(gyro.getAngle()) - 15 > 0 + sensitivityRange && gyro.getAngle() >= minAngle)
                    theJag.set(-.2);
                else
                    theJag.set(0);
            }

            else if (Math.toRadians(gyro.getAngle()) - 15 > datClass.getElevation() + sensitivityRange && gyro.getAngle() >= minAngle)
                theJag.set(-.2);
            else if (Math.toRadians(gyro.getAngle()) - 15 < datClass.getElevation() - sensitivityRange && gyro.getAngle() <= maxAngle)
                theJag.set(.2);
            else
                theJag.set(0);
        }
    }
}