// 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.NamedCommands;
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;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.commands.Algue1Test;
import frc.robot.commands.Algue2Test;
import frc.robot.commands.AlgueExpire;
import frc.robot.commands.CorailAspir;
import frc.robot.commands.CorailTest;
import frc.robot.commands.CoralAlgueInspire;
import frc.robot.commands.CoralExpire;
import frc.robot.commands.Depart;
import frc.robot.commands.DepartPince;
import frc.robot.commands.ElevateurManuel;
import frc.robot.commands.L2;
import frc.robot.commands.L3;
import frc.robot.commands.L4;
import frc.robot.commands.PinceManuel;
import frc.robot.commands.PinceManuel2;
import frc.robot.commands.StationPince;
import frc.robot.commands.reset;
import frc.robot.subsystems.Elevateur;
import frc.robot.subsystems.Pince;
import frc.robot.subsystems.Bougie;
public class RobotContainer {
  
  CommandXboxController manette1 = new CommandXboxController(0);
  CommandXboxController manette2 = new CommandXboxController(1);
  Bougie bougie = new Bougie();
  Pince pince = new Pince();
  Elevateur elevateur = new Elevateur();
  ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY);
  PinceManuel pinceManuel = new PinceManuel(pince);
  PinceManuel2 pinceManuel2 = new PinceManuel2(pince);
  public RobotContainer() {
    configureBindings();
    elevateur.setDefaultCommand(new RunCommand(()->{
      elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.2));
    }, elevateur));
    pince.setDefaultCommand(new RunCommand(()->{
      pince.pivote(MathUtil.applyDeadband(manette2.getRightY()/5, 0.2));
    }, pince));
     NamedCommands.registerCommand("Station",new StationPince(pince, elevateur));
    NamedCommands.registerCommand("L4", new L4(elevateur, pince));
    NamedCommands.registerCommand("L3", new L3(elevateur, pince));
    NamedCommands.registerCommand("CoralExpire",new CoralExpire(pince,bougie));
    NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie));

  }

  private void configureBindings() {
    // manette1
    manette1.a().whileTrue(new Depart(elevateur, pince));
    manette1.b().whileTrue(new L2(elevateur,pince));
    manette1.x().whileTrue(new L3(elevateur, pince));
    manette1.y().whileTrue(new L4(elevateur, pince));
    manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
    manette1.rightBumper().whileTrue(new StationPince(pince, elevateur));
    //manette2
    manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie));
    manette2.povUp().whileTrue(new PinceManuel(pince));
    manette2.povDown().whileTrue(new PinceManuel2(pince));
    manette2.a().whileTrue(new CorailAspir(pince));
    manette2.b().whileTrue(new CoralExpire(pince, bougie));
    manette2.povLeft().whileTrue(new Algue1Test(pince));
    manette2.povRight().whileTrue(new Algue2Test(pince));
    manette2.x().whileTrue(new CorailTest(pince));
    manette2.start().whileTrue(new reset(elevateur));
  }
  
  public Command getAutonomousCommand() {
    return Commands.print("No autonomous command configured");
  }
}