package com.thegongoliers.output.drivetrain;

import com.thegongoliers.math.GMath;

/**
 * A drivetrain module which will force the drivetrain to accelerate slower. 
 */
public class PowerEfficiencyModule extends BaseDriveModule {

    /**
     * The ramping time in seconds from 0 to full speed
     * Type: double
     */
    public static final String VALUE_RAMPING_TIME = "seconds_to_full_speed";

    /**
     * The maximum difference between the two wheel speeds to run the power efficiency module on. Defaults to 2.
     * Type: double
     */
    public static final String VALUE_TURN_THRESHOLD = "turn_threshold";

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

    private static final double DEFAULT_TURN_THRESHOLD = 2.0;

    /**
     * Default constructor
     * @param secondsToReachFullSpeed the ramping time in seconds from 0 to full speed
     * @param turnThreshold the maximum difference between the two wheel speeds to run the power efficiency module on. Defaults to 2.
     */
    public PowerEfficiencyModule(double secondsToReachFullSpeed, double turnThreshold){
        super();

        if (secondsToReachFullSpeed < 0) throw new IllegalArgumentException("Seconds to reach full speed must be non-negative");
        if (turnThreshold < 0) throw new IllegalArgumentException("Turn threshold must be non-negative");

        values.put(VALUE_RAMPING_TIME, secondsToReachFullSpeed);
        values.put(VALUE_TURN_THRESHOLD, turnThreshold);
    }

    /**
     * Default constructor
     * @param secondsToReachFullSpeed the ramping time in seconds from 0 to full speed
     */
    public PowerEfficiencyModule(double secondsToReachFullSpeed){
        this(secondsToReachFullSpeed, DEFAULT_TURN_THRESHOLD);
    }

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

        double maximumRate = getMaxRate(deltaTime);
        return applyRateLimit(currentSpeed, desiredSpeed, maximumRate);
    }

    private boolean shouldApplyRateLimit(DriveSpeed speed) {
        double threshold = (double) getValue(VALUE_TURN_THRESHOLD);
        return Math.abs(speed.getLeftSpeed() - speed.getRightSpeed()) >= threshold;
    }

    private double getMaxRate(double deltaTime) {
        double strength = (double) getValue(VALUE_RAMPING_TIME);
        return strength == 0 ? 1 : deltaTime / strength;
    }

    private DriveSpeed applyRateLimit(DriveSpeed lastSpeed, DriveSpeed desiredSpeed, double maxRate){
        double leftSpeed = GMath.rateLimit(maxRate, desiredSpeed.getLeftSpeed(), lastSpeed.getLeftSpeed());
        double rightSpeed = GMath.rateLimit(maxRate, desiredSpeed.getRightSpeed(), lastSpeed.getRightSpeed());
        return new DriveSpeed(leftSpeed, rightSpeed);
    }

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

}
@@ -1154,25 +1154,24 @@
 uper();%0A
-%0A
         
 if (seco
@@ -1162,19 +1162,30 @@
         
-if 
+setRampingTime
 (seconds
@@ -1204,126 +1204,526 @@
 peed
- %3C 0) throw new IllegalArgumentException(%22Seconds to reach full speed must be non-negative%22);%0A        if (turnThreshol
+);%0A        setTurnThreshold(turnThreshold);%0A    %7D%0A%0A    /**%0A     * Default constructor%0A     * @param secondsToReachFullSpeed the ramping time in seconds from 0 to full speed%0A     */%0A    public PowerEfficiencyModule(double secondsToReachFullSpeed)%7B%0A        this(secondsToReachFullSpeed, DEFAULT_TURN_THRESHOLD);%0A    %7D%0A%0A    /**%0A     * @param secondsToReachFullSpeed the ramping time in seconds from 0 to full speed%0A     */%0A    public void setRampingTime(double secondsToReachFullSpeed)%7B%0A        if (secondsToReachFullSpee
 d %3C 
@@ -1765,21 +1765,34 @@
 on(%22
-Turn threshol
+Seconds to reach full spee
 d mu
@@ -1813,17 +1813,16 @@
 tive%22);%0A
-%0A
         
@@ -1886,324 +1886,377 @@
     
-    values.put(VALUE_TURN_THRESHOLD, turnThreshold);%0A    %7D%0A%0A    /**%0A     * Default constructor%0A     * @param secondsToReachFullSpeed the ramping time in seconds from 0 to full speed%0A     */%0A    public PowerEfficiencyModule(double secondsToReachFullSpeed)%7B%0A        this(secondsToReachFullSpeed, DEFAULT_TURN_THRESHOLD
+%7D%0A%0A    /**%0A     * @param turnThreshold the maximum difference between the two wheel speeds to run the power efficiency module on. Defaults to 2.%0A     */%0A    public void setTurnThreshold(double turnThreshold)%7B%0A        if (turnThreshold %3C 0) throw new IllegalArgumentException(%22Turn threshold must be non-negative%22);%0A        values.put(VALUE_TURN_THRESHOLD, turnThreshold
 );%0A 

Add methods to set ramping time and turn threshold to power efficiency module