// 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.math.filter.SlewRateLimiter;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.subsystems.Drivetrain;

public class RobotContainer 
{
    private final Drivetrain m_drive = new Drivetrain();

    private final SlewRateLimiter m_speedlimiter = new SlewRateLimiter(3);
    private final SlewRateLimiter m_rotlimiter = new SlewRateLimiter(3);

    private final GenericHID m_controller = 
        new GenericHID(Constants.PortDriverController);

    public RobotContainer() 
    {
        configureBindings();
    }

    private void configureBindings() 
    {
        m_drive.setDefaultCommand(m_drive.run(() -> 
        m_drive.drive(
            m_speedlimiter.calculate(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed,
            m_rotlimiter.calculate(m_controller.getRawAxis(2)) * Constants.DriveMaxAngular
        ))); 
    }

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