package frc.robot.subsystems;

import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;

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

    private BangBangController bangBangController;
    private SimpleMotorFeedforward motorFeedforward;

    private Encoder frontMotorEncoder;
    private Encoder rearMotorEncoder;

    public Shooter() {
        frontMotor = new WPI_VictorSPX(ShooterConstants.kFrontMotorCAN);
        rearMotor = new WPI_VictorSPX(ShooterConstants.kRearMotorCAN);

        rearMotor.follow(frontMotor);

        bangBangController = new BangBangController(ShooterConstants.kBangBangTolerance);

        motorFeedforward = new SimpleMotorFeedforward(
            ShooterConstants.kFFS, 
            ShooterConstants.kFFG,
            ShooterConstants.kFFV
        );

        frontMotorEncoder = new Encoder(
            ShooterConstants.kFrontEncoderA, 
            ShooterConstants.kFrontEncoderB
        );
        frontMotorEncoder.setReverseDirection(ShooterConstants.kInvertFrontEncoder);
        frontMotorEncoder.setDistancePerPulse(ShooterConstants.kDistancePerPulse);

        rearMotorEncoder = new Encoder(
            ShooterConstants.kRearEncoderA, 
            ShooterConstants.kRearEncoderB
        );
        rearMotorEncoder.setReverseDirection(ShooterConstants.kInvertRearEncoder);
        rearMotorEncoder.setDistancePerPulse(ShooterConstants.kDistancePerPulse);
    }

    public double getFrontEncoder() {
        return frontMotorEncoder.getRate();
    }

    public double getRearEncoder() {
        return rearMotorEncoder.getRate();
    }

    public boolean shooterAtSpeed() {
        return (frontMotorEncoder.getRate() + rearMotorEncoder.getRate()) / 2 >= ShooterConstants.kShooterFireSpeed;
    }

    public Command runAutoShooter() {
        return run(() -> {
            double output = bangBangController.calculate(
                (frontMotorEncoder.getRate() + rearMotorEncoder.getRate()) / 2,
                ShooterConstants.kShooterFireSpeed
            ) + motorFeedforward.calculate(ShooterConstants.kShooterFireSpeed);

            frontMotor.setVoltage(output);
        });
    }

    public Command runShooter(double speed) {
        return run(() -> {
            frontMotor.set(speed);
        });
    }

    public Command stopShooter() {
        return run(() -> {
            frontMotor.set(0);
        });
    }
}