// 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 java.util.Map;

import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;

//subsystems
import frc.robot.subsystems.BasePilotable;
import frc.robot.subsystems.Gratte;
import frc.robot.subsystems.bras.BrasTelescopique;
import frc.robot.subsystems.bras.Pince;
import frc.robot.subsystems.bras.Pivot;
import frc.robot.subsystems.Limelight;
import frc.robot.commands.Apriltag;
import frc.robot.commands.Avancer;
// command
import frc.robot.commands.BrakeFerme;
import frc.robot.commands.BrakeOuvre;
import frc.robot.commands.Cone;
import frc.robot.commands.Cube;
import frc.robot.commands.GratteBaisser;
import frc.robot.commands.GratteMonte;
import frc.robot.commands.Gyro;
import frc.robot.commands.Reculer;
import frc.robot.commands.Tape;
import frc.robot.commands.bras.BrasManuel;
import frc.robot.commands.bras.DescendrePivotBras;
import frc.robot.commands.bras.FermePince;
import frc.robot.commands.bras.MonterPivotBras;
import frc.robot.commands.bras.OuvrePince;
import frc.robot.commands.bras.PivotManuel;
import frc.robot.commands.ActiverLimeLight;
public class RobotContainer {

  CommandXboxController manette1 = new CommandXboxController(0);
  CommandXboxController manette2 = new CommandXboxController(1);

  // dashboard
  SendableChooser<String> chooser = new SendableChooser<>();
  String enhaut = "en haut";
  String aumilieux = "au milieux";
  String enbas = "en bas";
  String nulpart = "nul part";
  ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto", BuiltInLayouts.kList)
      .withSize(2, 2).withProperties(Map.of("Label position", "LEFT"));
  GenericEntry autobalance = layoutauto.add("choix balance", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
  GenericEntry autosortir = layoutauto.add("choix sortir", false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
  GenericEntry reculerdistances = layoutauto.addPersistent("reculerdistancesortir", -66).getEntry();
  GenericEntry reculerdistanceb = layoutauto.addPersistent("reculerdistancebalance", -31).getEntry();
  GenericEntry avancerdistance = layoutauto.addPersistent("avancer", 35).getEntry();
  
  // subsystems
  BasePilotable basePilotable = new BasePilotable();
  Gratte gratte = new Gratte();
  BrasTelescopique brasTelescopique = new BrasTelescopique();
  Pince pince = new Pince();
  Pivot pivot = new Pivot();
  Limelight limelight = new Limelight();

  // commands
  BrakeFerme brakeFerme = new BrakeFerme(basePilotable);
  BrakeOuvre brakeOuvre = new BrakeOuvre(basePilotable);
  GratteBaisser gratteBaisser = new GratteBaisser(gratte);
  GratteMonte gratteMonte = new GratteMonte(gratte);
  Gyro gyro = new Gyro(basePilotable);
  FermePince fermePince = new FermePince(pince);
  OuvrePince ouvrePince = new OuvrePince(pince);
  Cone cone = new Cone(limelight, basePilotable, () -> -manette1.getLeftY());
  Reculer reculers = new Reculer(basePilotable, () -> reculerdistances.getDouble(0));
  Reculer reculerb = new Reculer(basePilotable, () -> reculerdistanceb.getDouble(0));
  Avancer avancer = new Avancer(basePilotable, () -> avancerdistance.getDouble(0));
  Cube cube = new Cube(limelight, basePilotable, () -> manette1.getLeftY());
  Apriltag aprilTag = new Apriltag(limelight, basePilotable, () -> manette1.getLeftY());
  Tape tape = new Tape(limelight, basePilotable, () -> manette1.getLeftY());
  PivotManuel pivotManuel = new PivotManuel(pivot, manette1::getRightY);
  BrasManuel brasManuel = new BrasManuel(brasTelescopique, manette1::getRightX);
  ActiverLimeLight activerLimeLight = new ActiverLimeLight(limelight);

  public RobotContainer() {
    chooser.addOption(enhaut, enhaut);
    chooser.addOption(enbas, enbas);
    chooser.addOption(aumilieux, aumilieux);
    chooser.addOption(nulpart, nulpart);
    layoutauto.add("choix hauteur", chooser);
    configureBindings();
    CameraServer.startAutomaticCapture();
    basePilotable.setDefaultCommand(new RunCommand(() -> {
      basePilotable.drive(-manette1.getLeftY(), -manette1.getLeftX());
    }, basePilotable));
    brasTelescopique.setDefaultCommand(brasManuel);
    pivot.setDefaultCommand(pivotManuel);
  }

  private void configureBindings() {
    // manette 1
    manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince));
    manette2.a().onTrue(brakeOuvre);
    manette2.b().onTrue(brakeFerme);
    manette1.leftBumper().whileTrue(aprilTag);
    manette1.rightBumper().whileTrue(tape);
    manette1.povUp().whileTrue(creerCommandBras(51, -37));
    manette1.povDown().whileTrue(creerCommandBras(12, -9));
    manette1.povRight().whileTrue(creerCommandBras(45, -13));
    manette1.povLeft().whileTrue(creerCommandBras(0, 0));
    manette1.y().whileTrue(activerLimeLight);
    //manette 2
    manette2.povDown().whileTrue(creerCommandBras(5, -12));
    manette2.povUp().whileTrue(creerCommandBras(44, 0));
    manette2.rightBumper().whileTrue(cube);
    manette2.leftBumper().whileTrue(cone);
    manette2.y().whileTrue(gyro);
    manette2.start().onTrue(new InstantCommand(basePilotable::resetGyro));
    manette1.b().whileTrue(gratteMonte);
    manette1.x().whileTrue(gratteBaisser);
  }

  private Command creerCommandBras(double distancePivot, double distanceBras) {
    return Commands.either(
        new MonterPivotBras(brasTelescopique, pivot, distanceBras, distancePivot),
        new DescendrePivotBras(brasTelescopique, pivot, distanceBras, distancePivot),
        () -> pivot.distance() < distancePivot);
  }

  public Command getAutonomousCommand() {

    return Commands.deadline(
        Commands.waitSeconds(14),
        new SequentialCommandGroup(
          new BrakeFerme(basePilotable),
            Commands.select(Map.ofEntries(
                Map.entry(enhaut, creerCommandBras(51, -40)),
                Map.entry(aumilieux, creerCommandBras(45, -13)),
                Map.entry(enbas, creerCommandBras(12, -9)),
                Map.entry(nulpart, creerCommandBras(0, 0))), chooser::getSelected),
            new OuvrePince(pince).unless(() -> chooser.getSelected().equals(nulpart)),
            Commands.waitSeconds(1),
           new FermePince(pince).unless(() -> chooser.getSelected().equals(nulpart)),
            creerCommandBras(0, 0).unless(() -> chooser.getSelected().equals(nulpart)),
            Commands.waitSeconds(1),
            Commands.either(reculers, reculerb, () -> autosortir.getBoolean(true)),
            new Avancer(basePilotable, () -> avancerdistance.getDouble(0)).unless(() -> !autosortir.getBoolean(true) || !autobalance.getBoolean(false)),
            Commands.either(new Gyro(basePilotable), Commands.none(), () -> autobalance.getBoolean(true))))
        .andThen(new BrakeOuvre(basePilotable));

  }
}