package frc.robot.subsystems;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkMax;
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.Feeder;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.PersistMode;
import com.revrobotics.ResetMode;

public class FeederSubsystem extends SubsystemBase {
    private final SparkMax feederMotor = new SparkMax(
        Feeder.MOTOR_ID,
        MotorType.kBrushless
    );
    

    private RelativeEncoder encoder;
        
    private double position;
    private double velocity;
    
    SparkClosedLoopController closedLoopController = feederMotor.getClosedLoopController();
    public FeederSubsystem() {
        this.encoder = this.feederMotor.getEncoder();
        feederMotor.configure(Feeder.MOTORCONFIG, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
    }
    @Override
    public void periodic () {
        this.position = this.encoder.getPosition();
        this.velocity = this.encoder.getVelocity();
        SmartDashboard.putNumber("Feeder relative angle (turns)", this.position);
        SmartDashboard.putNumber("Feeder velocity (rpm)", this.velocity);
        SmartDashboard.putNumber("Feeder power (-1 to 1)", this.feederMotor.get());
        SmartDashboard.putNumber("Feeder setpoint", this.closedLoopController.getSetpoint());
        SmartDashboard.putString("Feeder setpoint units", this.closedLoopController.getControlType().toString());
        SmartDashboard.putBoolean("Feeder at setpoint? (boolean)", this.closedLoopController.isAtSetpoint());
    }
    public void setspeed (double SPEED) {
        this.feederMotor.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);
    }
    public void stop () {
        this.feederMotor.stopMotor();
    }
    public double getVelocity() {
        return this.velocity;
    }
    public double getPosition() {
        return this.position;
    }
    public boolean atSetpoint() {
        return this.closedLoopController.isAtSetpoint();
    }
}