package org.firstinspires.ftc.teamcode.kinematics;

import com.qualcomm.robotcore.hardware.PIDCoefficients;

public class PID {
    public final PIDCoefficients coefficients;
    private double lastX;
    private double lastUpdateTime;
    private boolean firstIteration;
    private double i;

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

    public PID(PIDCoefficients coefficients) {
        this.coefficients = coefficients;
        this.firstIteration = true;
        this.i = 0;
    }

    public double update(double x) {
        double time = System.nanoTime() / NANOS_PER_SEC;

        double res = this.coefficients.p * x;
        if (!this.firstIteration) {
            double dt = time - this.lastUpdateTime;

            this.i += x * dt;
            double d = (x - lastX) / dt;

            res += this.coefficients.i * this.i;
            res += this.coefficients.d * d;
        } else {
            this.firstIteration = false;
        }

        this.lastX = x;
        this.lastUpdateTime = time;

        return res;
    }
}
