package frc.robot.subsystems;

import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

import static frc.robot.constants.MotorIDsConstants.*;
import static frc.robot.constants.ClosedLoopTestConstants.*;

public class ClosedLoopTest extends SubsystemBase {
    SparkMax motor;

    SparkClosedLoopController controller;

    RelativeEncoder encoder;

    public ClosedLoopTest() {
        motor = new SparkMax(kMotorClosedLoopTestCANID, MotorType.kBrushless);

        motor.configure(
            motorConfig, 
            ResetMode.kResetSafeParameters, 
            PersistMode.kPersistParameters
        );

        controller = motor.getClosedLoopController();

        encoder = motor.getEncoder();
        encoder.setPosition(0);
    }

    public Command incrementPosition(double increment) {
        return runOnce(() -> {
            setPosition(encoder.getPosition() + increment);
        });
    }

    public Command setPosition(double position) {
        return runOnce(() -> {
            controller.setReference(
                position, 
                ControlType.kMAXMotionPositionControl, 
                ClosedLoopSlot.kSlot0
            );
        });
    }

    public Command setVelocity(double velocity) {
        return runOnce(() -> {
            controller.setReference(
                velocity, 
                ControlType.kMAXMotionVelocityControl, 
                ClosedLoopSlot.kSlot0
            );
        });
    }

    public double getEncoderPosition() {
        return encoder.getPosition();
    }
}