// 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.cameraserver.CameraServer;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.RunCommand;

// Manette
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
<<<<<<< HEAD
import frc.robot.command.Balayer;
=======
import frc.robot.command.GuiderBas;
import frc.robot.command.GuiderHaut;
>>>>>>> 33990971eb46b4232501c29b34451800e658504b
// Subsystem
import frc.robot.subsystem.Accumulateur;
import frc.robot.subsystem.Balayeuse;
import frc.robot.subsystem.Drive;
import frc.robot.subsystem.Grimpeur;
import frc.robot.subsystem.Guideur;
import frc.robot.subsystem.Lanceur;


public class RobotContainer {
  
  Drive drive = new Drive();
  Accumulateur accumulateur = new Accumulateur();
  Balayeuse balayeuse = new Balayeuse();
  Grimpeur grimpeur = new Grimpeur();
  Guideur guideur = new Guideur();
  Lanceur lanceur = new Lanceur();
  Balayer balayer = new Balayer(balayeuse, accumulateur);

  GuiderHaut guiderHaut = new GuiderHaut(guideur);
  GuiderBas guiderBas = new GuiderBas(guideur);
   CommandJoystick joystick = new CommandJoystick(0);
   CommandXboxController manette = new CommandXboxController(1);
  
  public RobotContainer() {

    CameraServer.startAutomaticCapture();
    manette.a().onTrue(guiderBas);
    manette.b().onTrue(guiderHaut);
    configureBindings();
     drive.setDefaultCommand(new RunCommand(()->{
     drive.drive(-MathUtil.applyDeadband(joystick.getY(),0.2), MathUtil.applyDeadband(-joystick.getX(),0.2), MathUtil.applyDeadband(-joystick.getZ(), 0.2));
    },drive));
  }

  private void configureBindings() {

   joystick.button(3).toggleOnTrue(balayer);

  }

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