package frc.robot.subsystems;

import java.util.function.DoubleSupplier;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ElevatorConstants;

public class Elevator extends SubsystemBase {
    private VictorSP motor1;
    private VictorSP motor2;

    private DigitalInput bottomBeamBreak;
    private DigitalInput topBeamBreak;

    public Elevator() {
        motor1 = new VictorSP(ElevatorConstants.kMotor1PWM);
        motor2 = new VictorSP(ElevatorConstants.kMotor2PWM);

        motor1.addFollower(motor2);

        bottomBeamBreak = new DigitalInput(ElevatorConstants.kBottomBeamBreakDIO);
        topBeamBreak = new DigitalInput(ElevatorConstants.kTopBeamBreakDIO);
    }

    public boolean getBottomBeamBreak() {
        return bottomBeamBreak.get();
    }

    public boolean getTopBeamBreak() {
        return topBeamBreak.get();
    }

    public Command setSpeed(DoubleSupplier speed) {
        return run(() -> {
            double desiredSpeed = speed.getAsDouble();

            if (desiredSpeed < 0) {
                if (bottomBeamBreak.get()) {
                    motor1.set(0);
                } else {
                    motor1.set(desiredSpeed);
                }
            } else {
                if (topBeamBreak.get()) {
                    motor1.set(0);
                } else {
                    motor1.set(desiredSpeed);
                }
            }
        });
    }

    public Command goToBottom(DoubleSupplier speed, double timeout) {
        return setSpeed(() -> -Math.abs(speed.getAsDouble()))
                .until(bottomBeamBreak::get)
                .withTimeout(timeout);
    }

    public Command goToTop(DoubleSupplier speed, double timeout) {
        return setSpeed(() -> Math.abs(speed.getAsDouble()))
                .until(topBeamBreak::get)
                .withTimeout(timeout);
    }

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