package frc.robot.subsystems;

import java.util.function.DoubleSupplier;

import edu.wpi.first.math.controller.BangBangController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.Encoder;
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.ShooterConstants;

public class Shooter extends SubsystemBase {
    private VictorSP motor1;
    private VictorSP motor2;

    private BangBangController controller;

    private SimpleMotorFeedforward ff;

    private Encoder motor1Encoder;
    private Encoder motor2Encoder;

    public Shooter() {
        motor1 = new VictorSP(ShooterConstants.kMotor1PWMID);
        motor2 = new VictorSP(ShooterConstants.kMotor2PWMID);

        motor1.addFollower(motor2);

        controller = new BangBangController(ShooterConstants.kBangBangTolerance);

        ff = new SimpleMotorFeedforward(
            ShooterConstants.kFFS, 
            ShooterConstants.kFFV,
            ShooterConstants.kFFA
        );

        motor1Encoder = new Encoder(
            ShooterConstants.kMotor1EncoderChannelA, 
            ShooterConstants.kMotor1EncoderChannelB
        );
        motor1Encoder.setDistancePerPulse(ShooterConstants.kEncoderConversionFactor);

        motor2Encoder = new Encoder(
            ShooterConstants.kMotor2EncoderChannelA, 
            ShooterConstants.kMotor2EncoderChannelB
        );
        motor2Encoder.setDistancePerPulse(ShooterConstants.kEncoderConversionFactor);
    }

    public Command runAutomaticSpeedControl(double speed) {
        return run(() -> {
            double output = controller.calculate(
                (motor1Encoder.getRate() + motor2Encoder.getRate()) / 2,
                speed
            ) + ff.calculate(speed);

            motor1.setVoltage(output);
        });
    }

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

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