// 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.Drivetrain;
import frc.robot.subsystems.TruckHorn;

public class RobotContainer {
  private Drivetrain drivetrain;
  private TruckHorn horn;

  private CommandXboxController driver;

  public RobotContainer() {
    drivetrain = new Drivetrain();
    horn = new TruckHorn();

    driver = new CommandXboxController(OIConstants.kDriverUSB);

    configureBindings();
  }

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

    horn.setDefaultCommand(horn.stop());
  }

  public Command getAutonomousCommand() {
    return drivetrain.driveTank(() -> .4, () -> .4)
      .alongWith(horn.blastHorn()).withTimeout(3)
      .andThen(
        drivetrain.driveTank(() -> .5, () -> -.5)
          .alongWith(horn.threeBlasts(2, 1))
      );
  }
}
