// 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 static edu.wpi.first.units.Units.*;

import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
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.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.commands.Aspirer;
import frc.robot.commands.DescendreBalyeuse;
import frc.robot.commands.DescendreGrimpeur;
import frc.robot.commands.Inverser;
import frc.robot.commands.Lancer;
import frc.robot.commands.LancerAspirer;
import frc.robot.commands.LancerBaseVitesse;
import frc.robot.commands.Limelighter;
import frc.robot.commands.ModeOposer;
import frc.robot.commands.ModeOposerBalayeuse;
import frc.robot.commands.ModeOposerDemeleur;
import frc.robot.commands.MonterBalyeuse;
import frc.robot.commands.MonterGrimpeur;
import frc.robot.commands.ModeAuto.AspirerAuto;
import frc.robot.commands.ModeAuto.BougerDroiteAuto;
import frc.robot.commands.ModeAuto.BougerGaucheAuto;
import frc.robot.commands.ModeAuto.GrimperMur;
import frc.robot.commands.ModeAuto.GrimperReservoir;
import frc.robot.commands.ModeAuto.LancerAuto;
import frc.robot.commands.ModeAuto.LimelighterAuto;
import frc.robot.commands.ModeAuto.RetourMilieuDroite;
import frc.robot.commands.ModeAuto.RetourMilieuGauche;
import frc.robot.commands.ModeAuto.TournerVersMur;
import frc.robot.commands.ModeAuto.TournerVersReservoir;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.Balayeuse;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Grimpeur;
import frc.robot.subsystems.LEDSubsystem;
import frc.robot.subsystems.Lanceur;
import frc.robot.subsystems.LimeLight3;
import frc.robot.subsystems.Limelight3G;

public class RobotContainer {
  private final SendableChooser<Command> autoChooser;
  Balayeuse balayeuse = new Balayeuse();
  Grimpeur grimpeur = new Grimpeur();
  Lanceur lanceur = new Lanceur();
  LimeLight3 limeLight3 = new LimeLight3();
  Limelight3G limeLight3G = new Limelight3G();
  LEDSubsystem ledSubsystem = new LEDSubsystem();
  CommandXboxController manette = new CommandXboxController(0);
  CommandXboxController manette1 = new CommandXboxController(1);
  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
    private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity

    /* Setting up bindings for necessary control of the swerve drive platform */
    private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
            .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors

    public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();


  public RobotContainer() {
    NamedCommands.registerCommand("droite", new BougerDroiteAuto(drivetrain));
    NamedCommands.registerCommand("gauche", new BougerGaucheAuto(drivetrain));
    NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain));
    NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain));
    NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G));
    NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain,limeLight3G));
    NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche(drivetrain, limeLight3G));
    NamedCommands.registerCommand("Limelighter", new LimelighterAuto(limeLight3G,drivetrain));
    NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse));
    NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse));
    NamedCommands.registerCommand("TournerA180", new TournerVersReservoir(drivetrain));
    NamedCommands.registerCommand("TournerAZero", new TournerVersMur(drivetrain));
    NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur));
    NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur));
    autoChooser = AutoBuilder.buildAutoChooser();
    SmartDashboard.putData("Auto Chooser", autoChooser);
    
    CameraServer.startAutomaticCapture();
    
    configureBindings();
  }

  private void configureBindings() {
    ledSubsystem.setDefaultCommand(ledSubsystem.updateLEDs());
    drivetrain.setDefaultCommand(
          drivetrain.applyRequest(() ->
            drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed*0.7, 0.05)) 
              .withVelocityX(MathUtil.applyDeadband(-manette.getLeftY()*Math.abs(-manette.getLeftY())*MaxSpeed*0.7, 0.05)) 
             .withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05))
        )
      );
    //manette 1
    // manette1.povUp().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
    // manette1.povDown().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
    // manette1.povRight().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
    // manette1.povLeft().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
    
    manette.povUp().whileTrue(new MonterGrimpeur(grimpeur));
    manette.povDown().whileTrue(new DescendreGrimpeur(grimpeur));
    manette.rightTrigger().whileTrue(new Lancer(lanceur, limeLight3G));
    manette.leftTrigger().whileTrue(new Limelighter(limeLight3G, drivetrain));
    manette.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
    manette.rightBumper().whileTrue(new Aspirer(balayeuse));
    manette.b().whileTrue(new GrimperReservoir(limeLight3G, drivetrain));
    manette.x().whileTrue(new GrimperMur(limeLight3, drivetrain));


    //manette 2
    manette1.rightTrigger().whileTrue(new Aspirer(balayeuse));
    manette1.rightBumper().whileTrue(new MonterBalyeuse(balayeuse));
    manette1.leftTrigger().whileTrue(new LancerBaseVitesse(lanceur));
    manette1.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
    manette1.x().whileTrue(new LancerAspirer(lanceur, balayeuse, limeLight3G));
    manette1.b().whileTrue(new ModeOposer(lanceur));
    manette1.a().whileTrue(new ModeOposerDemeleur(lanceur));
    manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse));
    manette1.povRight().whileTrue(new BougerDroiteAuto(drivetrain));
    manette1.povLeft().whileTrue(new BougerGaucheAuto(drivetrain));
    manette1.start().whileTrue(new Inverser(drivetrain));
  }

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