// 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.math.MathUtil;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
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.Commands;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.commands.AccAuto;
import frc.robot.commands.Avancer;
import frc.robot.commands.Force1;
//import frc.robot.commands.Lancer;
import frc.robot.commands.Force2;
import frc.robot.commands.Force3;
import frc.robot.commands.Force4;
import frc.robot.commands.Force5;
import frc.robot.commands.Force6;
import frc.robot.commands.Force7;
import frc.robot.commands.LanceurAuto;
import frc.robot.commands.Force1;
import frc.robot.commands.AvancerGauche;
import frc.robot.commands.accumulateurtest;
import frc.robot.subsystems.Accumulateur;
import frc.robot.subsystems.Drive;
import frc.robot.subsystems.Lanceur;

public class RobotContainer {
  
String avancergaucheString = "avancergaucheString";
String avancerautoString = "avancerautoString";
  ShuffleboardLayout layoutauto = Shuffleboard.getTab("Dashboard").getLayout("auto", BuiltInLayouts.kList);
  SendableChooser<String> chooser = new SendableChooser<>();
  Lanceur lanceur = new Lanceur();
  Accumulateur accumulateur = new Accumulateur();
  Drive drive = new Drive();
  Avancer avancer = new Avancer(drive);
 AvancerGauche avancerGauche = new AvancerGauche(drive);

  ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard");
  ShuffleboardLayout forces = Shuffleboard.getTab("Dashboard")
  .getLayout("forces", BuiltInLayouts.kList)
  .withSize(1, 7)
  .withPosition(1, 0);
  
  GenericEntry force1 = forces.add("Force1", 2).getEntry();
  GenericEntry force2 = forces.add("Force2", 4).getEntry();
  GenericEntry force3 = forces.add("Force3", 6).getEntry();
  GenericEntry force4 = forces.add("Force4", 8).getEntry();
  GenericEntry force5 = forces.add("Force5", 10).getEntry();
  GenericEntry force6 = forces.add("Force6", 12).getEntry();
  GenericEntry force7 = forces.add("Force7", 14).getEntry();

  CommandXboxController manette = new CommandXboxController(0);
  CommandJoystick joystick1 = new CommandJoystick(0);
  public RobotContainer() {
    CameraServer.startAutomaticCapture();
    configureBindings();
    drive.setDefaultCommand(new RunCommand(()->{
     drive.drive(-joystick1.getY(), -joystick1.getX(), MathUtil.applyDeadband(-joystick1.getZ(), 0.2));
    },drive));
  }
  
  private void configureBindings() {
    chooser.addOption(avancerautoString, avancerautoString);
    chooser.addOption(avancergaucheString, avancergaucheString);
    layoutauto.add("choix hauteur", chooser);
  accumulateurtest accumulateurtest = new accumulateurtest(accumulateur);
  Force1 Force1 = new Force1(lanceur, force1);
  Force2 Force2 = new Force2(lanceur, force2);
  Force3 Force3 = new Force3(lanceur, force3);
  Force4 Force4 = new Force4(lanceur, force4);
  Force5 Force5 = new Force5(lanceur, force5);
  Force6 Force6 = new Force6(lanceur, force6);
  Force7 Force7 = new Force7(lanceur, force7);

  //touche

    joystick1.button(3).whileTrue(accumulateurtest);
    joystick1.button(1).whileTrue(Force1);
    joystick1.button(7).whileTrue(Force2);
    joystick1.button(8).whileTrue(Force3);
    joystick1.button(9).whileTrue(Force4);
    joystick1.button(10).whileTrue(Force5);
    joystick1.button(11).whileTrue(Force6);
    joystick1.button(12).whileTrue(Force7);
  }
  
 
  public Command getAutonomousCommand() {
    chooser.getSelected();
    return new SequentialCommandGroup(new ParallelCommandGroup(new LanceurAuto(lanceur).withTimeout(6)
    ,new AccAuto(accumulateur).withTimeout(6)), Commands.waitSeconds(6), 
    Commands.select(Map.ofEntries(
      Map.entry(avancerautoString,avancer),
    	Map.entry(avancergaucheString, avancerGauche)
    ), chooser::getSelected)
    );
  }
}