// 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.button.CommandXboxController;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Elevator;

public class RobotContainer {
  private Drivetrain drivetrain;
  private Elevator elevator;
  private Arm arm;

  private CommandXboxController driver;
  private CommandXboxController operator;

  public RobotContainer() {
    drivetrain = new Drivetrain();
    elevator = new Elevator();
    arm = new Arm();

    driver = new CommandXboxController(OIConstants.kDriverUSB);
    operator = new CommandXboxController(OIConstants.kOperatorUSB);

    configureBindings();
  }

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

    elevator.setDefaultCommand(
      elevator.setSpeed(operator::getLeftY)
    );

    arm.setDefaultCommand(
      arm.setSpeed(operator::getRightY)
    );
  }

  public Command getAutonomousCommand() {
    return drivetrain.driveTank(() -> .4, () -> -.4);
  }
}
