package com.thegongoliers.output.actuators;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.SpeedController;

public class GSpeedController implements SpeedController {

    private SpeedController mSpeedController;
    private Encoder mEncoder;
    private double mDistanceCorrectionStrength, mVelocityCorrectionStrength;

    /**
     * A speed controller with added functionality
     * @param speedController the underlying speed controller
     * @param encoder the encoder which senses the movement of the motor controlled by the speed controller
     * @param distanceCorrectionStrength the correction strength for setting a distance (ex. 0.1 when distance is in feet)
     * @param velocityCorrectionStrength the correction strength for setting a velocity (ex. 1 / max velocity)
     */
    public GSpeedController(SpeedController speedController, Encoder encoder, double distanceCorrectionStrength, double velocityCorrectionStrength){
        mSpeedController = speedController;
        mEncoder = encoder;
        mDistanceCorrectionStrength = distanceCorrectionStrength;
        mVelocityCorrectionStrength = velocityCorrectionStrength;
    }

    @Override
    public void pidWrite(double output) {
        mSpeedController.pidWrite(output);
    }

    @Override
    public void set(double speed) {
        mSpeedController.set(speed);
    }

    /**
     * @param velocity the velocity of the motor in encoder units / second
     */
    public void setVelocity(double velocity){
        double error = velocity - getVelocity();
        System.out.println(mSpeedController.get() + error * mVelocityCorrectionStrength);
        set(mSpeedController.get() + error * mVelocityCorrectionStrength);
    }

    /**
     * @return the velocity of the motor in encoder units / second
     */
    public double getVelocity(){
        return mEncoder.getRate();
    }

    /**
     * @param distance the distance of the motor in encoder units
     */
    public void setDistance(double distance){
        double error = distance - getDistance();
        set(error * mDistanceCorrectionStrength);
    }

    /**
     * @return the distance of the motor in encoder units
     */
    public double getDistance(){
        return mEncoder.getDistance();
    }

    /**
     * Resets the distance on the encoder
     */
    public void resetDistance(){
        mEncoder.reset();
    }

    @Override
    public double get() {
        return mSpeedController.get();
    }

    @Override
    public void setInverted(boolean isInverted) {
        mSpeedController.setInverted(isInverted);
    }

    @Override
    public boolean getInverted() {
        return mSpeedController.getInverted();
    }

    @Override
    public void disable() {
        mSpeedController.disable();
    }

    @Override
    public void stopMotor() {
        mSpeedController.stopMotor();
    }

}
@@ -38,16 +38,129 @@
 ators;%0A%0A
+import com.thegongoliers.input.odometry.DistanceSensor;%0Aimport com.thegongoliers.input.odometry.VelocitySensor;%0A%0A
 import e
@@ -234,16 +234,71 @@
 troller;
+%0Aimport edu.wpi.first.wpilibj.interfaces.Potentiometer;
 %0A%0Apublic
@@ -413,23 +413,81 @@
 ate 
-Encoder mEncode
+DistanceSensor mDistanceSensor;%0A    private VelocitySensor mVelocitySenso
 r;%0A 
@@ -1195,69 +1195,1695 @@
     
-mSpeedController = speedController;%0A        mEncoder = encode
+this(speedController, encoder::getDistance, encoder::getRate, distanceCorrectionStrength, velocityCorrectionStrength);%0A    %7D%0A%0A    /**%0A     * A speed controller with added functionality%0A     * Potentiometers can not use velocity control!%0A     * @param speedController the underlying speed controller%0A     * @param potentiometer the potentiometer which senses the movement of the motor controlled by the speed controller%0A     * @param distanceCorrectionStrength the correction strength for setting a distance (ex. 0.1 when distance is in feet)%0A     * @param velocityCorrectionStrength the correction strength for setting a velocity (ex. 1 / max velocity)%0A     */%0A    public GSpeedController(SpeedController speedController, Potentiometer potentiometer, double distanceCorrectionStrength, double velocityCorrectionStrength)%7B%0A        this(speedController, potentiometer::get, () -%3E 0.0, distanceCorrectionStrength, velocityCorrectionStrength);%0A    %7D%0A%0A    /**%0A     * A speed controller with added functionality%0A     * @param speedController the underlying speed controller%0A     * @param distanceSensor%0A     * @param velocitySensor%0A     * @param distanceCorrectionStrength the correction strength for setting a distance (ex. 0.1 when distance is in feet)%0A     * @param velocityCorrectionStrength the correction strength for setting a velocity (ex. 1 / max velocity)%0A     */%0A    public GSpeedController(SpeedController speedController, DistanceSensor distanceSensor, VelocitySensor velocitySensor, double distanceCorrectionStrength, double velocityCorrectionStrength)%7B%0A        mSpeedController = speedController;%0A        mDistanceSensor = distanceSensor;%0A        mVelocitySensor = velocitySenso
 r;%0A 
@@ -3714,23 +3714,34 @@
 rn m
-Encoder.getRate
+VelocitySensor.getVelocity
 ();%0A
@@ -4108,151 +4108,34 @@
 rn m
-Encoder.getDistance();%0A    %7D%0A%0A    /**%0A     * Resets the distance on the encoder%0A     */%0A    public void resetDistance()%7B%0A        mEncoder.reset
+DistanceSensor.getDistance
 ();%0A

Allow GSpeedController to use a potentiometer