// IntakeRollersSubsystem.java
package frc.robot.subsystems;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.motors.DBugSparkMax;

public class IntakeRollersSubsystem extends SubsystemBase {
    private final DBugSparkMax _motor;

    private static final int MOTOR_CANID = 1;

    public IntakeRollersSubsystem() {
        _motor = DBugSparkMax.create(MOTOR_CANID);
    }

    public State getState() {
        return State.fromValue(_motor.get());
    }

    public void setState(State target) {
        _motor.set(target._value);
    }

    public Command getSetStateCommand(State target) {
        return new FunctionalCommand(
            () -> setState(target),
            () -> {},
            _ignored -> {},
            () -> getState() == target,
            this
        );
    }

    public static enum State {
        IN(1), OUT(-1), OFF(0),;

        private final double _value;

        State(double value) {
            _value = value;
        }

        private static State fromValue(double value) {
            if (value > 0) return IN;
            if (value < 0) return OUT;
            return OFF;
        }
    }
}

// RobotContainer.java
// --snip--
  private final IntakeRollersSubsystem m_intakeRollersSubsystem = new IntakeRollersSubsystem();
// --snip--
  public Command getCollectCommand() {
    return m_elevatorSubsystem
      .setStateCommand(ElevatorSubsystem.State.BOTTOM)
      .andThen(m_intakeRollersSubsystem.getSetStateCommand(IntakeRollersSubsystem.State.IN))
      .andThen(Commands.waitSeconds(2))
      .andThen(
        m_elevatorSubsystem.setStateCommand(ElevatorSubsystem.State.TOP)
          .alongWith(m_intakeRollersSubsystem.getSetStateCommand(IntakeRollersSubsystem.State.OUT))
      );
  }
}