// 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 org.littletonrobotics.junction.Logger;

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 GenericHID m_controller = Constants.controller;

    public RobotContainer() 
    {
        configureBindings();
    }

    private double processInput(double val) 
    {  
        double input = deadzone(-0.06, 0.06, val);
        return input * input * input;
    }

    private double deadzone(double low, double high, double val)
    {
        if(val > low && val < high) return 0.0;
        return ((val > 0.0) ? val - high : val - low);
    }

    private void configureBindings() 
    {
         m_drive.setDefaultCommand(m_drive.run(() -> 
         m_drive.tank(
             processInput(m_controller.getRawAxis(2)) * Constants.DriveMaxSpeed,
             processInput(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed
         )));

        // m_drive.setDefaultCommand(m_drive.run(() -> {
        //     Logger.recordOutput("inputL", processInput(m_controller.getRawAxis(2)));
        //     Logger.recordOutput("inputR", processInput(m_controller.getRawAxis(1)));
        // }));
        // m_controller.b().onTrue(Commands.runOnce(() -> {
        //     m_drive.reset();
        // })); 

        // m_controller.x().and(m_controller.leftBumper())
        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
        //             System.out.println("Quasistatic Forward")),
        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
        // m_controller.x().and(m_controller.rightBumper())
        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
        //             System.out.println("Quasistatic Reverse")),
        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
        // m_controller.y().and(m_controller.leftBumper())
        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
        //             System.out.println("Dynamic Forward")),
        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
        // m_controller.y().and(m_controller.rightBumper())
        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
        //             System.out.println("Dynamic Reverse")),
        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
    }

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