package frc.robot.subsystems;

import java.util.function.DoubleSupplier;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ArmConstants;

public class Arm extends SubsystemBase {
    private VictorSP motor;

    private AnalogPotentiometer potentiometer;

    public Arm() {
        motor = new VictorSP(ArmConstants.kMotorPWM);

        potentiometer = new AnalogPotentiometer(
            ArmConstants.kPotentiometerAnalogIn, 
            ArmConstants.kPotentiometerMaxAngle
        );
    }

    public double getArmAngle() {
        return potentiometer.get();
    }

    public Command setSpeed(DoubleSupplier speed){
        return run(() -> {
            motor.set(speed.getAsDouble());
        });
    }

    public Command goToAngle(DoubleSupplier speed, double angle, double timeout) {
        boolean shouldNegate = potentiometer.get() > angle;

        return setSpeed(() -> {
            if (shouldNegate) {
                return -1 * Math.abs(speed.getAsDouble());
            } else {
                return Math.abs(speed.getAsDouble());
            }
        })
        .until(() -> MathUtil.isNear(angle, potentiometer.get(), 3))
        .withTimeout(timeout);
    }

    public Command stop() {
        return setSpeed(() -> 0);
    }
}
