// 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 edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;

// Manettes
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;

// Commands
import frc.robot.command.AllumeLED;
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;
import frc.robot.command.Limelight_tracker;
import frc.robot.command.PistonFerme;
import frc.robot.command.Pistongrimpeur;

// 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.Limelight;


public class RobotContainer {
  ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
  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();
  Limelight limelight = new Limelight();
  LED LED = new LED();
   CommandJoystick joystick = new CommandJoystick(0);
   CommandXboxController manette = new CommandXboxController(1);

   //command
   PistonFerme pistonFerme = new PistonFerme(grimpeur);
   Balayer balayer = new Balayer(balayeuse, accumulateur);
  GuiderHaut guiderHaut = new GuiderHaut(guideur);
  Lancer lancer = new Lancer(lanceur,limelight);
  LancerNote lancernote = new LancerNote(lanceur, accumulateur);
  Lancerampli lancerampli = new Lancerampli(lanceur,limelight);
   GrimpeurDroit grimpeurDroit = new GrimpeurDroit(grimpeur, manette::getLeftX);
  GrimpeurGauche grimpeurGauche = new GrimpeurGauche(grimpeur, manette::getRightX);
  AllumeLED allumeLED = new AllumeLED(LED, accumulateur); 
  Pistongrimpeur pistongrimpeur = new Pistongrimpeur(grimpeur, LED);
  public RobotContainer() {
    dashboard.addCamera("limelight", "limelight","limelight.local:5800");
    NamedCommands.registerCommand("balayer",new Balayer(balayeuse, accumulateur));
    NamedCommands.registerCommand("lancer", new LancerNote(lanceur, accumulateur));
    NamedCommands.registerCommand("piston", new PistonFerme(grimpeur));
    autoChooser = AutoBuilder.buildAutoChooser();

    CameraServer.startAutomaticCapture();
    manette.a().whileTrue(new GuiderBas(guideur));
    manette.b().whileTrue(new GuiderHaut(guideur));
    joystick.button(3).whileTrue(balayer);
    joystick.button(1).whileTrue(new LancerNote(lanceur, accumulateur));
    joystick.button(4).whileTrue(new ParallelCommandGroup(new Lancer(lanceur,limelight),new Limelight_tracker(limelight,drive)));
    joystick.button(2).whileTrue(new ParallelCommandGroup(new Lancerampli(lanceur,limelight),new Limelight_tracker(limelight,drive),new GuiderHaut(guideur)));
    
    // deplacement
    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 manuel
    grimpeur.setDefaultCommand(new RunCommand(()->{
      grimpeur.droit(manette.getLeftX());
    grimpeur.gauche(manette.getRightX());}
      ,grimpeur));

      LED.setDefaultCommand(allumeLED);
      dashboard.add("autochooser",autoChooser)
      .withSize(2,1)
      .withPosition(1,1);
  }

  private void configureBindings() {}

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