// 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 org.wpilib.examples.hatchbottraditional;

import static org.wpilib.driverstation.Gamepad.Button;

import org.wpilib.command2.Command;
import org.wpilib.command2.button.GamepadButton;
import org.wpilib.driverstation.Gamepad;
import org.wpilib.examples.hatchbottraditional.Constants.AutoConstants;
import org.wpilib.examples.hatchbottraditional.Constants.OIConstants;
import org.wpilib.examples.hatchbottraditional.commands.ComplexAuto;
import org.wpilib.examples.hatchbottraditional.commands.DefaultDrive;
import org.wpilib.examples.hatchbottraditional.commands.DriveDistance;
import org.wpilib.examples.hatchbottraditional.commands.GrabHatch;
import org.wpilib.examples.hatchbottraditional.commands.HalveDriveVelocity;
import org.wpilib.examples.hatchbottraditional.commands.ReleaseHatch;
import org.wpilib.examples.hatchbottraditional.subsystems.DriveSubsystem;
import org.wpilib.examples.hatchbottraditional.subsystems.HatchSubsystem;
import org.wpilib.smartdashboard.SendableChooser;
import org.wpilib.smartdashboard.SmartDashboard;

/**
 * This class is where the bulk of the robot should be declared. Since Command-based is a
 * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
 * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
 * subsystems, commands, and button mappings) should be declared here.
 */
public class RobotContainer {
  // The robot's subsystems
  private final DriveSubsystem robotDrive = new DriveSubsystem();
  private final HatchSubsystem hatchSubsystem = new HatchSubsystem();

  // The autonomous routines

  // A simple auto routine that drives forward a specified distance, and then stops.
  private final Command simpleAuto =
      new DriveDistance(
          AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveVelocity, robotDrive);

  // A complex auto routine that drives forward, drops a hatch, and then drives backward.
  private final Command complexAuto = new ComplexAuto(robotDrive, hatchSubsystem);

  // A chooser for autonomous commands
  SendableChooser<Command> chooser = new SendableChooser<>();

  // The driver's controller
  Gamepad driverController = new Gamepad(OIConstants.kDriverControllerPort);

  /** The container for the robot. Contains subsystems, OI devices, and commands. */
  public RobotContainer() {
    // Configure the button bindings
    configureButtonBindings();

    // Configure default commands
    // Set the default drive command to split-stick arcade drive
    robotDrive.setDefaultCommand(
        // A split-stick arcade command, with forward/backward controlled by the left
        // hand, and turning controlled by the right.
        new DefaultDrive(
            robotDrive, () -> -driverController.getLeftY(), () -> -driverController.getRightX()));

    // Add commands to the autonomous command chooser
    chooser.setDefaultOption("Simple Auto", simpleAuto);
    chooser.addOption("Complex Auto", complexAuto);

    // Put the chooser on the dashboard
    SmartDashboard.putData("Autonomous", chooser);
    // Put subsystems to dashboard.
    SmartDashboard.putData("Drivetrain", robotDrive);
    SmartDashboard.putData("HatchSubsystem", hatchSubsystem);
  }

  /**
   * Use this method to define your button->command mappings. Buttons can be created by
   * instantiating a {@link org.wpilib.driverstation.GenericHID} or one of its subclasses ({@link
   * org.wpilib.driverstation.Joystick} or {@link Gamepad}), and then passing it to a {@link
   * org.wpilib.command2.button.JoystickButton}.
   */
  private void configureButtonBindings() {
    // Grab the hatch when the 'Face Down' button is pressed.
    new GamepadButton(driverController, Button.FACE_DOWN).onTrue(new GrabHatch(hatchSubsystem));
    // Release the hatch when the 'Face Right' button is pressed.
    new GamepadButton(driverController, Button.FACE_RIGHT).onTrue(new ReleaseHatch(hatchSubsystem));
    // While holding the bumper button, drive at half velocity
    new GamepadButton(driverController, Button.RIGHT_BUMPER)
        .whileTrue(new HalveDriveVelocity(robotDrive));
  }

  /**
   * Use this to pass the autonomous command to the main {@link Robot} class.
   *
   * @return the command to run in autonomous
   */
  public Command getAutonomousCommand() {
    return chooser.getSelected();
  }
}
