package com.thegongoliers.output.drivetrain;

import com.thegongoliers.input.time.Clock;
import com.thegongoliers.input.time.RobotClock;

import edu.wpi.first.wpilibj.interfaces.Gyro;

/**
 * A drivetrain module which will stabilize the drivetrain (rotation-wise) while attempting to drive straight
 */
public class StabilityModule extends BaseDriveModule {

    /**
     * The gyroscope which determines the robot's heading
     * Type: edu.wpi.first.wpilibj.interfaces.Gyro
     */
    public static final String VALUE_GYRO = "gyro";

    /**
     * The stabilizing strength (higher values may become unstable, recommended ~0.02. Values must be >= 0).
     * Type: double
     */
    public static final String VALUE_STRENGTH = "strength";

    /**
     * The amount of time to allow the drivetrain to settle after turn inputs stop before applying turn corrections.
     * Type: double
     */
    public static final String VALUE_SETTLING_TIME = "settling_time";

    /**
     * The input threshold for turning to activate the stability module (between 0 and 1, defaults to 0)
     * Type: double
     */
    public static final String VALUE_THRESHOLD = "threshold";

    /**
     * The clock to use to calculate the settling time (defaults to the robot's clock).
     * Type: com.thegongoliers.input.time.Clock
     */
    public static final String VALUE_CLOCK = "clock";

    /**
     * The name of the module
     */
    public static final String NAME = "Stability";

    private double lastHeading;
    private double lastStopTime;

    /**
     * Default constructor
     * @param gyro the gyroscope which determines the robot's heading
     * @param strength the stabilizing strength (higher values may become unstable, recommended ~0.02. Values must be >= 0)
     * @param settlingTime the amount of time to allow the drivetrain to settle after turn inputs stop before applying turn corrections
     */
    public StabilityModule(Gyro gyro, double strength, double settlingTime){
        super();
        values.put(VALUE_GYRO, gyro);
        values.put(VALUE_STRENGTH, strength);
        values.put(VALUE_SETTLING_TIME, settlingTime);
        values.put(VALUE_THRESHOLD, 0.01);
        values.put(VALUE_CLOCK, new RobotClock());

        lastHeading = gyro.getAngle();
        lastStopTime = 0;
    }

    @Override
    public DriveSpeed run(DriveSpeed currentSpeed, DriveSpeed desiredSpeed, double deltaTime) {
        double strength = (double) getValue(VALUE_STRENGTH);
        Gyro gyro = (Gyro) getValue(VALUE_GYRO);
        double settlingTime = (double) getValue(VALUE_SETTLING_TIME);
        double threshold = (double) getValue(VALUE_THRESHOLD);
        Clock clock = (Clock) getValue(VALUE_CLOCK);

        double left = desiredSpeed.getLeftSpeed();
        double right = desiredSpeed.getRightSpeed();

        if (Math.abs(left - right) > threshold){
            // Turning
            lastHeading = gyro.getAngle();
            lastStopTime = clock.getTime();
        } else {
            // Driving straight
            if (clock.getTime() - lastStopTime < settlingTime){
                lastHeading = gyro.getAngle();
            }
            double forward = (left + right) / 2;
            double turn = strength * (lastHeading - gyro.getAngle());
            return DriveSpeed.fromArcade(forward, turn);
        }

        return new DriveSpeed(left, right);
    }

    @Override
    public String getName() {
        return NAME;
    }

}
@@ -2430,179 +2430,344 @@
     
-double strength = (double) getValue(VALUE_STRENGTH);%0A        Gyro gyro = (Gyro) getValue(VALUE_GYRO);%0A        double settlingTime = (double) getValue(VALUE_SETTLING_TIME);
+if (isTurning(desiredSpeed))%7B%0A            updateDesiredHeading();%0A            updateStopTime();%0A        %7D else if (isSettling())%7B%0A            updateDesiredHeading();%0A        %7D else %7B%0A            return getCorrectedDriveSpeed(desiredSpeed);%0A        %7D%0A%0A        return desiredSpeed;%0A    %7D%0A%0A    private boolean isTurning(DriveSpeed speed) %7B
 %0A   
@@ -2794,20 +2794,17 @@
 d = 
-(d
+getD
 ouble
-) get
 Valu
@@ -2835,136 +2835,48 @@
     
-Clock clock = (Clock) getValue(VALUE_CLOCK);%0A%0A        double left = desiredSpeed.getLeftSpeed();%0A        double right = desiredS
+return Math.abs(speed.getLeftSpeed() - s
 peed
@@ -2895,87 +2895,129 @@
 ed()
+) %3E threshold
 ;%0A
-%0A
     
+%7D%0A%0A
     
-if (Math.abs(left - right) %3E threshold)%7B%0A            // Turning%0A
+private void updateDesiredHeading() %7B%0A        Gyro gyro = (Gyro) getValue(VALUE_GYRO);    
     
+%0A
     
@@ -3047,32 +3047,124 @@
 getAngle();%0A    
+%7D%0A%0A    private void updateStopTime() %7B%0A        Clock clock = (Clock) getValue(VALUE_CLOCK);%0A
         lastStop
@@ -3195,69 +3195,182 @@
     
+%7D%0A%0A
     
-%7D else %7B%0A            // Driving straight%0A            if (
+private boolean isSettling() %7B%0A        double settlingTime = getDoubleValue(VALUE_SETTLING_TIME);%0A        Clock clock = (Clock) getValue(VALUE_CLOCK);%0A%0A        return 
 cloc
@@ -3414,195 +3414,201 @@
 Time
-)%7B
+;
 %0A    
+%7D%0A%0A
     
-        lastHeading = gyro.getAngle();%0A            %7D%0A            double forward = (left + right) / 2;%0A            double turn = strength * (lastHeading - gyro.getAngle
+private DriveSpeed getCorrectedDriveSpeed(DriveSpeed desiredSpeed) %7B%0A        double forward = calculateForwardSpeed(desiredSpeed);%0A        double turn = calculateTurnCorrection
 ()
-)
 ;%0A
-    
     
@@ -3664,56 +3664,356 @@
     
+%7D%0A%0A
     
-%7D%0A%0A        return new DriveSpeed(left, right
+private double calculateForwardSpeed(DriveSpeed speed) %7B%0A        return (speed.getLeftSpeed() + speed.getRightSpeed()) / 2;%0A    %7D%0A%0A    private double calculateTurnCorrection() %7B%0A        double strength = getDoubleValue(VALUE_STRENGTH);%0A        Gyro gyro = (Gyro) getValue(VALUE_GYRO);%0A        return strength * (lastHeading - gyro.getAngle()
 );%0A 

Refactor stability module