package com.edinarobotics.zephyr.parts;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Relay;

/**
 *The wrapper for the shooter components, contains the 2 jaguars driving the shooter
 * along with the rotating jaguar and the piston.
 */
public class ShooterComponents {
    public static final int ROTATE_RIGHT_SIGN = 1;
    public static final int ROTATE_LEFT_SIGN = -1;
    
    private Jaguar shooterLeftJaguar;
    private Jaguar shooterRightJaguar;
    private Jaguar shooterRotator;
    private Relay ballLoadPiston;
    private DigitalInput leftLimitSwitch;
    private DigitalInput rightLimitSwitch;
    private Encoder encoder;
    /*
     * Constructs shooterLeftJaguar, shooterRightJaguar, shooterRotator and ballLoadPiston
     * with leftJaguar, rightJaguar, rotator and piston respectively.
     */
    public ShooterComponents(int leftJaguar, int rightJaguar, int rotator, int piston,
                             int leftLimitSwitch, int rightLimitSwitch, int encoderA, int encoderB){
        shooterLeftJaguar = new Jaguar(leftJaguar);
        shooterRightJaguar = new Jaguar(rightJaguar);
        shooterRotator = new Jaguar(rotator);
        ballLoadPiston = new Relay(piston);
        this.leftLimitSwitch = new DigitalInput(leftLimitSwitch);
        this.rightLimitSwitch = new DigitalInput(rightLimitSwitch);
        this.encoder = new Encoder(encoderA, encoderB);
        encoder.setReverseDirection(true);
        encoder.start();
    }
    /*
     * sets the shooterLeftJaguar to speed and shooterRightJaguar to -speed
     */
    public void setSpeed(double speed){
        shooterLeftJaguar.set(-speed);
        shooterRightJaguar.set(speed);
    }
    /*
     * Sets the rotator to speed
     */
    public void rotate(double speed){
        if((sgn(speed) == ROTATE_LEFT_SIGN && leftLimitSwitch.get()) || 
           (sgn(speed) == ROTATE_RIGHT_SIGN) && rightLimitSwitch.get()){
            //Limit switches are pressed, stop rotating.
            shooterRotator.set(0);
            return;
        }
        shooterRotator.set(speed);
    }
    /*
     * Sets the piston up if position is true, else it lowers it.
     */
    public void firePiston(boolean position){
        ballLoadPiston.set((position ? Relay.Value.kForward :Relay.Value.kReverse));
    }
    
    /**
     * Returns the encoder attached to the shooter.
     * @return The {@link Encoder} object that can be used to access the encoder
     * attached to the shooter.
     */
    public Encoder getEncoder(){
        return encoder;
    }
    
    /**
     * A very simple implementation of the sgn function. Returns
     * {@code 1},{@code -1} or {@code 0} representing the sign of the number.
     * @param num The number whose sign will be returned.
     * @return {@code -1} if the number is negative,
     * {@code 0} if the number is 0, or {@code 1} if the number is positive.
     */
    private int sgn(double num){
        if(num>0){
            return 1;
        }
        else if(num<0){
            return -1;
        }
        return 0;
    }
}

@@ -45,116 +45,111 @@
 ort 
-edu.wpi.first.wpilibj.DigitalInput;%0Aimport edu.wpi.first.wpilibj.Encoder;%0Aimport edu.wpi.first.wpilibj.Jagua
+com.edinarobotics.utils.sensors.FilterDouble;%0Aimport com.edinarobotics.utils.sensors.SimpleAverageFilte
 r;%0Ai
@@ -176,21 +176,17 @@
 wpilibj.
-Relay
+*
 ;%0A%0A/**%0A 
@@ -353,17 +353,16 @@
 mponents
- 
 %7B%0A    pu
@@ -718,24 +718,57 @@
 er encoder;%0A
+    private FilterDouble filter;%0A
     /*%0A     
@@ -1569,13 +1569,108 @@
 er.s
-tart(
+etDistancePerPulse(1.0/180.0);%0A        encoder.start();%0A        filter = new SimpleAverageFilter(200
 );%0A 
@@ -2021,16 +2021,18 @@
 SIGN && 
+!(
 leftLimi
@@ -2045,16 +2045,17 @@
 h.get())
+)
  %7C%7C %0A   
@@ -2099,16 +2099,18 @@
 IGN) && 
+!(
 rightLim
@@ -2124,16 +2124,17 @@
 h.get())
+)
 %7B%0A      
@@ -2754,32 +2754,129 @@
 der;%0A    %7D%0A    %0A
+    public double getEncoderValue()%7B%0A        return filter.filter(encoder.getRate());%0A    %7D%0A    %0A
     /**%0A     * A

Add encoder value filtering to ShooterComponents.