// 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.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.constants.OIConstants;
import frc.robot.constants.ShooterConstants;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Shooter;

public class RobotContainer {
  private Drivetrain drivetrain;
  private Shooter shooter;

  private CommandXboxController driver;

  public RobotContainer() {
    drivetrain = new Drivetrain();
    shooter = new Shooter();

    driver = new CommandXboxController(OIConstants.kDriverUSB);

    configureBindings();
  }

  private void configureBindings() {
    drivetrain.setDefaultCommand(
      drivetrain.driveArcade(
        driver::getLeftY, 
        driver::getLeftX
      )
    );

    driver.povCenter().negate().onTrue(
      drivetrain.goToAngle(driver.getHID().getPOV(), 3)
    );

    shooter.setDefaultCommand(
      shooter.setSpeed(
        driver::getRightTriggerAxis
      )
    );

    driver.a().whileTrue(
      shooter.runAutomaticSpeedControl(ShooterConstants.kShootSpeed)
    );
  }

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