package com.thegongoliers.output.drivetrain.swerve;

import com.thegongoliers.input.odometry.EncoderSensor;
import com.thegongoliers.input.time.Clock;
import com.thegongoliers.output.control.MotionController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;

public class SwerveWheel {

    private final MotorController mDriveMotor;
    private final MotorController mTurnMotor;
    private final EncoderSensor mDriveEncoder;
    private final EncoderSensor mTurnEncoder;
    private final Translation2d mLocation;
    private final MotionController mAngleController;

    private final Clock mClock;

    private double lastTime = 0.0;

    public SwerveWheel(MotorController driveMotor,
                       MotorController turnMotor,
                       EncoderSensor driveEncoder,
                       EncoderSensor turnEncoder,
                       Translation2d location,
                       MotionController angleController,
                       Clock clock) {

        mDriveMotor = driveMotor;
        mTurnMotor = turnMotor;
        mDriveEncoder = driveEncoder;
        mTurnEncoder = turnEncoder;
        mLocation = location;
        mAngleController = angleController;
        mClock = clock;
    }

    public Translation2d getLocation() {
        return mLocation;
    }

    public double getAngle() {
        return mTurnEncoder.getDistance();
    }

    public double getAngularVelocity() {
        return mTurnEncoder.getVelocity();
    }

    public double getDistance() {
        return mDriveEncoder.getDistance();
    }

    public double getVelocity() {
        return mDriveEncoder.getVelocity();
    }

    public SwerveModuleState getState() {
        return new SwerveModuleState(getVelocity(), Rotation2d.fromDegrees(getAngle()));
    }

    public void reset() {
        mDriveEncoder.reset();
        mTurnEncoder.reset();
        mAngleController.reset();
    }

    public void set(double driveSpeed, double angle) {
        double dt = mClock.getTime() - lastTime;
        if (dt > 0.1) {
            dt = 0.01;
        }
        lastTime = mClock.getTime();

        var state = SwerveModuleState.optimize(
                new SwerveModuleState(driveSpeed, Rotation2d.fromDegrees(angle)),
                new Rotation2d(getAngle()));

        mDriveMotor.set(state.speedMetersPerSecond);
        mAngleController.setSetpoint(state.angle.getDegrees());
        mTurnMotor.set(mAngleController.calculate(getAngle(), dt));
    }

}

@@ -795,16 +795,60 @@
 = 0.0;%0A%0A
+    private double mStopThreshold = 0.001;%0A%0A
     publ
@@ -1975,32 +1975,138 @@
 gle()));%0A    %7D%0A%0A
+    public void setStopThreshold(double threshold) %7B%0A        mStopThreshold = Math.abs(threshold);%0A    %7D%0A%0A
     public void 
@@ -2588,24 +2588,144 @@
 Angle()));%0A%0A
+        if (Math.abs(state.speedMetersPerSecond) %3C mStopThreshold) %7B%0A            stop();%0A            return;%0A        %7D%0A%0A
         mDri
@@ -2904,10 +2904,97 @@
 %0A    %7D%0A%0A
+    public void stop() %7B%0A        mDriveMotor.set(0);%0A        mTurnMotor.set(0);%0A    %7D%0A%0A
 %7D%0A

Stop swerve drive wheel if speed is too low