power-play/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/AccelLimiter.java

package org.firstinspires.ftc.teamcode.kinematics;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.util.Range;

import org.firstinspires.ftc.teamcode.Hardware;

@Config
public class AccelLimiter {
    private final Hardware hardware;

    private double posPower;
    private double yawPower;
    private double lastUpdateTime;

    public static double MAX_POS_ACCELERATION = 13;
    public static double MAX_YAW_ACCELERATION = 1;

    private static final double NANOS_PER_SEC = 1000 * 1000 * 1000;

    public AccelLimiter(Hardware hardware) {
        this.hardware = hardware;
        this.posPower = 0;
        this.yawPower = 0;
    }

    public void move(double powerX, double powerY, double turn, double speed) {
        double hypot = Math.hypot(powerX, powerY);

        double time = System.nanoTime() / NANOS_PER_SEC;
        double dt = time - this.lastUpdateTime;

        double posLimit = MAX_POS_ACCELERATION * dt;
        double yawLimit = MAX_YAW_ACCELERATION * dt;

        this.posPower += Range.clip(hypot - this.posPower, -posLimit, posLimit);
        this.yawPower += Range.clip(turn - this.yawPower, -yawLimit, yawLimit);

        this.hardware.move(powerX / hypot * this.posPower, powerY / hypot * this.posPower, this.yawPower, speed);

        this.lastUpdateTime = time;
    }

    public void stop() {
        // yes, yes, i know this isnt good, but for now its only called when power values are low anyway
        this.hardware.stop();
    }
}