package frc.robot.commands;

import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.PIDController;

import frc.robot.Constants.Algae;
import frc.robot.subsystems.AlgaeArmSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import java.util.function.DoubleSupplier;

public class AlgaeA extends Command {
    private final PIDController pidController;
    private final AlgaeArmSubsystem algaeArmSubsystem;
    private final DoubleSupplier setpointSupplier;

    public AlgaeA(AlgaeArmSubsystem algaeArmSubsystem) {
        this.algaeArmSubsystem = algaeArmSubsystem;
        this.pidController = new PIDController(
            Algae.AlgaeP,
            Algae.AlgaeI,
            Algae.AlgaeD
        );
        this.setpointSupplier = () -> Algae.AlgaeDP;

        addRequirements(algaeArmSubsystem);
        pidController.setTolerance(
            Algae.Algae_POSITION_TOLERANCE,
            Algae.Algae_VELOCITY_TOLERANCE
        );
    }

    @Override
    public void initialize() {
        pidController.reset();
    }

    @Override
    public void execute() {
        double measurement = algaeArmSubsystem.getPosition();
        double setpoint = setpointSupplier.getAsDouble();
        double output = pidController.calculate(measurement, setpoint);

        ElevatorFeedforward feedforward = new ElevatorFeedforward(
            Algae.AlgaeS,
            Algae.AlgaeG,
            Algae.AlgaeV,
            Algae.AlgaeA
        );
        double feedforwardOutput = feedforward.calculate(algaeArmSubsystem.getVelocity());
        algaeArmSubsystem.setRaw(output + feedforwardOutput);
    }

    @Override
    public boolean isFinished() {
        return false;
    }

    @Override
    public void end(boolean interrupted) {
        this.algaeArmSubsystem.stop();
}
}