// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.PneumaticsModuleType;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.constants.CompressorConstants;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.Climber;
import frc.robot.subsystems.Kicker;

public class RobotContainer {
  private Kicker kicker;
  private Climber climber;

  private Compressor compressor;

  private CommandXboxController primary;

  public RobotContainer() {
    kicker = new Kicker();
    climber = new Climber();

    compressor = new Compressor(CompressorConstants.kModuleType);

    if (CompressorConstants.kModuleType == PneumaticsModuleType.REVPH) {
      compressor.enableAnalog(CompressorConstants.kMinPSI, CompressorConstants.kMaxPSI);
    } else {
      compressor.enableDigital();
    }

    primary = new CommandXboxController(OIConstants.kDriverUSB);

    configureBindings();
  }

  private void configureBindings() {
    climber.setDefaultCommand(climber.climberDown());

    primary.a().onTrue(kicker.kick());

    primary.b().whileTrue(climber.climberUp());
  }

  public Command getAutonomousCommand() {
    return Commands.print("No autonomous command configured");
  }
}
