package first.robot.subsystems;

import java.util.function.DoubleSupplier;

import org.wpilib.command2.Command;
import org.wpilib.command2.SubsystemBase;
import org.wpilib.epilogue.Logged;

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

import first.robot.constants.ClimberConstants;
import first.robot.constants.ClimberConstants.ClimberPositions;

@Logged
public class Climber extends SubsystemBase {
    private SparkMax motor;

    private RelativeEncoder encoder;

    private SparkClosedLoopController controller;

    private ClimberPositions targetPosition;

    public Climber() {
        motor = new SparkMax(
            ClimberConstants.kCANPort,
            ClimberConstants.kMotorCANID, 
            MotorType.kBrushless
        );

        motor.configure(
            ClimberConstants.kMotorConfig, 
            ResetMode.kResetSafeParameters, 
            PersistMode.kPersistParameters
        );

        encoder = motor.getEncoder();

        controller = motor.getClosedLoopController();

        targetPosition = null;
    }

    public double getTargetPositionMeters() {
        if (targetPosition == null) {
            return -1;
        } else {
            return targetPosition.getPositionMeters();
        }
    }

    public double getCurrentPositionMeters() {
        return encoder.getPosition().get();
    }

    public boolean isAtSetpoint() {
        return controller.isAtSetpoint().get();
    }

    public Command maintainPosition(ClimberPositions position) {
        return run(() -> {
            targetPosition = position;

            controller.setSetpoint(
                position.getPositionMeters(), 
                ControlType.kPosition
            );
        });
    }

    public Command manualSpeed(DoubleSupplier speed) {
        return run(() -> {
            targetPosition = null;

            motor.setThrottle(speed.getAsDouble());
        });
    }

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