// 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 com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.constants.ArmConstants;
import frc.robot.constants.IOConstants;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.DriveTrain;
import frc.robot.subsystems.Grabber;

public class RobotContainer {
    CommandPS5Controller driver;

    Arm arm;

    DriveTrain driveTrain;
    Grabber grabber;

    public RobotContainer() {
        arm = new Arm();
        driveTrain = new DriveTrain();
        grabber = new Grabber(); 
        driver = new CommandPS5Controller(IOConstants.kDriverID0);
        configureBindings();
    }

    private void configureBindings() {
        driveTrain.setDefaultCommand(
            driveTrain.driveArcade(
              driver::getLeftY, 
              driver::getLeftX 
            )
        );  
        grabber.setDefaultCommand(grabber.stop());
        driver.R1().whileTrue(grabber.runGrabberMotor(1));
        driver.L1().whileTrue(grabber.runGrabberMotor(-1));
        arm.setDefaultCommand(arm.stop());
        driver.povUp().whileTrue(arm.runArmMotor(1));
        driver.povDown().whileTrue(arm.runArmMotor(-1));
    }

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