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 Dampener {
    private final Hardware hardware;

    // dampening
    private double[] posPowerHist;
    private double[] yawPowerHist;

    // accel limiting
    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 Dampener(Hardware hardware) {
        this.hardware = hardware;

        this.posPowerHist = new double[4];
        this.yawPowerHist = new double[4];

        this.posPower = 0;
        this.yawPower = 0;
        this.lastUpdateTime = System.nanoTime();
    }

    private static double shiftBackAverage(double[] arr, double x) {
        double total = 0;
        for (int i = 0; i < arr.length - 1; i++) {
            total += arr[i] = arr[i + 1];
        }
        total += arr[arr.length - 1] = x;
        return total / arr.length;
    }

    public void move(double powerX, double powerY, double turn, double speed) {
        // we like to work with the magnitude of the position velocity vector instead of the individual components
        double hypot = Math.hypot(powerX, powerY);
        double powerXNormalized = powerX / hypot;
        double powerYNormalized = powerY / hypot;

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

        // dampen
        double posPower = shiftBackAverage(this.posPowerHist, hypot);
        double yawPower = shiftBackAverage(this.yawPowerHist, turn);

        // limit accel
        double posAccelLimit = MAX_POS_ACCELERATION * dt;
        double yawAccelLimit = MAX_YAW_ACCELERATION * dt;
        this.posPower += Range.clip(posPower - this.posPower, -posAccelLimit, posAccelLimit);
        this.yawPower += Range.clip(yawPower - this.yawPower, -yawAccelLimit, yawAccelLimit);

        // then we finally move
        this.hardware.move(powerXNormalized * this.posPower, powerYNormalized * this.posPower, this.yawPower, speed);
    }

    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();

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

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

        if (Math.abs(this.posPower) > posLimit || Math.abs(this.yawPower) > yawLimit) {
            hardware.opMode.telemetry.log().add("warning: robot is likely experiencing deceleration slippage");
        }
    }
}
