// 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 java.util.Map;

import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.constants.ArmConstants;
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();
    configureShuffleboard();
  }

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

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

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

    operator.a().onTrue(elevator.goToTop(() -> .5, 3));
    operator.b().onTrue(elevator.goToBottom(() -> .5, 3));
    operator.x().onTrue(arm.goToAngle(() -> .5, 45, 3));
    operator.y().onTrue(arm.goToAngle(() -> .5, 135, 3));
  }

  private void configureShuffleboard() {
    ShuffleboardTab robotIndicatorTab = Shuffleboard.getTab(OIConstants.kRobotIndicatorsTabName);

    robotIndicatorTab.addBoolean("Elevator Top Beam Break", elevator::getTopBeamBreak)
      .withPosition(0, 0)
      .withSize(2, 1)
      .withWidget(BuiltInWidgets.kBooleanBox);

    robotIndicatorTab.addBoolean("Elevator Bottom Beam Break", elevator::getBottomBeamBreak)
      .withPosition(0, 1)
      .withSize(2, 1)
      .withWidget(BuiltInWidgets.kBooleanBox);

    robotIndicatorTab.addDouble("Arm Angle", arm::getArmAngle)
      .withPosition(2, 0)
      .withSize(2, 2)
      .withWidget(BuiltInWidgets.kDial)
      .withProperties(Map.of(
        "Max", ArmConstants.kPotentiometerMaxAngle
      ));
  }

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