// 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 javax.smartcardio.CommandAPDU;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
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 deadzone(double low, double high, double val) {
        if(val > low && val < high) return 0.0;
        return val;
    }

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

        // debug controls
        new Trigger(() -> m_controller.getRawButton(1)).onTrue(Commands.runOnce(() -> {
            Pose2d pose = m_drive.pose2d();
            System.out.println("x: " + pose.getX() +" y: "+ pose.getY());
            System.out.println("yaw: " + Constants.gyro.toRotation2d().getDegrees());
        })); 
        new Trigger(() -> m_controller.getRawButton(2)).onTrue(Commands.runOnce(() -> {
            System.out.println("Reset");
            m_drive.reset();
        })); 


        new Trigger(() -> m_controller.getRawButton(3) && m_controller.getRawButton(8))
            .onTrue(Commands.sequence(Commands.runOnce(() -> 
                    System.out.println("Quasistatic Forward")),
                m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
        new Trigger(() -> m_controller.getRawButton(3) && m_controller.getRawButton(7))
            .onTrue(Commands.sequence(Commands.runOnce(() -> 
                    System.out.println("Quasistatic Reverse")),
                m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
        new Trigger(() -> m_controller.getRawButton(4) && m_controller.getRawButton(8))
            .onTrue(Commands.sequence(Commands.runOnce(() -> 
                    System.out.println("Dynamic Forward")),
                m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
        new Trigger(() -> m_controller.getRawButton(4) && m_controller.getRawButton(7))
            .onTrue(Commands.sequence(Commands.runOnce(() -> 
                    System.out.println("Dynamic Reverse")),
                m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
                
        // new Trigger(() -> m_controller.getRawButton(3)).onTrue(Commands.runOnce(() -> {
        //     System.out.println("Running Quasistatis routine");
        //     if(m_controller.getRawButton(8)) {
        //         System.out.println("Reverse");
        //         Commands.sequence(m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse));
        //     } else {
        //         System.out.println("Forward");
        //         Commands.sequence(m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward));
        //     }
        // })); 
        // new Trigger(() -> m_controller.getRawButton(4) && m_controller()).onTrue(() -> {
        //     System.out.println("Running Dynamic routine");
        //     if(m_controller.getRawButton(8)) {
        //         System.out.println("Reverse");
        //         return m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse);
        //     } else {
        //         System.out.println("Forward");
        //         return m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward);
        //     }
        // }()); 
    }

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