package com.thegongoliers.output.motors;

import com.thegongoliers.annotations.Untested;

import java.util.function.BooleanSupplier;

@Untested
public class LimiterMotorModule implements MotorModule {

    private BooleanSupplier mPositiveLimit;
    private BooleanSupplier mNegativeLimit;

    public LimiterMotorModule(BooleanSupplier positiveLimit, BooleanSupplier negativeLimit) {
        mPositiveLimit = positiveLimit;
        mNegativeLimit = negativeLimit;

        if (mPositiveLimit == null) {
            mPositiveLimit = () -> false;
        }

        if (mNegativeLimit == null) {
            mNegativeLimit = () -> false;
        }
    }

    @Override
    public double run(double currentSpeed, double desiredSpeed, double deltaTime) {
        if (shouldStop(desiredSpeed)) {
            return 0.0;
        }
        return desiredSpeed;
    }

    public boolean isAtPositiveLimit(){
        return mPositiveLimit.getAsBoolean();
    }

    public boolean isAtNegativeLimit(){
        return mNegativeLimit.getAsBoolean();
    }

    private boolean shouldStop(double speed) {
        if (speed > 0 && isAtPositiveLimit()) {
            return true;
        }

        if (speed < 0 && isAtNegativeLimit()) {
            return true;
        }

        return false;
    }
}

@@ -643,24 +643,264 @@
    %7D%0A    %7D%0A%0A
+    public void setNegativeLimit(BooleanSupplier mNegativeLimit) %7B%0A        this.mNegativeLimit = mNegativeLimit;%0A    %7D%0A%0A    public void setPositiveLimit(BooleanSupplier mPositiveLimit) %7B%0A        this.mPositiveLimit = mPositiveLimit;%0A    %7D%0A%0A
     @Overrid

Add setters for limited motor