// 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.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;

import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
// Manette

// Manettes
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.command.AllumeLED;
// Commands
import frc.robot.command.Balayer;
import frc.robot.command.GrimpeurDroit;
import frc.robot.command.GrimpeurGauche;
import frc.robot.command.GuiderBas;
import frc.robot.command.GuiderHaut;
import frc.robot.command.Lancer;
import frc.robot.command.LancerNote;
import frc.robot.command.Lancerampli;
// Subsystems
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.LED;
import frc.robot.subsystem.Lanceur;
import frc.robot.subsystem.Pixy;


public class RobotContainer {
  private final SendableChooser<Command> autoChooser;
  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);
  Lancer lancer = new Lancer(lanceur);
  LancerNote lancernote = new LancerNote(lanceur, accumulateur);
  Lancerampli lancerampli = new Lancerampli(lanceur);
   CommandJoystick joystick = new CommandJoystick(0);
   CommandXboxController manette = new CommandXboxController(1);
   GrimpeurDroit grimpeurDroit = new GrimpeurDroit(grimpeur, manette::getLeftX);
  GrimpeurGauche grimpeurGauche = new GrimpeurGauche(grimpeur, manette::getLeftY);
  LED LED = new LED();
  AllumeLED allumeLED = new AllumeLED(LED);
  Pixy pixy = new Pixy();
  public RobotContainer() {
    NamedCommands.registerCommand("balayer",new Balayer(balayeuse, accumulateur));
    NamedCommands.registerCommand("lancer", new LancerNote(lanceur, accumulateur));
    autoChooser = AutoBuilder.buildAutoChooser();

    CameraServer.startAutomaticCapture();
    manette.a().whileTrue(guiderBas);
    manette.b().whileTrue(guiderHaut);
    joystick.button(3).toggleOnTrue(balayer);
    joystick.button(1).whileTrue(lancernote);
    
    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));
    grimpeur.setDefaultCommand(new RunCommand(()->{
      grimpeur.droit(manette.getLeftX());}
      ,grimpeur));
      LED.setDefaultCommand(allumeLED);
  }

  private void configureBindings() {
  
  }

  public Command getAutonomousCommand(){
    return autoChooser.getSelected();
  }
}