package frc.robot.commands;

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

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

public class L2 extends Command {
    private final PIDController pidController;
    private final ElevatorSubsystem elevatorSubsystem;
    private final DoubleSupplier setpointSupplier;

    public L2(ElevatorSubsystem elevatorSubsystem) {
        this.elevatorSubsystem = elevatorSubsystem;
        this.pidController = new PIDController(
            Elevator.ElevatorP,
            Elevator.ElevatorI,
            Elevator.ElevatorD
        );
        this.setpointSupplier = () -> Elevator.L2;

        addRequirements(elevatorSubsystem);
        pidController.setTolerance(
            Elevator.Elevator_POSITION_TOLERANCE,
            Elevator.Elevator_VELOCITY_TOLERANCE
        );
    }

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

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

        ElevatorFeedforward feedforward = new ElevatorFeedforward(
            Elevator.ElevatorS,
            Elevator.ElevatorG,
            Elevator.ElevatorV,
            Elevator.ElevatorA
        );
        double feedforwardOutput = feedforward.calculate(elevatorSubsystem.getVelocity());
        elevatorSubsystem.setRaw(output + feedforwardOutput);
    }

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

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