package frc.robot.subsystems;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.RelativeEncoder;

import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.Shooter;
import com.revrobotics.spark.SparkBase.ControlType;

import java.util.function.DoubleSupplier;

import com.revrobotics.PersistMode;
import com.revrobotics.ResetMode;

public class ShooterSubsystem extends SubsystemBase {
    private final SparkFlex shooterMotor = new SparkFlex(
        Shooter.MOTOR_ID,
        MotorType.kBrushless
    );
    private final SparkFlex shooterMotorSLAVE = new SparkFlex(
        Shooter.MOTOR_IDSLAVE,
        MotorType.kBrushless
    );
    

    private RelativeEncoder encoder;
        
    private double position;
    private double velocity;
    
    SparkClosedLoopController closedLoopController = shooterMotor.getClosedLoopController();
    public ShooterSubsystem() {
        this.encoder = this.shooterMotor.getEncoder();
        shooterMotor.configure(Shooter.MOTORCONFIG.inverted(true), ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
        shooterMotorSLAVE.configure(Shooter.MOTORCONFIG.follow(shooterMotor, true), ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
    }
    @Override
    public void periodic () {
        this.position = this.encoder.getPosition();
        this.velocity = this.encoder.getVelocity();
        SmartDashboard.putNumber("Shooter relative angle (turns)", this.position);
        SmartDashboard.putNumber("Shooter velocity (rpm)", this.velocity);
        SmartDashboard.putNumber("Shooter power (-1 to 1)", this.shooterMotor.get());
        SmartDashboard.putNumber("Shooter setpoint", this.closedLoopController.getSetpoint());
        SmartDashboard.putString("Shooter setpoint units", this.closedLoopController.getControlType().toString());
        SmartDashboard.putBoolean("Shooter at setpoint? (boolean)", this.closedLoopController.isAtSetpoint());
    }
    public void setspeed (double SPEED) {
        this.shooterMotor.set(SPEED);
    }
    public void setwatts (double WATTS) { // set the motor to use the input watts of power watts instead of amps because in theory it is voltage independent but maybe not? because current usually equals torque but voltage usually equals speed so as the battery voltage goes down it would mean that with this equation the torque would go up while the zero load speed would go down which is probably fine?
        closedLoopController.setSetpoint(WATTS/RobotController.getBatteryVoltage(), ControlType.kCurrent); // watts/voltage = current
    }
    public void setrpm (double RPM) { // set the motor to spin at the input number of rpm
        closedLoopController.setSetpoint(RPM, ControlType.kVelocity);
    } // rpm = Shooter.kFlywheelAutoAimPolynomial.predict(distance);
    public void setDistance (DoubleSupplier distanceMeters) {
        setrpm(Shooter.kFlywheelAutoAimPolynomial.predict(distanceMeters.getAsDouble()));
    }
    public void stop () {
        this.shooterMotor.stopMotor();
    }
    public double getVelocity() {
        return this.velocity;
    }
    public double getPosition() {
        return this.position;
    }
    public boolean atSetpoint() {
        return this.closedLoopController.isAtSetpoint();
    }
}